diff --git a/CMakeLists.txt b/CMakeLists.txt index 31ef0b667e..21b0ed67ed 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,6 +69,7 @@ add_subdirectory(doc/how_to_guides/parallel_planning) add_subdirectory(doc/how_to_guides/chomp_planner) add_subdirectory(doc/how_to_guides/persistent_scenes_and_states) add_subdirectory(doc/how_to_guides/pilz_industrial_motion_planner) +add_subdirectory(doc/how_to_guides/planner_cost_functions) add_subdirectory(doc/how_to_guides/using_ompl_constrained_planning) add_subdirectory(doc/tutorials/pick_and_place_with_moveit_task_constructor) add_subdirectory(doc/tutorials/quickstart_in_rviz) diff --git a/doc/examples/moveit_cpp/config/moveit_cpp.yaml b/doc/examples/moveit_cpp/config/moveit_cpp.yaml index ffa333f8a4..0dbd6f88ee 100644 --- a/doc/examples/moveit_cpp/config/moveit_cpp.yaml +++ b/doc/examples/moveit_cpp/config/moveit_cpp.yaml @@ -14,5 +14,6 @@ planning_pipelines: plan_request_params: planning_attempts: 1 planning_pipeline: ompl + planner_id: "RRTConnectkConfigDefault" max_velocity_scaling_factor: 1.0 max_acceleration_scaling_factor: 1.0 diff --git a/doc/examples/moveit_cpp/launch/moveit_cpp_tutorial.launch.py b/doc/examples/moveit_cpp/launch/moveit_cpp_tutorial.launch.py index b1ea4678f2..4e1a9143c2 100644 --- a/doc/examples/moveit_cpp/launch/moveit_cpp_tutorial.launch.py +++ b/doc/examples/moveit_cpp/launch/moveit_cpp_tutorial.launch.py @@ -11,6 +11,7 @@ def generate_launch_description(): MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .planning_pipelines("ompl", ["ompl"]) .moveit_cpp( file_path=get_package_share_directory("moveit2_tutorials") + "/config/moveit_cpp.yaml" diff --git a/doc/how_to_guides/planner_cost_functions/CMakeLists.txt b/doc/how_to_guides/planner_cost_functions/CMakeLists.txt new file mode 100644 index 0000000000..aa39114b5c --- /dev/null +++ b/doc/how_to_guides/planner_cost_functions/CMakeLists.txt @@ -0,0 +1,13 @@ +add_executable(planner_cost_functions_example src/planner_cost_functions_main.cpp) +target_include_directories(planner_cost_functions_example PRIVATE include) +ament_target_dependencies(planner_cost_functions_example ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) + +install(TARGETS planner_cost_functions_example + DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME} +) 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..d38f02ae42 --- /dev/null +++ b/doc/how_to_guides/planner_cost_functions/config/cost_fn_config.rviz @@ -0,0 +1,473 @@ +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 All + Reliability Policy: Reliable + Value: /stomp_visualization + 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 new file mode 100644 index 0000000000..2e4175d8c4 --- /dev/null +++ b/doc/how_to_guides/planner_cost_functions/config/planner_cost_moveit_cpp.yaml @@ -0,0 +1,21 @@ +# Default configurations. More information can be found in the moveit_cpp and planning scene monitor tutorials +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" + publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene" + monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 + +# Define which planning pipelines are loaded by moveit_cpp. These loaded pipelines will be available for simple and parallel planning +planning_pipelines: + pipeline_names: ["stomp", "ompl"] + +plan_request_params: + planning_attempts: 1 + planning_pipeline: stomp + planner_id: "stomp" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 4.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 new file mode 100644 index 0000000000..ff2f117ab0 --- /dev/null +++ b/doc/how_to_guides/planner_cost_functions/launch/planner_cost_functions.launch.py @@ -0,0 +1,195 @@ +import os +import yaml +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "rviz_config", + default_value="panda_moveit_config_demo.rviz", + description="RViz configuration file", + ) + ) + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) + + +def launch_setup(context, *args, **kwargs): + + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .planning_pipelines(pipelines=["ompl", "stomp"]) + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) + .trajectory_execution(file_path="config/moveit_controllers.yaml") + .moveit_cpp( + os.path.join( + get_package_share_directory("moveit2_tutorials"), + "config", + "planner_cost_moveit_cpp.yaml", + ) + ) + .to_moveit_configs() + ) + + # Warehouse config + sqlite_database = os.path.join( + get_package_share_directory("moveit_resources_benchmarking"), + "databases", + "panda_kitchen_test_db.sqlite", + ) + + warehouse_ros_config = { + "warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection", + "warehouse": { + "warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection", + "host": sqlite_database, + "port": 33828, + "scene_name": "kitchen_panda_scene_sensed1", + }, + } + + # Load additional OMPL pipeline + # ompl_stomp_planning_pipeline_config = { + # "ompl_stomp": { + # "planning_plugin": "ompl_interface/OMPLPlanner", + # "request_adapters": """\ + # stomp_moveit/StompSmoothingAdapter \ + # default_planner_request_adapters/AddTimeOptimalParameterization \ + # default_planner_request_adapters/FixWorkspaceBounds \ + # default_planner_request_adapters/FixStartStateBounds \ + # default_planner_request_adapters/FixStartStateCollision \ + # default_planner_request_adapters/FixStartStatePathConstraints \ + # """, + # "start_state_max_bounds_error": 0.1, + # "planner_configs": { + # "RRTConnectkConfigDefault": { + # "type": "geometric::RRTConnect", + # "range": 0.0, # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()} + # } + # }, + # } + # } + + # MoveItCpp demo executable + moveit_cpp_node = Node( + name="planner_cost_functions_example", + package="moveit2_tutorials", + executable="planner_cost_functions_example", + output="screen", + parameters=[ + moveit_config.to_dict(), + # ompl_stomp_planning_pipeline_config, + warehouse_ros_config, + ], + ) + + # RViz + rviz_config_file = os.path.join( + get_package_share_directory("moveit2_tutorials"), + "config", + "cost_fn_config.rviz", + ) + + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + moveit_config.joint_limits, + ], + ) + + # Static TF + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], + ) + + # Publish TF + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[moveit_config.robot_description], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[moveit_config.robot_description, ros2_controllers_path], + output="screen", + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], + ) + + arm_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + hand_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["panda_hand_controller", "-c", "/controller_manager"], + ) + + nodes_to_start = [ + static_tf, + robot_state_publisher, + rviz_node, + moveit_cpp_node, + ros2_control_node, + joint_state_broadcaster_spawner, + arm_controller_spawner, + hand_controller_spawner, + ] + + return nodes_to_start diff --git a/doc/how_to_guides/planner_cost_functions/planner_cost_functions_tutorial.rst b/doc/how_to_guides/planner_cost_functions/planner_cost_functions_tutorial.rst new file mode 100644 index 0000000000..9ea29a45c7 --- /dev/null +++ b/doc/how_to_guides/planner_cost_functions/planner_cost_functions_tutorial.rst @@ -0,0 +1,4 @@ +Using cost functions in the MoveIt Planning Pipeline +==================================================== + +TODO 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 new file mode 100644 index 0000000000..690f9d1472 --- /dev/null +++ b/doc/how_to_guides/planner_cost_functions/src/planner_cost_functions_main.cpp @@ -0,0 +1,355 @@ +#include +#include +// MoveitCpp +#include +#include + +#include + +#include + +// Cost functions +#include +// Warehouse +#include +#include +#include +#include +#include +#include + +namespace rvt = rviz_visual_tools; + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("plannner_cost_fn_example"); +const std::string PLANNING_GROUP = "panda_arm"; +static const std::vector CONTROLLERS(1, "panda_arm_controller"); +} // namespace +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 +{ +public: + Demo(rclcpp::Node::SharedPtr node) + : node_{ node } + , moveit_cpp_{ std::make_shared(node) } + , planning_component_{ std::make_shared(PLANNING_GROUP, moveit_cpp_) } + , visual_tools_(node, "panda_link0", "plannner_cost_fn_example", moveit_cpp_->getPlanningSceneMonitorNonConst()) + { + moveit_cpp_->getPlanningSceneMonitorNonConst()->providePlanningSceneService(); + + visual_tools_.deleteAllMarkers(); + visual_tools_.loadRemoteControl(); + + Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); + text_pose.translation().z() = 1.75; + visual_tools_.publishText(text_pose, "Cost Function Tutorial", rvt::WHITE, rvt::XLARGE); + visual_tools_.trigger(); + } + + bool loadPlanningSceneAndQuery() + { + std::string hostname = ""; + int port = 0.0; + std::string scene_name = ""; + + node_->get_parameter_or(std::string("warehouse.host"), hostname, std::string("127.0.0.1")); + node_->get_parameter_or(std::string("warehouse.port"), port, 33829); + + if (!node_->get_parameter("warehouse.scene_name", scene_name)) + { + RCLCPP_WARN(LOGGER, "Warehouse scene_name NOT specified"); + } + + moveit_warehouse::PlanningSceneStorage* planning_scene_storage = nullptr; + + // Initialize database connection + try + { + warehouse_ros::DatabaseLoader db_loader(node_); + warehouse_ros::DatabaseConnection::Ptr warehouse_connection = db_loader.loadDatabase(); + warehouse_connection->setParams(hostname, port, 20); + if (warehouse_connection->connect()) + { + planning_scene_storage = new moveit_warehouse::PlanningSceneStorage(warehouse_connection); + RCLCPP_INFO(LOGGER, "Connected to database: '%s'", hostname.c_str()); + } + else + { + RCLCPP_ERROR(LOGGER, "Failed to connect to database"); + return false; + } + } + catch (std::exception& e) + { + RCLCPP_ERROR(LOGGER, "Failed to initialize planning scene storage: '%s'", e.what()); + return false; + } + + // Load planning scene + moveit_msgs::msg::PlanningScene scene_msg; + try + { + if (!planning_scene_storage) + { + RCLCPP_ERROR(LOGGER, "No planning scene storage"); + return false; + } + + if (planning_scene_storage->hasPlanningScene(scene_name)) // Just the world (no robot) + { + moveit_msgs::msg::PlanningSceneWorld world_meta_data; + if (!planning_scene_storage->getPlanningSceneWorld(world_meta_data, scene_name)) + { + RCLCPP_ERROR(LOGGER, "Failed to load planning scene world '%s'", scene_name.c_str()); + return false; + } + scene_msg.world = world_meta_data; + scene_msg.robot_model_name = "No robot information. Using only world geometry."; + } + else + { + RCLCPP_ERROR(LOGGER, "Failed to find planning scene '%s'", scene_name.c_str()); + return false; + } + } + catch (std::exception& ex) + { + RCLCPP_ERROR(LOGGER, "Error loading planning scene: %s", ex.what()); + return false; + } + + // Add object to planning scene + { // Lock PlanningScene + planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_->getPlanningSceneMonitorNonConst()); + scene->processPlanningSceneWorldMsg(scene_msg.world); + } // Unlock PlanningScene + + RCLCPP_INFO(LOGGER, "Loaded planning scene successfully"); + + // Get planning scene query + for (int index = 0; index < 10; index++) + { + moveit_warehouse::MotionPlanRequestWithMetadata planning_query; + std::string query_name = "kitchen_panda_scene_sensed" + std::to_string(index) + "_query"; + try + { + planning_scene_storage->getPlanningQuery(planning_query, scene_name, query_name); + } + catch (std::exception& ex) + { + RCLCPP_ERROR(LOGGER, "Error loading motion planning query '%s': %s", query_name.c_str(), ex.what()); + } + + motion_plan_requests.push_back(static_cast(*planning_query)); + } + visual_tools_.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); + visual_tools_.trigger(); + return true; + } + + struct PipelineConfig + { + std::string planning_pipeline; + std::string planner_id; + bool use_cost_function; + }; + + /// \brief Request a motion plan based on the assumption that a goal is set and print debug information. + void planAndVisualize(std::vector pipeline_configs, + moveit_msgs::msg::MotionPlanRequest const& motion_plan_request) + { + visual_tools_.deleteAllMarkers(); + visual_tools_.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + + // Set goal state + planning_component_->setGoal(motion_plan_request.goal_constraints); + // Set start state as current state + planning_component_->setStartStateToCurrentState(); + + // Get start state + auto robot_start_state = planning_component_->getStartState(); + + // Get planning scene + auto planning_scene_monitor = moveit_cpp_->getPlanningSceneMonitorNonConst(); + planning_scene_monitor->updateFrameTransforms(); + auto planning_scene = [planning_scene_monitor] { + planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor); + return planning_scene::PlanningScene::clone(ls); + }(); + + auto group_name = motion_plan_request.group_name; + + moveit_cpp::PlanningComponent::PlanRequestParameters plan_request_parameters; + plan_request_parameters.load(node_); + RCLCPP_INFO_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_configs.size()); + for (auto const& pipeline_config : pipeline_configs) + { + plan_request_parameters.planning_pipeline = pipeline_config.planning_pipeline; + plan_request_parameters.planner_id = pipeline_config.planner_id; + + // Set cost function + if (pipeline_config.use_cost_function) + { + planning_component_->setStateCostFunction([robot_start_state, group_name, + planning_scene](const Eigen::VectorXd& state_vector) mutable { + auto clearance_cost_fn = + moveit::cost_functions::createMinJointDisplacementCostFn(*robot_start_state, group_name, planning_scene); + return clearance_cost_fn(state_vector); + }); + } + else + { + planning_component_->setStateCostFunction(nullptr); + } + auto solution = planning_component_->plan(plan_request_parameters, planning_scene); + if (pipeline_config.use_cost_function) + { + solution.planner_id += std::string("_with_cost_term"); + } + + solutions.push_back(solution); + } + + 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 + for (auto const& plan_solution : solutions) + { + if (plan_solution.trajectory) + { + RCLCPP_INFO_STREAM(LOGGER, plan_solution.planner_id.c_str() + << ": " << colorToString(rviz_visual_tools::Colors(color_index)) + << ", Path length: " << robot_trajectory::path_length(*plan_solution.trajectory)); + // Visualize the trajectory in Rviz + visual_tools_.publishTrajectoryLine(plan_solution.trajectory, joint_model_group_ptr, + rviz_visual_tools::Colors(color_index)); + color_index++; + } + } + visual_tools_.trigger(); + } + + moveit_visual_tools::MoveItVisualTools& getVisualTools() + { + return visual_tools_; + } + + std::vector getMotionPlanRequests() + { + return motion_plan_requests; + } + +private: + std::shared_ptr node_; + std::shared_ptr moveit_cpp_; + std::shared_ptr planning_component_; + moveit_visual_tools::MoveItVisualTools visual_tools_; + std::vector motion_plan_requests; +}; +} // namespace plannner_cost_fn_example + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + RCLCPP_INFO(LOGGER, "Initialize node"); + + node_options.automatically_declare_parameters_from_overrides(true); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("plannner_cost_fn_example", "", node_options); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + std::thread([&executor]() { executor.spin(); }).detach(); + + plannner_cost_fn_example::Demo demo(node); + + if (!demo.loadPlanningSceneAndQuery()) + { + rclcpp::shutdown(); + return 0; + } + + RCLCPP_INFO(LOGGER, "Starting Cost Function Tutorial..."); + for (auto const& motion_plan_req : demo.getMotionPlanRequests()) + { + demo.planAndVisualize( + { { "ompl", "RRTConnectkConfigDefault", false }, { "stomp", "stomp", false }, { "stomp", "stomp", true } }, + motion_plan_req); + } + + demo.getVisualTools().prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo"); + rclcpp::shutdown(); + return 0; +}