Fix tesseract_monitoring package

tesseract/tesseract_monitoring/CMakeLists.txt

@@ -1,27 +1,29 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(tesseract_monitoring)
 
-# Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++11 -Wall -Wextra)
 
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   tesseract_ros
   tesseract_collision
   tesseract_msgs
+  dynamic_reconfigure
+)
+
+generate_dynamic_reconfigure_options(
+  cfg/EnvironmentMonitorDynamicReconfigure.cfg
 )
 
 catkin_package(
   INCLUDE_DIRS include
-#  LIBRARIES tesseract_ros_monitor
+  LIBRARIES ${PROJECT_NAME}_environment
   CATKIN_DEPENDS
     roscpp
     tesseract_ros
     tesseract_collision
     tesseract_msgs
+    dynamic_reconfigure
 #  DEPENDS system_lib
 )
 
@@ -41,6 +43,7 @@ add_library(${PROJECT_NAME}_environment
 target_link_libraries(${PROJECT_NAME}_environment
   ${catkin_LIBRARIES}
   ${Boost_LIBRARIES})
+add_dependencies(${PROJECT_NAME}_environment ${PROJECT_NAME}_gencfg)
 
 add_executable(demo_scene demos/demo_scene.cpp)
 target_link_libraries(demo_scene ${PROJECT_NAME}_environment ${catkin_LIBRARIES})

tesseract/tesseract_monitoring/cfg/EnvironmentMonitorDynamicReconfigure.cfg

@@ -1,12 +1,12 @@
 #!/usr/bin/env python
-PACKAGE = "moveit_ros_planning"
+PACKAGE = "tesseract_monitoring"
 from dynamic_reconfigure.parameter_generator_catkin import *
 gen = ParameterGenerator()
 
-gen.add("publish_planning_scene", bool_t, 1, "Set to True to publish Planning Scenes", False)
-gen.add("publish_planning_scene_hz", double_t, 2, "Set the maximum frequency at which planning scene updates are published", 4, 0.1, 100.0)
-gen.add("publish_geometry_updates", bool_t, 3, "Set to True to publish geometry updates of the planning scene", True)
-gen.add("publish_state_updates", bool_t, 4, "Set to True to publish geometry updates of the planning scene", False)
-gen.add("publish_transforms_updates", bool_t, 5, "Set to True to publish geometry updates of the planning scene", False)
+gen.add("publish_environment", bool_t, 1, "Set to True to publish environment", False)
+gen.add("publish_environment_hz", double_t, 2, "Set the maximum frequency at which environment updates are published", 4, 0.1, 100.0)
+gen.add("publish_geometry_updates", bool_t, 3, "Set to True to publish geometry updates of the environment", True)
+gen.add("publish_state_updates", bool_t, 4, "Set to True to publish geometry updates of the environment", False)
+gen.add("publish_transforms_updates", bool_t, 5, "Set to True to publish geometry updates of the environment", False)
 
-exit(gen.generate(PACKAGE, PACKAGE, "PlanningSceneMonitorDynamicReconfigure"))
+exit(gen.generate(PACKAGE, PACKAGE, "EnvironmentMonitorDynamicReconfigure"))

tesseract/tesseract_monitoring/demos/demo_scene.cpp

@@ -63,6 +63,7 @@ void sendSphere()
   obj.visual.shape_poses.push_back(sphere_pose);
   obj.collision.shapes.push_back(sphere);
   obj.collision.shape_poses.push_back(sphere_pose);
+  obj.collision.collision_object_types.push_back(tesseract::CollisionObjectTypes::UseShapeType);
 
   tesseract_msgs::AttachableObject ao_msg;
   tesseract_ros::attachableObjectToAttachableObjectMsg(ao_msg, obj);

tesseract/tesseract_monitoring/include/tesseract_ros_monitor/current_state_monitor.h → tesseract/tesseract_monitoring/include/tesseract_monitoring/current_state_monitor.h

@@ -34,8 +34,8 @@
 
 /* Author: Ioan Sucan */
 
-#ifndef TESSERACT_ROS_MONITOR_CURRENT_STATE_H
-#define TESSERACT_ROS_MONITOR_CURRENT_STATE_H
+#ifndef TESSERACT_MONITORING_CURRENT_STATE_H
+#define TESSERACT_MONITORING_CURRENT_STATE_H
 
 #include <ros/ros.h>
 #include <sensor_msgs/JointState.h>
@@ -49,13 +49,9 @@
 namespace tesseract
 {
 
-namespace tesseract_ros
+namespace tesseract_monitoring
 {
 
-namespace tesseract_ros_monitor
-{
-
-
 typedef std::function<void(const sensor_msgs::JointStateConstPtr& joint_state)> JointStateUpdateCallback;
 
 /** @class CurrentStateMonitor
@@ -75,7 +71,7 @@ class CurrentStateMonitor
    *  @param tf A pointer to the tf transformer to use
    *  @param nh A ros::NodeHandle to pass node specific options
    */
-  CurrentStateMonitor(const ROSBasicEnvConstPtr &env, ros::NodeHandle nh);
+  CurrentStateMonitor(const tesseract_ros::ROSBasicEnvConstPtr &env, ros::NodeHandle nh);
 
   ~CurrentStateMonitor();
 
@@ -213,7 +209,6 @@ class CurrentStateMonitor
 typedef std::shared_ptr<CurrentStateMonitor> CurrentStateMonitorPtr;
 typedef std::shared_ptr<const CurrentStateMonitor> CurrentStateMonitorConstPtr;
 
-}
 }
 }
 #endif

tesseract/tesseract_monitoring/include/tesseract_ros_monitor/environment_monitor.h → tesseract/tesseract_monitoring/include/tesseract_monitoring/environment_monitor.h

@@ -34,29 +34,27 @@
 
 /* Author: Ioan Sucan */
 
-#ifndef TESSERACT_ROS_MONITOR_ENVIRONMENT_H
-#define TESSERACT_ROS_MONITOR_ENVIRONMENT_H
+#ifndef TESSERACT_MONITORING_ENVIRONMENT_H
+#define TESSERACT_MONITORING_ENVIRONMENT_H
 
 #include <ros/ros.h>
 #include <tf/tf.h>
 #include <tf/message_filter.h>
-#include <pluginlib/class_loader.hpp>
 #include <message_filters/subscriber.h>
 #include <tesseract_ros/ros_basic_env.h>
-#include <tesseract_ros_monitor/current_state_monitor.h>
+#include <tesseract_monitoring/current_state_monitor.h>
 #include <tesseract_msgs/TesseractState.h>
 #include <boost/noncopyable.hpp>
 #include <boost/thread/shared_mutex.hpp>
 #include <boost/thread/recursive_mutex.hpp>
 #include <memory>
 
-namespace tesseract
-{
+class DynamicReconfigureImpl;
 
-namespace tesseract_ros
+namespace tesseract
 {
 
-namespace tesseract_ros_monitor
+namespace tesseract_monitoring
 {
 
 /**
@@ -95,7 +93,7 @@ class EnvironmentMonitor : private boost::noncopyable
 
   /// The name of the topic used by default for publishing the monitored tesseract environment (this is without "/" in the
   /// name, so the topic is prefixed by the node name)
-  static const std::string MONITORED_ENVIRONMENT_TOPIC;  // "monitored_tesseract"
+  static const std::string MONITORED_ENVIRONMENT_TOPIC;  // "/monitored_tesseract"
 
   /** @brief Constructor
    *  @param robot_description The name of the ROS parameter that contains the URDF (in string format)
@@ -364,32 +362,29 @@ class EnvironmentMonitor : private boost::noncopyable
   urdf::ModelInterfaceConstSharedPtr urdf_model_;  /**< URDF MODEL */
   srdf::ModelConstSharedPtr srdf_model_;           /**< SRDF MODEL */
 
-  std::shared_ptr<pluginlib::ClassLoader<tesseract_ros::ROSBasicEnv> > env_loader;
-
-//  class DynamicReconfigureImpl;
-//  DynamicReconfigureImpl* reconfigure_impl_;
+  DynamicReconfigureImpl* reconfigure_impl_;
 };
 typedef std::shared_ptr<EnvironmentMonitor> EnvironmentMonitorPtr;
 typedef std::shared_ptr<const EnvironmentMonitor> EnvironmentMonitorConstPtr;
 
 /** \brief This is a convenience class for obtaining access to an
- *         instance of a locked PlanningScene.
+ *         instance of a locked Environment.
  *
  * Instances of this class can be used almost exactly like instances
- * of a PlanningScenePtr because of the typecast operator and
+ * of a ROSBasicEnvPtr because of the typecast operator and
  * "operator->" functions.  Therefore you will often see code like this:
  * @code
- *   planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor);
- *   robot_model::RobotModelConstPtr model = ls->getRobotModel();
+ *   environment_monitor::LockedEnvironmentRO ls(environment_monitor);
+ *   const tesseract_ros::ROSBasicEnvPtr& env = ls->getEnvironment();
  * @endcode
 
- * The function "getRobotModel()" is a member of PlanningScene and not
+ * The function "getEnvironment()" is a member of EnvironmentMonitor and not
  * a member of this class.  However because of the "operator->" here
- * which returns a PlanningSceneConstPtr, this works.
+ * which returns a ROSBasicEnvConstPtr, this works.
  *
  * Any number of these "ReadOnly" locks can exist at a given time.
  * The intention is that users which only need to read from the
- * PlanningScene will use these and will thus not interfere with each
+ * ROSBasicEnvPtr will use these and will thus not interfere with each
  * other.
  *
  * @see LockedEnvironmentRW */
@@ -465,22 +460,22 @@ class LockedEnvironmentRO
 };
 
 /** \brief This is a convenience class for obtaining access to an
- *         instance of a locked PlanningScene.
+ *         instance of a locked Environment.
  *
  * Instances of this class can be used almost exactly like instances
- * of a PlanningScenePtr because of the typecast operator and
+ * of a EnvironmentMonitorPtr because of the typecast operator and
  * "operator->" functions.  Therefore you will often see code like this:
  * @code
- *   planning_scene_monitor::LockedPlanningSceneRW ls(planning_scene_monitor);
- *   robot_model::RobotModelConstPtr model = ls->getRobotModel();
+ *   environment_monitor::LockedEnvironmentRW ls(environment_monitor);
+ *   const tesseract_ros::ROSBasicEnvPtr& env = ls->getEnvironment();
  * @endcode
 
- * The function "getRobotModel()" is a member of PlanningScene and not
+ * The function "getEnvironment()" is a member of EnvironmentMonitor and not
  * a member of this class.  However because of the "operator->" here
- * which returns a PlanningScenePtr, this works.
+ * which returns a ROSBasicEnvPtr, this works.
  *
  * Only one of these "ReadWrite" locks can exist at a given time.  The
- * intention is that users which need to write to the PlanningScene
+ * intention is that users which need to write to the ROSBasicEnv
  * will use these, preventing other writers and readers from locking
  * the same PlanningScene at the same time.
  *
@@ -505,6 +500,5 @@ class LockedEnvironmentRW : public LockedEnvironmentRO
 };
 }
 }
-}
 
 #endif

tesseract/tesseract_monitoring/package.xml

@@ -14,6 +14,7 @@
   <depend>tesseract_ros</depend>
   <depend>tesseract_collision</depend>
   <depend>tesseract_msgs</depend>
+  <depend>dynamic_reconfigure</depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

tesseract/tesseract_monitoring/src/current_state_monitor.cpp

@@ -34,12 +34,14 @@
 
 /* Author: Ioan Sucan */
 
-#include <tesseract_ros_monitor/current_state_monitor.h>
+#include <tesseract_monitoring/current_state_monitor.h>
 #include <limits>
 
-using namespace tesseract;
-using namespace tesseract::tesseract_ros;
-using namespace tesseract::tesseract_ros::tesseract_ros_monitor;
+namespace tesseract
+{
+
+namespace tesseract_monitoring
+{
 
 CurrentStateMonitor::CurrentStateMonitor(const tesseract_ros::ROSBasicEnvConstPtr& env)
   : CurrentStateMonitor(env, ros::NodeHandle())
@@ -349,3 +351,5 @@ void CurrentStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstP
   // notify waitForCurrentState() *after* potential update callbacks
   state_update_condition_.notify_all();
 }
+}
+}