Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[Hybrid Planning] configurable planning scene topics #1052

Merged
merged 7 commits into from
Feb 10, 2022
Merged

[Hybrid Planning] configurable planning scene topics #1052

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
6161925
Make sure all the planning scene topics are configurable (for a dual-…
Jan 31, 2022
1948033
Set topic names to match what the demo previously had
Jan 31, 2022
32d2e8f
Merge branch 'main' into andyz/hybrid_ps_params
Feb 3, 2022
343c287
Merge branch 'main' into andyz/hybrid_ps_params
Feb 8, 2022
d2cf1b3
Move moveit_cpp params to a separate yaml file
Feb 8, 2022
7453188
Revert "Move moveit_cpp params to a separate yaml file"
Feb 9, 2022
bcc85ea
Merge branch 'main' into andyz/hybrid_ps_params
Feb 9, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 9 additions & 0 deletions ...os/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
AndyZe marked this conversation as resolved.
Show resolved Hide resolved
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);

Expand Down
7 changes: 7 additions & 0 deletions ...al_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Comment on lines +122 to +124
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Same question here about defaults. Should the defaults for all these be defined somewhere (maybe a header file with a struct that has default values).

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

MoveIt already does have that header file. I don't think we should use it, though -- I think we should enforce correct yaml.

(Using defaults leads to issues. See Henning's previous comment about using MoveItCppOptions and my response.)

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You don't have to provide a default when you call declare_parameter.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I tried something like this. It seems nice for several reasons but it crashes.

void DeclareParam(const std::string& param_name, const rclcpp::Node::SharedPtr& node)
{
  try
  {
    if (node->has_parameter(param_name))
    {
      rcl_interfaces::msg::ParameterDescriptor descriptor;
      descriptor.dynamic_typing = true;
      node->declare_parameter(param_name, rclcpp::ParameterValue{}, descriptor);
    }
    else
      RCLCPP_ERROR_STREAM(node->get_logger(),
                          "Error getting parameter '" << param_name << "', check parameter type in YAML file");
  }
  catch (const rclcpp::exceptions::InvalidParameterTypeException& e)
  {
    RCLCPP_ERROR_STREAM(node->get_logger(),
                        "Error getting parameter '" << param_name << "', check parameter type in YAML file");
    throw e;
  }
}

std::string param_name = PLANNING_PIPELINES_NS + "pipeline_names";
DeclareParam(param_name, node);  --> Crashes because no default value is provided, I suppose

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

has_parameter returns true if you have already declared the parameter, if you then call declare_parameter then it will throw an exception because it is already declared.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That's a good point, maybe it will work without that. I'll try again.

}

std::string group_name;
Expand All @@ -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 */
Expand Down
7 changes: 3 additions & 4 deletions ...ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
6 changes: 4 additions & 2 deletions moveit_ros/hybrid_planning/test/config/global_planner.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Comment on lines +9 to +12
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The naming of these parameters is atrocious from the perspective of the hybrid planner. I'm not sure what can be done about it since the parameters get passed to moveit_cpp.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think it would make sense to use the MoveItCpp options directly and load them with a moveit_cpp.yaml. But you are right, those parameters are confusing

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Moving the parameters to a separate moveit_cpp.yaml makes a lot of sense as well as reducing LOC. Done.

Yep, those parameters will still look confusing if somebody does ros2 param list.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Actually I'm not sure that will work. I have to test if the parameters are read properly or if it just uses defaults.

Copy link
Member Author

@AndyZe AndyZe Feb 9, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@henningkayser I tried this in commit d2cf1b3. You can quickly verify that it doesn't work:

  • Modify one of the topics in global_planner_moveit_cpp.yaml, for example, like this: monitored_planning_scene_topic: "test/global_planner/planning_scene"
  • Launch the hybrid planning demo: ros2 launch moveit_hybrid_planning hybrid_planning_demo.launch.py
  • List the subscriptions of the global planner: ros2 node info /global_planner_private_xxx

You will see that it does not subscribe to the right topic.

I'm going to revert that commit

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm going to try to write some small tests to try to understand how declaring and overriding parameters work. I think the ros2 interface is confusing and I don't know what the right answer is.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can we get this merged and make an issue in the meantime? It's better than the previous state because at least these parameters are being used.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That seems reasonable for now, I would still like to better understand how this works so we can use parameters in a way we understand better.

wait_for_initial_state_timeout: 10.0
planning_pipelines:
#namespace: "moveit_cpp" # optional, default is ~
Expand Down
4 changes: 4 additions & 0 deletions moveit_ros/hybrid_planning/test/config/local_planner.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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