diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 9a0e314a0f..c2e6a1a25e 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -1652,8 +1652,10 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs:: for (std::size_t i = 0; i < shape_vector.size(); ++i) { if (i >= shape_poses_vector.size()) + { append(shapes::constructShapeFromMsg(shape_vector[i]), geometry_msgs::msg::Pose()); // Empty shape pose => Identity + } else append(shapes::constructShapeFromMsg(shape_vector[i]), shape_poses_vector[i]); } diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index c9948d01be..4e44574381 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -473,7 +473,7 @@ void RobotState::setVariableEffort(const std::map& variable { markEffort(); for (const std::pair& it : variable_map) - acceleration_[robot_model_->getVariableIndex(it.first)] = it.second; + effort_[robot_model_->getVariableIndex(it.first)] = it.second; } void RobotState::setVariableEffort(const std::map& variable_map, diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index c9f3c34fc7..10a833d577 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -940,15 +940,16 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT max_velocity[j] = 1.0; if (bounds.velocity_bounded_) { - max_velocity[j] = std::min(fabs(bounds.max_velocity_), fabs(bounds.min_velocity_)) * velocity_scaling_factor; + max_velocity[j] = + std::min(std::fabs(bounds.max_velocity_), std::fabs(bounds.min_velocity_)) * velocity_scaling_factor; max_velocity[j] = std::max(0.01, max_velocity[j]); } max_acceleration[j] = 1.0; if (bounds.acceleration_bounded_) { - max_acceleration[j] = - std::min(fabs(bounds.max_acceleration_), fabs(bounds.min_acceleration_)) * acceleration_scaling_factor; + max_acceleration[j] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_)) * + acceleration_scaling_factor; max_acceleration[j] = std::max(0.01, max_acceleration[j]); } } @@ -960,13 +961,17 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT { moveit::core::RobotStatePtr waypoint = trajectory.getWayPointPtr(p); Eigen::VectorXd new_point(num_joints); + // The first point should always be kept bool diverse_point = (p == 0); for (size_t j = 0; j < num_joints; ++j) { new_point[j] = waypoint->getVariablePosition(idx[j]); - if (p > 0 && std::abs(new_point[j] - points.back()[j]) > min_angle_change_) + // If any joint angle is different, it's a unique waypoint + if (p > 0 && std::fabs(new_point[j] - points.back()[j]) > min_angle_change_) + { diverse_point = true; + } } if (diverse_point) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp index c009488317..6539f1d020 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp @@ -267,7 +267,7 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin } } -void TrajectoryGeneratorPTP::plan(const planning_scene::PlanningSceneConstPtr& scene, +void TrajectoryGeneratorPTP::plan(const planning_scene::PlanningSceneConstPtr& /*scene*/, const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) { diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/python_move_group_planning.test b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/python_move_group_planning.test new file mode 100644 index 0000000000..c6b286de4c --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/python_move_group_planning.test @@ -0,0 +1,9 @@ + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/python_move_group_planning.py b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/python_move_group_planning.py new file mode 100755 index 0000000000..93adb0d585 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/python_move_group_planning.py @@ -0,0 +1,118 @@ +#!/usr/bin/env python + +# Software License Agreement (BSD License) +# +# Copyright (c) 2021, Cristian C. Beltran-Hernandez +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Cristian C. Beltran-Hernandez nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Cristian C. Beltran-Hernandez +# +# This test is used to ensure planning with an attached object +# correctly validates collision states between the attached objects +# and the environment + +import unittest +import numpy as np +import rospy +import rostest +import os + +from moveit_msgs.msg import MoveItErrorCodes +import moveit_commander + +from geometry_msgs.msg import PoseStamped + + +class PythonMoveGroupPlanningTest(unittest.TestCase): + @classmethod + def setUpClass(self): + PLANNING_GROUP = "panda_arm" + self.group = moveit_commander.MoveGroupCommander(PLANNING_GROUP) + self.planning_scene_interface = moveit_commander.PlanningSceneInterface( + synchronous=True + ) + + @classmethod + def tearDown(self): + pass + + def test_planning_with_collision_objects(self): + # Add obstacle to the world + ps = PoseStamped() + ps.header.frame_id = "world" + ps.pose.position.x = 0.4 + ps.pose.position.y = 0.1 + ps.pose.position.z = 0.25 + self.planning_scene_interface.add_box( + name="box1", pose=ps, size=(0.1, 0.1, 0.5) + ) + + # Attach object to robot's TCP + ps2 = PoseStamped() + tcp_link = self.group.get_end_effector_link() + ps2.header.frame_id = tcp_link + ps2.pose.position.z = 0.15 + self.planning_scene_interface.attach_box( + link=tcp_link, + name="box2", + pose=ps2, + size=(0.1, 0.1, 0.1), + touch_links=["panda_rightfinger", "panda_leftfinger"], + ) + + # Plan a motion where the attached object 'box2' collides with the obstacle 'box1' + target_pose = self.group.get_current_pose(tcp_link) + target_pose.pose.position.y += 0.1 + + # # Set planner to be Pilz's Linear Planner + # self.group.set_planning_pipeline_id("pilz_industrial_motion_planner") + # self.group.set_planner_id("LIN") + # self.group.set_pose_target(target_pose) + # success, plan, time, error_code = self.group.plan() + + # # Planning should fail + # self.assertEqual(error_code.val, MoveItErrorCodes.INVALID_MOTION_PLAN) + + # Set planner to be Pilz's Point-To-Point Planner + self.group.set_planning_pipeline_id("pilz_industrial_motion_planner") + self.group.set_planner_id("PTP") + self.group.set_pose_target(target_pose) + success, plan, time, error_code = self.group.plan() + + # Planning should fail + self.assertFalse(success) + self.assertEqual(error_code.val, MoveItErrorCodes.INVALID_MOTION_PLAN) + + +if __name__ == "__main__": + PKGNAME = "moveit_ros_planning_interface" + NODENAME = "moveit_test_python_move_group" + rospy.init_node(NODENAME) + rostest.rosrun(PKGNAME, NODENAME, PythonMoveGroupPlanningTest) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp index 6117bb47fb..ba31c7627f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -231,7 +231,7 @@ bool testutils::checkCartesianLinearity(const moveit::core::RobotModelConstPtr& const trajectory_msgs::msg::JointTrajectory& trajectory, const planning_interface::MotionPlanRequest& req, const double translation_norm_tolerance, const double rot_axis_norm_tolerance, - const double rot_angle_tolerance) + const double /*rot_angle_tolerance*/) { std::string link_name; Eigen::Isometry3d goal_pose_expect; @@ -1126,7 +1126,8 @@ bool testutils::checkBlendResult(const pilz_industrial_motion_planner::Trajector const pilz_industrial_motion_planner::TrajectoryBlendResponse& blend_res, const pilz_industrial_motion_planner::LimitsContainer& limits, double joint_velocity_tolerance, double joint_acceleration_tolerance, - double cartesian_velocity_tolerance, double cartesian_angular_velocity_tolerance) + double /*cartesian_velocity_tolerance*/, + double /*cartesian_angular_velocity_tolerance*/) { // ++++++++++++++++++++++ // + Check trajectories + diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index b40d9bed84..da84587ae8 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -790,13 +790,22 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface /** \brief Stop any trajectory execution, if one is active */ void stop(); - /** \brief Specify whether the robot is allowed to look around before moving if it determines it should (default is - * true) */ - void allowLooking(bool flag); - /** \brief Specify whether the robot is allowed to replan if it detects changes in the environment */ void allowReplanning(bool flag); + /** \brief Maximum number of replanning attempts */ + void setReplanAttempts(int32_t attempts); + + /** \brief Sleep this duration between replanning attempts (in walltime seconds) */ + void setReplanDelay(double delay); + + /** \brief Specify whether the robot is allowed to look around + before moving if it determines it should (default is false) */ + void allowLooking(bool flag); + + /** \brief How often is the system allowed to move the camera to update environment model when looking */ + void setLookAroundAttempts(int32_t attempts); + /** \brief Build the MotionPlanRequest that would be sent to the move_group action with plan() or move() and store it in \e request */ void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request); diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index ed5d539270..67b696d01f 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -100,6 +100,8 @@ enum ActiveTargetType class MoveGroupInterface::MoveGroupInterfaceImpl { + friend MoveGroupInterface; + public: MoveGroupInterfaceImpl(const rclcpp::Node::SharedPtr& node, const Options& opt, const std::shared_ptr& tf_buffer, const rclcpp::Duration& wait_for_servers) @@ -128,8 +130,10 @@ class MoveGroupInterface::MoveGroupInterfaceImpl joint_state_target_->setToDefaultValues(); active_target_ = JOINT; can_look_ = false; + look_around_attempts_ = 0; can_replan_ = false; replan_delay_ = 2.0; + replan_attempts_ = 1; goal_joint_tolerance_ = 1e-4; goal_position_tolerance_ = 1e-4; // 0.1 mm goal_orientation_tolerance_ = 1e-3; // ~0.1 deg @@ -1108,29 +1112,6 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return allowed_planning_time_; } - void allowLooking(bool flag) - { - can_look_ = flag; - RCLCPP_INFO(LOGGER, "Looking around: %s", can_look_ ? "yes" : "no"); - } - - void allowReplanning(bool flag) - { - can_replan_ = flag; - RCLCPP_INFO(LOGGER, "Replanning: %s", can_replan_ ? "yes" : "no"); - } - - void setReplanningDelay(double delay) - { - if (delay >= 0.0) - replan_delay_ = delay; - } - - double getReplanningDelay() const - { - return replan_delay_; - } - void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request) const { request.group_name = opt_.group_name_; @@ -1389,7 +1370,9 @@ class MoveGroupInterface::MoveGroupInterfaceImpl double goal_position_tolerance_; double goal_orientation_tolerance_; bool can_look_; + int32_t look_around_attempts_; bool can_replan_; + int32_t replan_attempts_; double replan_delay_; // joint state goal @@ -2260,12 +2243,53 @@ void MoveGroupInterface::forgetJointValues(const std::string& name) void MoveGroupInterface::allowLooking(bool flag) { - impl_->allowLooking(flag); + impl_->can_look_ = flag; + RCLCPP_DEBUG(LOGGER, "Looking around: %s", flag ? "yes" : "no"); +} + +void MoveGroupInterface::setLookAroundAttempts(int32_t attempts) +{ + if (attempts < 0) + { + RCLCPP_ERROR(LOGGER, "Tried to set negative number of look-around attempts"); + } + else + { + RCLCPP_DEBUG_STREAM(LOGGER, "Look around attempts: " << attempts); + impl_->look_around_attempts_ = attempts; + } } void MoveGroupInterface::allowReplanning(bool flag) { - impl_->allowReplanning(flag); + impl_->can_replan_ = flag; + RCLCPP_DEBUG(LOGGER, "Replanning: %s", flag ? "yes" : "no"); +} + +void MoveGroupInterface::setReplanAttempts(int32_t attempts) +{ + if (attempts < 0) + { + RCLCPP_ERROR(LOGGER, "Tried to set negative number of replan attempts"); + } + else + { + RCLCPP_DEBUG_STREAM(LOGGER, "Replan Attempts: " << attempts); + impl_->replan_attempts_ = attempts; + } +} + +void MoveGroupInterface::setReplanDelay(double delay) +{ + if (delay < 0.0) + { + RCLCPP_ERROR(LOGGER, "Tried to set negative replan delay"); + } + else + { + RCLCPP_DEBUG_STREAM(LOGGER, "Replan Delay: " << delay); + impl_->replan_delay_ = delay; + } } std::vector MoveGroupInterface::getKnownConstraints() const diff --git a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp index cdfc1f4e98..926fb2e652 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp +++ b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp @@ -160,18 +160,10 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl { if (std::find(object_ids.begin(), object_ids.end(), collision_object.id) != object_ids.end()) { - if (collision_object.mesh_poses.empty() && collision_object.primitive_poses.empty()) - continue; - if (!collision_object.mesh_poses.empty()) - result[collision_object.id] = collision_object.mesh_poses[0]; - else - result[collision_object.id] = collision_object.primitive_poses[0]; + result[collision_object.id] = collision_object.pose; } } } - else - RCLCPP_WARN(LOGGER, "Could not call planning scene service to get object names"); - return result; } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index fd23bab662..9db5833343 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -245,6 +245,19 @@ void MotionPlanningDisplay::onInitialize() rviz_common::WindowManagerInterface* window_context = context_->getWindowManager(); frame_ = new MotionPlanningFrame(this, context_, window_context ? window_context->getParentWindow() : nullptr); + + std::string host_param; + if (node_->get_parameter("warehouse_host", host_param)) + { + frame_->ui_->database_host->setText(QString::fromStdString(host_param)); + } + + int port; + if (node_->get_parameter("warehouse_port", port)) + { + frame_->ui_->database_port->setValue(port); + } + connect(frame_, SIGNAL(configChanged()), this->getModel(), SIGNAL(configChanged())); resetStatusTextColor(); addStatusText("Initialized."); @@ -1323,18 +1336,6 @@ void MotionPlanningDisplay::load(const rviz_common::Config& config) PlanningSceneDisplay::load(config); if (frame_) { - QString host; - std::string host_param; - if (config.mapGetString("MoveIt_Warehouse_Host", &host)) - frame_->ui_->database_host->setText(host); - else if (node_->get_parameter("warehouse_host", host_param)) - { - host = QString::fromStdString(host_param); - frame_->ui_->database_host->setText(host); - } - int port; - if (config.mapGetInt("MoveIt_Warehouse_Port", &port) || node_->get_parameter("warehouse_port", port)) - frame_->ui_->database_port->setValue(port); float d; if (config.mapGetFloat("MoveIt_Planning_Time", &d)) frame_->ui_->planning_time->setValue(d); @@ -1382,7 +1383,6 @@ void MotionPlanningDisplay::load(const rviz_common::Config& config) } else { - std::string node_name = rclcpp::names::append(getMoveGroupNS(), "move_group"); double val; if (node_->get_parameter("default_workspace_bounds", val)) { @@ -1399,9 +1399,6 @@ void MotionPlanningDisplay::save(rviz_common::Config config) const PlanningSceneDisplay::save(config); if (frame_) { - config.mapSetValue("MoveIt_Warehouse_Host", frame_->ui_->database_host->text()); - config.mapSetValue("MoveIt_Warehouse_Port", frame_->ui_->database_port->value()); - // "Options" Section config.mapSetValue("MoveIt_Planning_Time", frame_->ui_->planning_time->value()); config.mapSetValue("MoveIt_Planning_Attempts", frame_->ui_->planning_attempts->value()); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui index 77c802bb7a..221318fd01 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui @@ -147,7 +147,7 @@ - 127.0.0.1 + @@ -164,7 +164,7 @@ 65535 - 33829 + 0 diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index f24a48e6b7..80704801d9 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -200,7 +200,7 @@ void PlanningSceneDisplay::onInitialize() node_ = ros_node_abstraction->get_raw_node(); planning_scene_topic_property_->initialize(ros_node_abstraction); - // the scene node that contains everything + // the scene node that contains everything and is located at the planning frame planning_scene_node_ = scene_node_->createChildSceneNode(); if (robot_category_) @@ -652,6 +652,8 @@ void PlanningSceneDisplay::update(float wall_dt, float ros_dt) executeMainLoopJobs(); + calculateOffsetPosition(); + if (planning_scene_monitor_) updateInternal(wall_dt, ros_dt); } @@ -664,7 +666,6 @@ void PlanningSceneDisplay::updateInternal(float wall_dt, float /*ros_dt*/) planning_scene_needs_render_)) { renderPlanningScene(); - calculateOffsetPosition(); current_scene_time_ = 0.0f; robot_state_needs_render_ = false; planning_scene_needs_render_ = false; diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp index d3450df244..8f389f0e21 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp @@ -666,7 +666,7 @@ bool StartScreenWidget::extractPackageNameFromPath() { QMessageBox::warning(this, "Package Not Found In ROS Workspace", QString("ROS was unable to find the package name '") - .append(config_data_->urdf_pkg_name_.c_str()) + .append(package_name.c_str()) .append("' within the ROS workspace. This may cause issues later.")); } diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/move_group.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/move_group.launch index 2d1f364601..ce6f1334d9 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/move_group.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/move_group.launch @@ -47,23 +47,22 @@ - + - + - + - + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_pipeline.launch.xml index 8e110c8cc7..4b4d0d663a 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_pipeline.launch.xml @@ -5,6 +5,6 @@ - +