[Hybrid Planning] configurable planning scene topics (#1052)

moveit · Feb 10, 2022 · dd240ef · dd240ef
1 parent 77e43f3
commit dd240ef
Show file tree

Hide file tree

Showing 5 changed files with 27 additions and 6 deletions.
diff --git a/...os/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp b/...os/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp
@@ -67,6 +67,15 @@ bool MoveItPlanningPipeline::initialize(const rclcpp::Node::SharedPtr& node)
   node->declare_parameter<double>(PLAN_REQUEST_PARAM_NS + "max_acceleration_scaling_factor", 1.0);
   node->declare_parameter<std::string>("ompl.planning_plugin", "ompl_interface/OMPLPlanner");
 
+  // Planning Scene options
+  node->declare_parameter<std::string>(PLANNING_SCENE_MONITOR_NS + "name", UNDEFINED);
+  node->declare_parameter<std::string>(PLANNING_SCENE_MONITOR_NS + "robot_description", UNDEFINED);
+  node->declare_parameter<std::string>(PLANNING_SCENE_MONITOR_NS + "joint_state_topic", UNDEFINED);
+  node->declare_parameter<std::string>(PLANNING_SCENE_MONITOR_NS + "attached_collision_object_topic", UNDEFINED);
+  node->declare_parameter<std::string>(PLANNING_SCENE_MONITOR_NS + "publish_planning_scene_topic", UNDEFINED);
+  node->declare_parameter<std::string>(PLANNING_SCENE_MONITOR_NS + "monitored_planning_scene_topic", UNDEFINED);
+  node->declare_parameter<double>(PLANNING_SCENE_MONITOR_NS + "wait_for_initial_state_timeout", 10.0);
+
   // Trajectory Execution Functionality (required by the MoveItPlanningPipeline but not used within hybrid planning)
   node->declare_parameter<std::string>("moveit_controller_manager", UNDEFINED);
 

diff --git a/...al_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h b/...al_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h
@@ -118,6 +118,10 @@ class LocalPlannerComponent
       declareOrGetParam<std::string>("local_solution_topic_type", local_solution_topic_type, undefined, node);
       declareOrGetParam<bool>("publish_joint_positions", publish_joint_positions, false, node);
       declareOrGetParam<bool>("publish_joint_velocities", publish_joint_velocities, false, node);
+      // Planning scene monitor options
+      declareOrGetParam<std::string>("monitored_planning_scene", monitored_planning_scene_topic, undefined, node);
+      declareOrGetParam<std::string>("collision_object_topic", collision_object_topic, undefined, node);
+      declareOrGetParam<std::string>("joint_states_topic", joint_states_topic, undefined, node);
     }
 
     std::string group_name;
@@ -133,6 +137,9 @@ class LocalPlannerComponent
     bool publish_joint_positions;
     bool publish_joint_velocities;
     double local_planning_frequency;
+    std::string monitored_planning_scene_topic;
+    std::string collision_object_topic;
+    std::string joint_states_topic;
   };
 
   /** \brief Constructor */

diff --git a/...ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp b/...ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp
@@ -92,10 +92,9 @@ bool LocalPlannerComponent::initialize()
   }
 
   // Start state and scene monitors
-  RCLCPP_INFO(LOGGER, "Starting planning scene monitors");
-  planning_scene_monitor_->startSceneMonitor();
-  planning_scene_monitor_->startWorldGeometryMonitor();
-  planning_scene_monitor_->startStateMonitor();
+  planning_scene_monitor_->startSceneMonitor(config_.monitored_planning_scene_topic);
+  planning_scene_monitor_->startWorldGeometryMonitor(config_.collision_object_topic);
+  planning_scene_monitor_->startStateMonitor(config_.joint_states_topic);
 
   // Load trajectory operator plugin
   try

diff --git a/moveit_ros/hybrid_planning/test/config/global_planner.yaml b/moveit_ros/hybrid_planning/test/config/global_planner.yaml
@@ -6,8 +6,10 @@ planning_scene_monitor_options:
   robot_description: "robot_description"
   joint_state_topic: "/joint_states"
   attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
-  publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene"
-  monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene"
+  # Subscribe to this topic (The name comes from the perspective of moveit_cpp)
+  publish_planning_scene_topic: "/planning_scene"
+  # Publish this topic, e.g. to visualize with RViz
+  monitored_planning_scene_topic: "/global_planner/planning_scene"
   wait_for_initial_state_timeout: 10.0
 planning_pipelines:
   #namespace: "moveit_cpp"  # optional, default is ~

diff --git a/moveit_ros/hybrid_planning/test/config/local_planner.yaml b/moveit_ros/hybrid_planning/test/config/local_planner.yaml
@@ -8,6 +8,10 @@ local_solution_topic_type: "std_msgs/Float64MultiArray" # or trajectory_msgs/Joi
 publish_joint_positions: true
 publish_joint_velocities: false
 group_name: "panda_arm"
+# Subscribe to this topic
+monitored_planning_scene: "/planning_scene"
+collision_object_topic: "/collision_object"
+joint_states_topic: "/joint_states"
 
 # ForwardTrajectory param
 stop_before_collision: true