From e01d5387a7b5f6096c61dbba26a4af5012766600 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 15 Dec 2023 18:58:46 +0100 Subject: [PATCH 01/14] Fix angular distance calculation in floating joint model (backport #2538) (#2543) (cherry picked from commit 83ff55a4adb5ab1c4b0813e1220e4b3242190c88) Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Co-authored-by: Sebastian Castro --- .../robot_model/src/floating_joint_model.cpp | 11 ++--- .../test/test_robot_trajectory.cpp | 47 +++++++++++++++++-- 2 files changed, 48 insertions(+), 10 deletions(-) diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index dfe6ba5e55..873d348afe 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -36,6 +36,7 @@ /* Author: Ioan Sucan */ #include +#include #include #include #include @@ -121,12 +122,10 @@ double FloatingJointModel::distanceTranslation(const double* values1, const doub double FloatingJointModel::distanceRotation(const double* values1, const double* values2) const { - double dq = - fabs(values1[3] * values2[3] + values1[4] * values2[4] + values1[5] * values2[5] + values1[6] * values2[6]); - if (dq + std::numeric_limits::epsilon() >= 1.0) - return 0.0; - else - return acos(dq); + // The values are in "xyzw" order but Eigen expects "wxyz". + const auto q1 = Eigen::Quaterniond(values1[6], values1[3], values1[4], values1[5]).normalized(); + const auto q2 = Eigen::Quaterniond(values2[6], values2[3], values2[4], values2[5]).normalized(); + return q2.angularDistance(q1); } void FloatingJointModel::interpolate(const double* from, const double* to, const double t, double* state) const diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp index b80ff79929..c8fcb8aa9a 100644 --- a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp @@ -54,7 +54,7 @@ class RobotTrajectoryTestFixture : public testing::Test { robot_model_ = moveit::core::loadTestingRobotModel(robot_model_name_); robot_state_ = std::make_shared(robot_model_); - robot_state_->setToDefaultValues(arm_jmg_name_, arm_state_name_); + robot_state_->setToDefaultValues(); robot_state_->setVariableVelocity(/*index*/ 0, /*value*/ 1.0); robot_state_->setVariableAcceleration(/*index*/ 0, /*value*/ -0.1); robot_state_->update(); @@ -66,7 +66,7 @@ class RobotTrajectoryTestFixture : public testing::Test void initTestTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory) { - // Init a traj + // Init a trajectory ASSERT_TRUE(robot_model_->hasJointModelGroup(arm_jmg_name_)) << "Robot model does not have group: " << arm_jmg_name_; @@ -78,7 +78,10 @@ class RobotTrajectoryTestFixture : public testing::Test double duration_from_previous = 0.1; std::size_t waypoint_count = 5; for (std::size_t ix = 0; ix < waypoint_count; ++ix) + { trajectory->addSuffixWayPoint(*robot_state_, duration_from_previous); + } + // Quick check that getDuration is working correctly EXPECT_EQ(trajectory->getDuration(), duration_from_previous * waypoint_count) << "Generated trajectory duration incorrect"; @@ -302,6 +305,17 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryLength) { robot_trajectory::RobotTrajectoryPtr trajectory; initTestTrajectory(trajectory); + EXPECT_FLOAT_EQ(robot_trajectory::path_length(*trajectory), 0.0); + + // modify joint values so the smoothness is nonzero + std::vector positions; + for (size_t i = 0; i < trajectory->size(); ++i) + { + auto waypoint = trajectory->getWayPointPtr(i); + waypoint->copyJointGroupPositions(arm_jmg_name_, positions); + positions[0] += 0.01 * i; + waypoint->setJointGroupPositions(arm_jmg_name_, positions); + } EXPECT_GT(robot_trajectory::path_length(*trajectory), 0.0); } @@ -310,6 +324,16 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectorySmoothness) robot_trajectory::RobotTrajectoryPtr trajectory; initTestTrajectory(trajectory); + // modify joint values so the smoothness is nonzero + std::vector positions; + for (size_t i = 0; i < trajectory->size(); ++i) + { + auto waypoint = trajectory->getWayPointPtr(i); + waypoint->copyJointGroupPositions(arm_jmg_name_, positions); + positions[0] += 0.01 * i; + waypoint->setJointGroupPositions(arm_jmg_name_, positions); + } + const auto smoothness = robot_trajectory::smoothness(*trajectory); ASSERT_TRUE(smoothness.has_value()); EXPECT_GT(smoothness.value(), 0.0); @@ -324,13 +348,28 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDensity) robot_trajectory::RobotTrajectoryPtr trajectory; initTestTrajectory(trajectory); - const auto density = robot_trajectory::waypoint_density(*trajectory); + // If trajectory has all equal state, and length zero, density should be null. + auto density = robot_trajectory::waypoint_density(*trajectory); + ASSERT_FALSE(density.has_value()); + + // modify joint values so the density is nonzero + std::vector positions; + for (size_t i = 0; i < trajectory->size(); ++i) + { + auto waypoint = trajectory->getWayPointPtr(i); + waypoint->copyJointGroupPositions(arm_jmg_name_, positions); + positions[0] += 0.01 * i; + waypoint->setJointGroupPositions(arm_jmg_name_, positions); + } + + density = robot_trajectory::waypoint_density(*trajectory); ASSERT_TRUE(density.has_value()); EXPECT_GT(density.value(), 0.0); // Check for empty trajectory trajectory->clear(); - EXPECT_FALSE(robot_trajectory::waypoint_density(*trajectory).has_value()); + density = robot_trajectory::waypoint_density(*trajectory); + EXPECT_FALSE(density.has_value()); } int main(int argc, char** argv) From e9f71ed770eb728a4b3c38eac7ef01b4519996a1 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 15 Dec 2023 19:02:49 +0100 Subject: [PATCH 02/14] Don't assume gripper controller for single joint control in MoveIt Setup Assistant (backport #2555) (#2559) * For single joint controllers which are not gripper controllers, still output joints list * Use OR * Only check for GripperActionController Co-authored-by: Sebastian Jahr --------- Co-authored-by: Sebastian Jahr (cherry picked from commit 81094a63898ace7829687d2d6aa3ccb3cdd81b58) Co-authored-by: Forrest Rogers-Marcovitz <39061824+forrest-rm@users.noreply.github.com> --- .../moveit_setup_controllers/src/ros2_controllers_config.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp index 3ce21bffe6..97f1907211 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp +++ b/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp @@ -192,7 +192,7 @@ bool ROS2ControllersConfig::GeneratedControllersConfig::writeYaml(YAML::Emitter& emitter << YAML::Value; emitter << YAML::BeginMap; { - if (ci.joints_.size() != 1) + if (ci.type_ != "position_controllers/GripperActionController") { emitter << YAML::Key << "joints" << YAML::Value << ci.joints_; } From d1e097a6082b91bc75f1d3d0b56519a76ed41c0e Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 15 Dec 2023 19:43:20 +0100 Subject: [PATCH 03/14] =?UTF-8?q?=F0=9F=9B=A0=EF=B8=8F=20Bump=20actions/se?= =?UTF-8?q?tup-python=20from=204=20to=205=20(backport=20#2585)=20(#2611)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Bumps [actions/setup-python](https://github.com/actions/setup-python) from 4 to 5. - [Release notes](https://github.com/actions/setup-python/releases) - [Commits](https://github.com/actions/setup-python/compare/v4...v5) --- updated-dependencies: - dependency-name: actions/setup-python dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> Co-authored-by: Sebastian Jahr (cherry picked from commit a84e71ac317510ac72292ad35347cfbb35b2db0a) Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/format.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml index 95a8dc0bf4..44537cde76 100644 --- a/.github/workflows/format.yaml +++ b/.github/workflows/format.yaml @@ -16,7 +16,7 @@ jobs: runs-on: ubuntu-22.04 steps: - uses: actions/checkout@v4 - - uses: actions/setup-python@v4 + - uses: actions/setup-python@v5 with: python-version: '3.10' - name: Install clang-format-12 From b4e9c8662ebf61f45ad5508c5345942ff2df0eab Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Tue, 9 Jan 2024 10:37:38 -0500 Subject: [PATCH 04/14] [Humble] Get ros2_control from latest binaries (#2635) --- moveit2.repos | 4 ---- 1 file changed, 4 deletions(-) diff --git a/moveit2.repos b/moveit2.repos index 0d855cfcf2..6c9f2668e4 100644 --- a/moveit2.repos +++ b/moveit2.repos @@ -9,7 +9,3 @@ repositories: type: git url: https://github.com/ros-planning/moveit_resources.git version: humble - ros2_control: - type: git - url: https://github.com/ros-controls/ros2_control.git - version: 2.13.0 From 7ce2fa90dbf3d7841306a0e868cad473b001f6ba Mon Sep 17 00:00:00 2001 From: Nacho Mellado Date: Tue, 23 Jan 2024 15:42:23 +0100 Subject: [PATCH 05/14] [TOTG] Exit loop when position can't change (backport #2307) (#2646) Co-authored-by: Sebastian Jahr --- .../time_optimal_trajectory_generation.cpp | 16 ++++++++++ ...est_time_optimal_trajectory_generation.cpp | 30 +++++++++++++++++++ 2 files changed, 46 insertions(+) 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 ba0374adb5..cc5de33234 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -324,6 +324,12 @@ Trajectory::Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, co , time_step_(time_step) , cached_time_(std::numeric_limits::max()) { + if (time_step_ == 0) + { + valid_ = false; + RCLCPP_ERROR(LOGGER, "The trajectory is invalid because the time step is 0."); + return; + } trajectory_.push_back(TrajectoryStep(0.0, 0.0)); double after_acceleration = getMinMaxPathAcceleration(0.0, 0.0, true); while (valid_ && !integrateForward(trajectory_, after_acceleration) && valid_) @@ -565,6 +571,16 @@ bool Trajectory::integrateForward(std::list& trajectory, double trajectory.push_back(TrajectoryStep(path_pos, path_vel)); acceleration = getMinMaxPathAcceleration(path_pos, path_vel, true); + if (path_vel == 0 && acceleration == 0) + { + // The position will never change if velocity and acceleration are zero. + // The loop will spin indefinitely as no exit condition is met. + valid_ = false; + RCLCPP_ERROR(LOGGER, "Error while integrating forward: zero acceleration and velocity. Are any relevant " + "acceleration components limited to zero?"); + return true; + } + if (path_vel > getAccelerationMaxPathVelocity(path_pos) || path_vel > getVelocityMaxPathVelocity(path_pos)) { // Find more accurate intersection with max-velocity curve using bisection diff --git a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp index fa4a909d14..8863f80b92 100644 --- a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp @@ -496,6 +496,36 @@ TEST(time_optimal_trajectory_generation, testPluginAPI) ASSERT_EQ(first_trajectory_msg_end, third_trajectory_msg_end); } +TEST(time_optimal_trajectory_generation, testRelevantZeroMaxAccelerationsInvalidateTrajectory) +{ + const Eigen::Vector2d max_velocity(1, 1); + const Path path(std::list{ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 1) }); + + EXPECT_FALSE(Trajectory(path, max_velocity, /*max_acceleration=*/Eigen::Vector2d(0, 1)).isValid()); + EXPECT_FALSE(Trajectory(path, max_velocity, /*max_acceleration=*/Eigen::Vector2d(1, 0)).isValid()); + EXPECT_FALSE(Trajectory(path, max_velocity, /*max_acceleration=*/Eigen::Vector2d(0, 0)).isValid()); +} + +TEST(time_optimal_trajectory_generation, testIrrelevantZeroMaxAccelerationsDontInvalidateTrajectory) +{ + const Eigen::Vector2d max_velocity(1, 1); + + EXPECT_TRUE(Trajectory(Path(std::list{ Eigen::Vector2d(0, 0), Eigen::Vector2d(0, 1) }), max_velocity, + /*max_acceleration=*/Eigen::Vector2d(0, 1)) + .isValid()); + EXPECT_TRUE(Trajectory(Path(std::list{ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 0) }), max_velocity, + /*max_acceleration=*/Eigen::Vector2d(1, 0)) + .isValid()); +} + +TEST(time_optimal_trajectory_generation, testTimeStepZeroMakesTrajectoryInvalid) +{ + EXPECT_FALSE(Trajectory(Path(std::list{ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 1) }), + /*max_velocity=*/Eigen::Vector2d(1, 1), /*max_acceleration=*/Eigen::Vector2d(1, 1), + /*time_step=*/0) + .isValid()); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From 65970f09de726dd97e93ffb3841e6bdd8fe80257 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 20 Feb 2024 08:48:23 -0700 Subject: [PATCH 06/14] Pass along move_group_capabilities parameters (#2587) (#2696) --- moveit_configs_utils/moveit_configs_utils/launches.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/moveit_configs_utils/moveit_configs_utils/launches.py b/moveit_configs_utils/moveit_configs_utils/launches.py index 206ae94115..e8e588bd81 100644 --- a/moveit_configs_utils/moveit_configs_utils/launches.py +++ b/moveit_configs_utils/moveit_configs_utils/launches.py @@ -197,7 +197,12 @@ def generate_move_group_launch(moveit_config): DeclareBooleanLaunchArg("publish_monitored_planning_scene", default_value=True) ) # load non-default MoveGroup capabilities (space separated) - ld.add_action(DeclareLaunchArgument("capabilities", default_value="")) + ld.add_action( + DeclareLaunchArgument( + "capabilities", + default_value=moveit_config.move_group_capabilities["capabilities"], + ) + ) # inhibit these default MoveGroup capabilities (space separated) ld.add_action(DeclareLaunchArgument("disable_capabilities", default_value="")) From 4c8e3535cbe3618237c3731a98a3005d9bb7534a Mon Sep 17 00:00:00 2001 From: reidchristopher Date: Tue, 27 Feb 2024 03:48:38 -0600 Subject: [PATCH 07/14] Update moveit::core::error_msg_to_string (#2689) --- .../include/moveit/utils/moveit_error_code.h | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/moveit_core/utils/include/moveit/utils/moveit_error_code.h b/moveit_core/utils/include/moveit/utils/moveit_error_code.h index e42008f59a..b948cf3201 100644 --- a/moveit_core/utils/include/moveit/utils/moveit_error_code.h +++ b/moveit_core/utils/include/moveit/utils/moveit_error_code.h @@ -47,11 +47,7 @@ namespace core class MoveItErrorCode : public moveit_msgs::msg::MoveItErrorCodes { public: - MoveItErrorCode() - { - val = 0; - } - MoveItErrorCode(int code) + MoveItErrorCode(int code = 0) { val = code; } @@ -104,12 +100,18 @@ inline std::string error_code_to_string(MoveItErrorCode error_code) return std::string("START_STATE_IN_COLLISION"); case moveit::core::MoveItErrorCode::START_STATE_VIOLATES_PATH_CONSTRAINTS: return std::string("START_STATE_VIOLATES_PATH_CONSTRAINTS"); + case moveit::core::MoveItErrorCode::START_STATE_INVALID: + return std::string("START_STATE_INVALID"); case moveit::core::MoveItErrorCode::GOAL_IN_COLLISION: return std::string("GOAL_IN_COLLISION"); case moveit::core::MoveItErrorCode::GOAL_VIOLATES_PATH_CONSTRAINTS: return std::string("GOAL_VIOLATES_PATH_CONSTRAINTS"); case moveit::core::MoveItErrorCode::GOAL_CONSTRAINTS_VIOLATED: return std::string("GOAL_CONSTRAINTS_VIOLATED"); + case moveit::core::MoveItErrorCode::GOAL_STATE_INVALID: + return std::string("GOAL_STATE_INVALID"); + case moveit::core::MoveItErrorCode::UNRECOGNIZED_GOAL_TYPE: + return std::string("UNRECOGNIZED_GOAL_TYPE"); case moveit::core::MoveItErrorCode::INVALID_GROUP_NAME: return std::string("INVALID_GROUP_NAME"); case moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS: @@ -130,9 +132,14 @@ inline std::string error_code_to_string(MoveItErrorCode error_code) return std::string("SENSOR_INFO_STALE"); case moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE: return std::string("COMMUNICATION_FAILURE"); + case moveit::core::MoveItErrorCode::CRASH: + return std::string("CRASH"); + case moveit::core::MoveItErrorCode::ABORT: + return std::string("ABORT"); case moveit::core::MoveItErrorCode::NO_IK_SOLUTION: return std::string("NO_IK_SOLUTION"); } + return std::string("Unrecognized MoveItErrorCode. This should never happen!"); } } // namespace core From 360299c31dd266ea0892c4e2eddcba7e4f900cf8 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 14 Mar 2024 14:42:01 +0100 Subject: [PATCH 08/14] Use ACM in all MoveIt Servo collision checks (#2643) Co-authored-by: Sebastian Castro --- .../include/moveit_servo/collision_check.h | 3 --- moveit_ros/moveit_servo/src/collision_check.cpp | 14 +++++--------- 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h index b76ae98b40..94ecc53635 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h @@ -80,9 +80,6 @@ class CollisionCheck /** \brief Run one iteration of collision checking */ void run(); - /** \brief Get a read-only copy of the planning scene */ - planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const; - // Pointer to the ROS node const std::shared_ptr node_; diff --git a/moveit_ros/moveit_servo/src/collision_check.cpp b/moveit_ros/moveit_servo/src/collision_check.cpp index 14de0cb8ca..69ae157b32 100644 --- a/moveit_ros/moveit_servo/src/collision_check.cpp +++ b/moveit_ros/moveit_servo/src/collision_check.cpp @@ -77,11 +77,6 @@ CollisionCheck::CollisionCheck(const rclcpp::Node::SharedPtr& node, const ServoP current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); } -planning_scene_monitor::LockedPlanningSceneRO CollisionCheck::getLockedPlanningSceneRO() const -{ - return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_); -} - void CollisionCheck::start() { timer_ = node_->create_wall_timer(std::chrono::duration(period_), [this]() { return run(); }); @@ -101,16 +96,17 @@ void CollisionCheck::run() // Do a timer-safe distance-based collision detection collision_result_.clear(); - getLockedPlanningSceneRO()->getCollisionEnv()->checkRobotCollision(collision_request_, collision_result_, - *current_state_); + auto locked_scene = planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_); + locked_scene->getCollisionEnv()->checkRobotCollision(collision_request_, collision_result_, *current_state_, + locked_scene->getAllowedCollisionMatrix()); scene_collision_distance_ = collision_result_.distance; collision_detected_ |= collision_result_.collision; collision_result_.print(); collision_result_.clear(); // Self-collisions and scene collisions are checked separately so different thresholds can be used - getLockedPlanningSceneRO()->getCollisionEnvUnpadded()->checkSelfCollision( - collision_request_, collision_result_, *current_state_, getLockedPlanningSceneRO()->getAllowedCollisionMatrix()); + locked_scene->getCollisionEnvUnpadded()->checkSelfCollision(collision_request_, collision_result_, *current_state_, + locked_scene->getAllowedCollisionMatrix()); self_collision_distance_ = collision_result_.distance; collision_detected_ |= collision_result_.collision; collision_result_.print(); From 44a2edbb39723527a142b33c4d5b75408a53b4a6 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 14 Mar 2024 14:45:03 +0100 Subject: [PATCH 09/14] Use different packages for launch and config packages in generate_demo_launch (backport #2647) (#2650) (cherry picked from commit 627e95ac347ce7b58145d522b2f60a8c6b876097) Co-authored-by: Forrest Rogers-Marcovitz <39061824+forrest-rm@users.noreply.github.com> Co-authored-by: Sebastian Jahr --- .../moveit_configs_utils/launches.py | 21 ++++++++++++------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/moveit_configs_utils/moveit_configs_utils/launches.py b/moveit_configs_utils/moveit_configs_utils/launches.py index e8e588bd81..4985b2caab 100644 --- a/moveit_configs_utils/moveit_configs_utils/launches.py +++ b/moveit_configs_utils/moveit_configs_utils/launches.py @@ -249,10 +249,12 @@ def generate_move_group_launch(moveit_config): return ld -def generate_demo_launch(moveit_config): +def generate_demo_launch(moveit_config, launch_package_path=None): """ Launches a self contained demo + launch_package_path is optional to use different launch and config packages + Includes * static_virtual_joint_tfs * robot_state_publisher @@ -261,6 +263,9 @@ def generate_demo_launch(moveit_config): * warehouse_db (optional) * ros2_control_node + controller spawners """ + if launch_package_path == None: + launch_package_path = moveit_config.package_path + ld = LaunchDescription() ld.add_action( DeclareBooleanLaunchArg( @@ -277,11 +282,11 @@ def generate_demo_launch(moveit_config): ) ) ld.add_action(DeclareBooleanLaunchArg("use_rviz", default_value=True)) - # If there are virtual joints, broadcast static tf by including virtual_joints launch virtual_joints_launch = ( - moveit_config.package_path / "launch/static_virtual_joint_tfs.launch.py" + launch_package_path / "launch/static_virtual_joint_tfs.launch.py" ) + if virtual_joints_launch.exists(): ld.add_action( IncludeLaunchDescription( @@ -293,7 +298,7 @@ def generate_demo_launch(moveit_config): ld.add_action( IncludeLaunchDescription( PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/rsp.launch.py") + str(launch_package_path / "launch/rsp.launch.py") ), ) ) @@ -301,7 +306,7 @@ def generate_demo_launch(moveit_config): ld.add_action( IncludeLaunchDescription( PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/move_group.launch.py") + str(launch_package_path / "launch/move_group.launch.py") ), ) ) @@ -310,7 +315,7 @@ def generate_demo_launch(moveit_config): ld.add_action( IncludeLaunchDescription( PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/moveit_rviz.launch.py") + str(launch_package_path / "launch/moveit_rviz.launch.py") ), condition=IfCondition(LaunchConfiguration("use_rviz")), ) @@ -320,7 +325,7 @@ def generate_demo_launch(moveit_config): ld.add_action( IncludeLaunchDescription( PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/warehouse_db.launch.py") + str(launch_package_path / "launch/warehouse_db.launch.py") ), condition=IfCondition(LaunchConfiguration("db")), ) @@ -341,7 +346,7 @@ def generate_demo_launch(moveit_config): ld.add_action( IncludeLaunchDescription( PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/spawn_controllers.launch.py") + str(launch_package_path / "launch/spawn_controllers.launch.py") ), ) ) From 01c86ecb024d20ea2108d0724dda953b79a71113 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 21 Mar 2024 16:18:35 +0100 Subject: [PATCH 10/14] Invoke OMPL debug print only when debug logging is enabled (backport #2608) (#2762) --------- Co-authored-by: Igor Medvedev <55915597+medvedevigorek@users.noreply.github.com> Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> --- .../src/model_based_planning_context.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index b961c0e02e..6ffb553b11 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -755,9 +755,13 @@ void ompl_interface::ModelBasedPlanningContext::postSolve() RCLCPP_DEBUG(LOGGER, "There were %d valid motions and %d invalid motions.", v, iv); // Debug OMPL setup and solution - std::stringstream debug_out; - ompl_simple_setup_->print(debug_out); - RCLCPP_DEBUG(LOGGER, "%s", rclcpp::get_c_string(debug_out.str())); + RCLCPP_DEBUG(LOGGER, "%s", + [&] { + std::stringstream debug_out; + ompl_simple_setup_->print(debug_out); + return debug_out.str(); + }() + .c_str()); } bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& res) From 5cb6c106b1253c4092ef65fe0e7a18dda4801cd9 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 25 Mar 2024 12:58:06 +0100 Subject: [PATCH 11/14] Fix doc reference to non-existent function (#2765) (#2766) (cherry picked from commit 43c5f6d03e423b53afa3a27f659adf2e6c3f4b5b) Co-authored-by: Henning Kayser --- .../robot_model/include/moveit/robot_model/joint_model_group.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h index 1b9fe18b77..b7cf3efb12 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h @@ -406,7 +406,7 @@ class JointModelGroup void interpolate(const double* from, const double* to, double t, double* state) const; /** \brief Get the number of variables that describe this joint group. This includes variables necessary for mimic - joints, so will always be >= the number of items returned by getActiveVariableNames() */ + joints, so will always be >= getActiveVariableCount() */ unsigned int getVariableCount() const { return variable_count_; From 82f6eb34e0cc86becbed128235d3d9e4bf8e8f38 Mon Sep 17 00:00:00 2001 From: Alex Navarro <56406642+alexnavtt@users.noreply.github.com> Date: Wed, 3 Apr 2024 02:38:38 -0500 Subject: [PATCH 12/14] Backport of #2172 and #2684 into Humble (#2779) * Parse xacro args from .setup_assistant config in MoveIt Configs Builder (#2172) Co-authored-by: Jafar (cherry picked from commit 79a2fa5c71763f1379aa2b0db436ea599b7fa12a) * Fix xacro args loading issue (#2684) * Fixed xacro args loading issue * Formatting fixes with pre-commit action --------- (cherry picked from commit cdb20aeaeb299e86fec0f44fde0b9f235a737ee4) --------- Co-authored-by: Anthony Baker --- .../moveit_configs_builder.py | 25 ++++++++++++------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 9c3636fb34..af23afc2c4 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -162,6 +162,7 @@ def __init__( self.__urdf_package = None self.__urdf_file_path = None + self.__urdf_xacro_args = None self.__srdf_file_path = None modified_urdf_path = Path("config") / (self.__robot_name + ".urdf.xacro") @@ -172,15 +173,20 @@ def __init__( if setup_assistant_file.exists(): setup_assistant_yaml = load_yaml(setup_assistant_file) config = setup_assistant_yaml.get("moveit_setup_assistant_config", {}) - urdf_config = config.get("urdf", config.get("URDF")) - if urdf_config and self.__urdf_package is None: - self.__urdf_package = Path( - get_package_share_directory(urdf_config["package"]) - ) - self.__urdf_file_path = Path(urdf_config["relative_path"]) - srdf_config = config.get("srdf", config.get("SRDF")) - if srdf_config: + if urdf_config := config.get("urdf", config.get("URDF")): + if self.__urdf_package is None: + self.__urdf_package = Path( + get_package_share_directory(urdf_config["package"]) + ) + self.__urdf_file_path = Path(urdf_config["relative_path"]) + + if xacro_args := urdf_config.get("xacro_args"): + self.__urdf_xacro_args = dict( + arg.split(":=") for arg in xacro_args.split(" ") if arg + ) + + if srdf_config := config.get("srdf", config.get("SRDF")): self.__srdf_file_path = Path(srdf_config["relative_path"]) if not self.__urdf_package or not self.__urdf_file_path: @@ -224,7 +230,8 @@ def robot_description( try: self.__moveit_configs.robot_description = { self.__robot_description: load_xacro( - robot_description_file_path, mappings=mappings + robot_description_file_path, + mappings=mappings or self.__urdf_xacro_args, ) } except ParameterBuilderFileNotFoundError as e: From b40f9ffb32c53d4516a232f4c731035232377a53 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 15 Apr 2024 13:01:28 +0200 Subject: [PATCH 13/14] CI: Allow non-interactive installation of rmw-connextdds (#2790) --- .docker/ci/Dockerfile | 2 ++ .docker/release/Dockerfile | 3 +++ 2 files changed, 5 insertions(+) diff --git a/.docker/ci/Dockerfile b/.docker/ci/Dockerfile index c6aa5ee42c..994d1bfe89 100644 --- a/.docker/ci/Dockerfile +++ b/.docker/ci/Dockerfile @@ -6,6 +6,8 @@ FROM ros:${ROS_DISTRO}-ros-base LABEL maintainer Robert Haschke rhaschke@techfak.uni-bielefeld.de ENV TERM xterm +# Allow non-interactive installation of ros-humble-rmw-connextdds +ENV RTI_NC_LICENSE_ACCEPTED yes # Setup (temporary) ROS workspace WORKDIR /root/ws_moveit diff --git a/.docker/release/Dockerfile b/.docker/release/Dockerfile index 8a1b6992c4..f95c3cc86a 100644 --- a/.docker/release/Dockerfile +++ b/.docker/release/Dockerfile @@ -5,6 +5,9 @@ ARG ROS_DISTRO=rolling FROM ros:${ROS_DISTRO}-ros-base LABEL maintainer Robert Haschke rhaschke@techfak.uni-bielefeld.de +# Allow non-interactive installation of ros-humble-rmw-connextdds +ENV RTI_NC_LICENSE_ACCEPTED yes + # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size RUN apt-get update -q && \ apt-get upgrade -q -y && \ From 08e9e8dce53d191f1ff0512f7986edd25cd45990 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 18 Apr 2024 20:59:23 +0200 Subject: [PATCH 14/14] CI: Fix building of ikfast plugins (#2791) Ignore missing authentication for Indigo packages using --force-yes. (cherry picked from commit 9ee6669c05d2b02c648a2bc8c484ce6a8052aa8b) Co-authored-by: Robert Haschke --- .../scripts/auto_create_ikfast_moveit_plugin.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh b/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh index e819fd91fe..02bb5cd1d0 100755 --- a/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh +++ b/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh @@ -120,7 +120,7 @@ FROM personalrobotics/ros-openrave RUN apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && \ apt-key del 421C365BD9FF1F717815A3895523BAEEB01FA116 && \ apt-get update && \ - apt-get install -y --no-install-recommends python-pip build-essential liblapack-dev ros-indigo-collada-urdf && \ + apt-get install -y --force-yes --no-install-recommends python-pip build-essential liblapack-dev ros-indigo-collada-urdf && \ apt-get clean && rm -rf /var/lib/apt/lists/* # enforce a specific version of sympy, which is known to work with OpenRave RUN pip install git+https://github.com/sympy/sympy.git@sympy-0.7.1