From adb5c89b7e16cdb724b39b3f111f4c1be17b58f6 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 24 May 2023 11:26:38 +0200 Subject: [PATCH] Add TrajectoryLineVisualization --- .../config/cost_fn_config.rviz | 459 ++++++++++++++++++ .../config/planner_cost_moveit_cpp.yaml | 2 +- .../launch/planner_cost_functions.launch.py | 2 +- .../src/planner_cost_functions_main.cpp | 129 ++++- 4 files changed, 573 insertions(+), 19 deletions(-) create mode 100644 doc/how_to_guides/planner_cost_functions/config/cost_fn_config.rviz diff --git a/doc/how_to_guides/planner_cost_functions/config/cost_fn_config.rviz b/doc/how_to_guides/planner_cost_functions/config/cost_fn_config.rviz new file mode 100644 index 0000000000..980b3ccdf7 --- /dev/null +++ b/doc/how_to_guides/planner_cost_functions/config/cost_fn_config.rviz @@ -0,0 +1,459 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PlanningScene1 + - /Trajectory1 + Splitter Ratio: 0.5 + Tree Height: 671 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_visual_tools/RvizVisualToolsGui + Name: RvizVisualToolsGui +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_rviz_plugin/PlanningScene + Enabled: false + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /moveit_cpp/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.20000000298023224 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: false + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + Path: true + Sphere: true + Text: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plannner_cost_fn_example + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 3x + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: hand + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: panda_link0 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.8018128871917725 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.4451259672641754 + Y: -0.10787936300039291 + Z: 0.5332368612289429 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.16039825975894928 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.0435791015625 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1058 + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + RvizVisualToolsGui: + collapsed: false + Selection: + collapsed: false + Tool Properties: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 1920 + Y: 22 diff --git a/doc/how_to_guides/planner_cost_functions/config/planner_cost_moveit_cpp.yaml b/doc/how_to_guides/planner_cost_functions/config/planner_cost_moveit_cpp.yaml index 87141bd7fb..508936676b 100644 --- a/doc/how_to_guides/planner_cost_functions/config/planner_cost_moveit_cpp.yaml +++ b/doc/how_to_guides/planner_cost_functions/config/planner_cost_moveit_cpp.yaml @@ -18,4 +18,4 @@ plan_request_params: planner_id: "stomp" max_velocity_scaling_factor: 1.0 max_acceleration_scaling_factor: 1.0 - planning_time: 10.0 + planning_time: 5.0 diff --git a/doc/how_to_guides/planner_cost_functions/launch/planner_cost_functions.launch.py b/doc/how_to_guides/planner_cost_functions/launch/planner_cost_functions.launch.py index 7862c03d0d..b2f1f8fc4d 100644 --- a/doc/how_to_guides/planner_cost_functions/launch/planner_cost_functions.launch.py +++ b/doc/how_to_guides/planner_cost_functions/launch/planner_cost_functions.launch.py @@ -84,7 +84,7 @@ def launch_setup(context, *args, **kwargs): rviz_config_file = os.path.join( get_package_share_directory("moveit2_tutorials"), "config", - "parallel_planning_config.rviz", + "cost_fn_config.rviz", ) rviz_node = Node( diff --git a/doc/how_to_guides/planner_cost_functions/src/planner_cost_functions_main.cpp b/doc/how_to_guides/planner_cost_functions/src/planner_cost_functions_main.cpp index b01443167d..62fc7f1d13 100644 --- a/doc/how_to_guides/planner_cost_functions/src/planner_cost_functions_main.cpp +++ b/doc/how_to_guides/planner_cost_functions/src/planner_cost_functions_main.cpp @@ -29,6 +29,63 @@ static const std::vector CONTROLLERS(1, "panda_arm_controller"); namespace plannner_cost_fn_example { +// Color to string +[[nodiscard]] std::string colorToString(rviz_visual_tools::Colors rviz_color) +{ + std::string color_str = "unknown"; + switch (rviz_color) + { + case rviz_visual_tools::Colors::BLACK: + color_str = "black"; + break; + case rviz_visual_tools::Colors::BROWN: + color_str = "brown"; + break; + case rviz_visual_tools::Colors::BLUE: + color_str = "blue"; + break; + case rviz_visual_tools::Colors::CYAN: + color_str = "cyan"; + break; + case rviz_visual_tools::Colors::GREY: + color_str = "grey"; + break; + case rviz_visual_tools::Colors::DARK_GREY: + color_str = "dark grey"; + break; + case rviz_visual_tools::Colors::GREEN: + color_str = "green"; + break; + case rviz_visual_tools::Colors::LIME_GREEN: + color_str = "lime green"; + break; + case rviz_visual_tools::Colors::MAGENTA: + color_str = "magenta"; + break; + case rviz_visual_tools::Colors::ORANGE: + color_str = "orange"; + break; + case rviz_visual_tools::Colors::PURPLE: + color_str = "purple"; + break; + case rviz_visual_tools::Colors::RED: + color_str = "red"; + break; + case rviz_visual_tools::Colors::PINK: + color_str = "pink"; + break; + case rviz_visual_tools::Colors::WHITE: + color_str = "white"; + break; + case rviz_visual_tools::Colors::YELLOW: + color_str = "yellow"; + break; + default: + break; + } + return color_str; +} + /// \brief Utility class to create and interact with the parallel planning demo class Demo { @@ -173,6 +230,21 @@ class Demo // Set goal state if (!planning_query_request_.goal_constraints.empty()) { + for (auto constraint : planning_query_request_.goal_constraints) + { + for (auto joint_constraint : constraint.joint_constraints) + { + RCLCPP_INFO_STREAM(LOGGER, "position: " << joint_constraint.position << "\n" + << "tolerance_above: " << joint_constraint.tolerance_above << "\n" + << "tolerance_below: " << joint_constraint.tolerance_below << "\n" + << "weight: " << joint_constraint.weight << "\n"); + } + for (auto position_constraint : constraint.position_constraints) + { + RCLCPP_WARN(LOGGER, "Goal constraints contain position constrains, please use joint constraints only in this " + "example."); + } + } planning_component_->setGoal(planning_query_request_.goal_constraints); } else @@ -182,8 +254,11 @@ class Demo } /// \brief Request a motion plan based on the assumption that a goal is set and print debug information. - void planAndPrint() + void planAndVisualize(std::vector> pipeline_planner_pairs) { + visual_tools_.deleteAllMarkers(); + visual_tools_.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + // Set start state as current state planning_component_->setStartStateToCurrentState(); @@ -211,25 +286,42 @@ class Demo return clearance_cost_fn(state_vector); }); - auto plan_solution = planning_component_->plan(); + moveit_cpp::PlanningComponent::PlanRequestParameters plan_request_parameters; + plan_request_parameters.load(node_); + RCLCPP_DEBUG_STREAM( + LOGGER, "Default plan request parameters loaded with --" + << " planning_pipeline: " << plan_request_parameters.planning_pipeline << ',' + << " planner_id: " << plan_request_parameters.planner_id << ',' + << " planning_time: " << plan_request_parameters.planning_time << ',' + << " planning_attempts: " << plan_request_parameters.planning_attempts << ',' + << " max_velocity_scaling_factor: " << plan_request_parameters.max_velocity_scaling_factor << ',' + << " max_acceleration_scaling_factor: " << plan_request_parameters.max_acceleration_scaling_factor); + + std::vector solutions; + solutions.reserve(pipeline_planner_pairs.size()); + for (auto const& pipeline_planner_pair : pipeline_planner_pairs) + { + plan_request_parameters.planning_pipeline = pipeline_planner_pair.first; + plan_request_parameters.planner_id = pipeline_planner_pair.second; + solutions.push_back(planning_component_->plan(plan_request_parameters, planning_scene)); + } + int color_index = 1; + auto robot_model_ptr = moveit_cpp_->getRobotModel(); + auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP); // Check if PlanningComponents succeeded in finding the plan - if (plan_solution) + for (auto const& plan_solution : solutions) { - // Visualize the trajectory in Rviz - auto robot_model_ptr = moveit_cpp_->getRobotModel(); - auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP); - - visual_tools_.publishTrajectoryLine(plan_solution.trajectory, joint_model_group_ptr); - visual_tools_.trigger(); + if (plan_solution.trajectory) + { + RCLCPP_INFO_STREAM(LOGGER, plan_solution.planner_id.c_str() + << ": " << colorToString(rviz_visual_tools::Colors(color_index))); + // Visualize the trajectory in Rviz + visual_tools_.publishTrajectoryLine(plan_solution.trajectory, joint_model_group_ptr, + rviz_visual_tools::Colors(color_index)); + color_index++; + } } - - // Execute the trajectory and block until it's finished - moveit_cpp_->execute(plan_solution.trajectory, true /* blocking*/, CONTROLLERS); - - // Start the next plan - visual_tools_.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - visual_tools_.deleteAllMarkers(); visual_tools_.trigger(); } @@ -273,8 +365,11 @@ int main(int argc, char** argv) // Experiment - Long motion with collisions, CHOMP and Pilz are likely to fail here due to the difficulty of the planning problem RCLCPP_INFO(LOGGER, "Experiment - Long motion with collisions"); demo.setQueryGoal(); - demo.planAndPrint(); + demo.planAndVisualize( + { { "ompl", "RRTConnectkConfigDefault" }, { "ompl", "RRTstarkConfigDefault" }, { "stomp", "stomp" } }); + + demo.getVisualTools().prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo"); rclcpp::shutdown(); return 0; }