Skip to content

Commit

Permalink
Move moveit_cpp params to a separate yaml file
Browse files Browse the repository at this point in the history
  • Loading branch information
AndyZe committed Feb 8, 2022
1 parent 343c287 commit d2cf1b3
Show file tree
Hide file tree
Showing 4 changed files with 26 additions and 31 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,8 @@ const std::string UNDEFINED = "<undefined>";

bool MoveItPlanningPipeline::initialize(const rclcpp::Node::SharedPtr& node)
{
node_ptr_ = node;

// TODO(andyz): how to standardize this for planning pipelines other than ompl?
// Maybe use loadPlanningPipelines() from moveit_cpp.cpp

Expand All @@ -67,20 +69,9 @@ 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);

node_ptr_ = node;

// Initialize MoveItCpp API
moveit_cpp::MoveItCpp::Options moveit_cpp_options(node);
moveit_cpp_ = std::make_shared<moveit_cpp::MoveItCpp>(node, moveit_cpp_options);
Expand Down
20 changes: 0 additions & 20 deletions moveit_ros/hybrid_planning/test/config/global_planner.yaml
Original file line number Diff line number Diff line change
@@ -1,21 +1 @@
global_planner_name: "moveit_hybrid_planning/MoveItPlanningPipeline"

# The rest of these parameters are typical for moveit_cpp
planning_scene_monitor_options:
name: "planning_scene_monitor"
robot_description: "robot_description"
joint_state_topic: "/joint_states"
attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
# 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 ~
pipeline_names: ["ompl"]
plan_request_params:
planning_attempts: 1
planning_pipeline: ompl
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
# The global planner uses moveit_cpp
planning_scene_monitor_options:
name: "planning_scene_monitor"
robot_description: "robot_description"
joint_state_topic: "/joint_states"
attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
# 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 ~
pipeline_names: ["ompl"]
plan_request_params:
planning_attempts: 1
planning_pipeline: ompl
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,10 @@ def generate_common_hybrid_launch_description():
global_planner_param = load_yaml(
"moveit_hybrid_planning", "config/global_planner.yaml"
)
# The global planner uses moveit_cpp
global_planner_moveit_cpp_param = load_yaml(
"moveit_hybrid_planning", "config/global_planner_moveit_cpp.yaml"
)
local_planner_param = load_yaml(
"moveit_hybrid_planning", "config/local_planner.yaml"
)
Expand All @@ -113,6 +117,7 @@ def generate_common_hybrid_launch_description():
parameters=[
common_hybrid_planning_param,
global_planner_param,
global_planner_moveit_cpp_param,
robot_description,
robot_description_semantic,
kinematics_yaml,
Expand Down

0 comments on commit d2cf1b3

Please sign in to comment.