From cce943457990cb2bafe84c85a5fb9028b3ce722b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 15 Sep 2021 15:00:44 +0200 Subject: [PATCH 001/229] PlanningSceneDisplay: always update the main scene node's pose (#2876) --- .../src/planning_scene_display.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 46bd04f67d..bea40fafa0 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 @@ -188,7 +188,7 @@ void PlanningSceneDisplay::onInitialize() { Display::onInitialize(); - // 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_) @@ -640,6 +640,8 @@ void PlanningSceneDisplay::update(float wall_dt, float ros_dt) executeMainLoopJobs(); + calculateOffsetPosition(); + if (planning_scene_monitor_) updateInternal(wall_dt, ros_dt); } @@ -652,7 +654,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; From c5ea816fec17e3590872c52e31e96c251c88af12 Mon Sep 17 00:00:00 2001 From: Gauthier Hentz <64833674+gautz@users.noreply.github.com> Date: Fri, 17 Sep 2021 17:09:58 +0200 Subject: [PATCH 002/229] PSI: get object.pose from new msg field (#2877) --- .../src/planning_scene_interface.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) 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 3c2b4a6db2..aeea4c995e 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 @@ -156,12 +156,7 @@ 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; } } return result; From 8c499f5d3dea8a75c6219be765d5a824e34d9056 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Mon, 20 Sep 2021 22:24:32 +0200 Subject: [PATCH 003/229] MPD: do not save/restore warehouse parameters (#2865) If we reload these values from the config, setting the ROS parameters is much less useful. At least the *type* of warehouse_ros plugin (mongo or sqlite) cannot be selected in the display, so you will probably need to meddle with the parameters anyway if you want to connect to a different db. search for parameters warehouse_host/port because they are usually set at the top level, but you might want to set them differently for different move_groups. --- .../src/motion_planning_display.cpp | 29 +++++++------------ .../ui/motion_planning_rviz_plugin_frame.ui | 4 +-- 2 files changed, 13 insertions(+), 20 deletions(-) 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 919797a49b..dc16f4c7ac 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 @@ -228,6 +228,16 @@ void MotionPlanningDisplay::onInitialize() rviz::WindowManagerInterface* window_context = context_->getWindowManager(); frame_ = new MotionPlanningFrame(this, context_, window_context ? window_context->getParentWindow() : nullptr); + + ros::NodeHandle move_group_nh(ros::names::append(getMoveGroupNS(), "move_group")); + std::string param_name; + std::string host_param; + int port; + if (move_group_nh.searchParam("warehouse_host", param_name) && move_group_nh.getParam(param_name, host_param)) + frame_->ui_->database_host->setText(QString::fromStdString(host_param)); + if (move_group_nh.searchParam("warehouse_port", param_name) && move_group_nh.getParam(param_name, port)) + frame_->ui_->database_port->setValue(port); + connect(frame_, SIGNAL(configChanged()), this->getModel(), SIGNAL(configChanged())); resetStatusTextColor(); addStatusText("Initialized."); @@ -1313,19 +1323,6 @@ void MotionPlanningDisplay::load(const rviz::Config& config) PlanningSceneDisplay::load(config); if (frame_) { - QString host; - ros::NodeHandle nh; - std::string host_param; - if (config.mapGetString("MoveIt_Warehouse_Host", &host)) - frame_->ui_->database_host->setText(host); - else if (nh.getParam("warehouse_host", host_param)) - { - host = QString::fromStdString(host_param); - frame_->ui_->database_host->setText(host); - } - int port; - if (config.mapGetInt("MoveIt_Warehouse_Port", &port) || nh.getParam("warehouse_port", port)) - frame_->ui_->database_port->setValue(port); float d; if (config.mapGetFloat("MoveIt_Planning_Time", &d)) frame_->ui_->planning_time->setValue(d); @@ -1373,8 +1370,7 @@ void MotionPlanningDisplay::load(const rviz::Config& config) } else { - std::string node_name = ros::names::append(getMoveGroupNS(), "move_group"); - ros::NodeHandle nh(node_name); + ros::NodeHandle nh(ros::names::append(getMoveGroupNS(), "move_group")); double val; if (nh.getParam("default_workspace_bounds", val)) { @@ -1391,9 +1387,6 @@ void MotionPlanningDisplay::save(rviz::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 From 8ed1826cf3bfd0c51563e5f0e48247ca5eee0ba8 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 21 Sep 2021 14:00:56 -0600 Subject: [PATCH 004/229] Readability and consistency improvements in TOTG (#2882) * Use std::fabs() everywhere * Better comments --- .../src/time_optimal_trajectory_generation.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) 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 a873becbbb..03cf56dd9a 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -937,15 +937,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]); } } @@ -957,13 +958,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++) + 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) From daa87acbf7e9113b07ed8a5bcd69ed9104906cfe Mon Sep 17 00:00:00 2001 From: Tobias Fischer Date: Wed, 22 Sep 2021 16:45:47 +1000 Subject: [PATCH 005/229] Temporarily pin orocos-kdl version to fix cross-platform CI (#2883) --- .github/ci_cross_platform_env.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/ci_cross_platform_env.yml b/.github/ci_cross_platform_env.yml index 26cffa0f9e..53533848c6 100644 --- a/.github/ci_cross_platform_env.yml +++ b/.github/ci_cross_platform_env.yml @@ -13,3 +13,5 @@ dependencies: - ros-distro-mutex 0.1 noetic - ros-noetic-catkin - ros-noetic-ros-environment + - orocos-kdl 1.5.0 + - python-orocos-kdl 1.5.0 From d1e1c3d27ab33978fdcc5d7f9494afaa76496adc Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 23 Sep 2021 16:48:33 +0200 Subject: [PATCH 006/229] Fixup docker build - Enable bootstrapping if image doesn't yet exist - Update maintainer for Dockerfiles --- .docker/ci-testing/Dockerfile | 2 +- .docker/ci/Dockerfile | 1 - .docker/source/Dockerfile | 2 +- .github/workflows/docker.yaml | 3 +++ 4 files changed, 5 insertions(+), 3 deletions(-) diff --git a/.docker/ci-testing/Dockerfile b/.docker/ci-testing/Dockerfile index 5ccfde66a1..2da8f6c940 100644 --- a/.docker/ci-testing/Dockerfile +++ b/.docker/ci-testing/Dockerfile @@ -3,7 +3,7 @@ ARG IMAGE=noetic FROM moveit/moveit:${IMAGE}-ci -MAINTAINER Dave Coleman dave@picknik.ai +MAINTAINER Robert Haschke rhaschke@techfak.uni-bielefeld.de # Switch to ros-shadow-fixed RUN echo "deb http://packages.ros.org/ros-shadow-fixed/ubuntu `lsb_release -cs` main" | tee /etc/apt/sources.list.d/ros-latest.list diff --git a/.docker/ci/Dockerfile b/.docker/ci/Dockerfile index a81766108e..d01995b09e 100644 --- a/.docker/ci/Dockerfile +++ b/.docker/ci/Dockerfile @@ -16,7 +16,6 @@ COPY . src/moveit # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size # https://docs.docker.com/develop/develop-images/dockerfile_best-practices/#minimize-the-number-of-layers RUN \ - ls -a src/moveit/ && \ # Update apt package list as previous containers clear the cache apt-get -q update && \ apt-get -q -y dist-upgrade && \ diff --git a/.docker/source/Dockerfile b/.docker/source/Dockerfile index 5c29830386..a27afa26d3 100644 --- a/.docker/source/Dockerfile +++ b/.docker/source/Dockerfile @@ -5,7 +5,7 @@ ARG IMAGE=noetic FROM moveit/moveit:${IMAGE}-ci-testing -MAINTAINER Dave Coleman dave@picknik.ai +MAINTAINER Robert Haschke rhaschke@techfak.uni-bielefeld.de ENV PYTHONIOENCODING UTF-8 # Export ROS_UNDERLAY for downstream docker containers diff --git a/.github/workflows/docker.yaml b/.github/workflows/docker.yaml index e940e91515..83931c5467 100644 --- a/.github/workflows/docker.yaml +++ b/.github/workflows/docker.yaml @@ -25,6 +25,7 @@ jobs: steps: - uses: addnab/docker-run-action@v3 name: Check for apt updates + continue-on-error: true id: apt with: image: ${{ env.IMAGE }} @@ -68,6 +69,7 @@ jobs: steps: - uses: addnab/docker-run-action@v3 name: Check for apt updates + continue-on-error: true id: apt with: image: ${{ env.IMAGE }} @@ -111,6 +113,7 @@ jobs: steps: - uses: addnab/docker-run-action@v3 name: Check for apt updates + continue-on-error: true id: apt with: image: ${{ env.IMAGE }} From 37aa5e49aa7f21050886cb9b2e7cd5fab55e0f22 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 28 Sep 2021 16:37:54 +0200 Subject: [PATCH 007/229] MSA: Correctly state not-found package name The warning message was accessing the config_data_ variable, which was assigned just a few lines later. --- moveit_setup_assistant/src/widgets/start_screen_widget.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp index 757db3252d..941cf320ec 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.")); } From 049be73b8343d67ffde400fed9ed4c387c6ea9e5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 28 Sep 2021 22:25:04 +0200 Subject: [PATCH 008/229] Load all planning pipelines into their own namespace (#2888) Reduce code redundancy, specifying the namespace once in planning_pipeline.launch. --- .../moveit_config_pkg_template/launch/move_group.launch | 9 ++++----- .../launch/planning_pipeline.launch.xml | 2 +- 2 files changed, 5 insertions(+), 6 deletions(-) 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 5db4ea98e1..bfe6301154 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 @@ - + From 000bb45ce8859204759d2d2c1ba4c671c3b2603d Mon Sep 17 00:00:00 2001 From: Wolf Vollprecht Date: Mon, 4 Oct 2021 13:13:01 +0200 Subject: [PATCH 009/229] pin openssl 1.1.1* for now (#2895) to fix the robostack Windows build --- .github/ci_cross_platform_env.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/ci_cross_platform_env.yml b/.github/ci_cross_platform_env.yml index 53533848c6..e74f96701e 100644 --- a/.github/ci_cross_platform_env.yml +++ b/.github/ci_cross_platform_env.yml @@ -15,3 +15,4 @@ dependencies: - ros-noetic-ros-environment - orocos-kdl 1.5.0 - python-orocos-kdl 1.5.0 + - openssl 1.1.1* From 35275a0b9cc06fca93ca291558257f41a7f5773f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Mon, 4 Oct 2021 14:08:09 +0200 Subject: [PATCH 010/229] MGI: add missing replan/look options to interface (#2892) - reordered methods because looking requires replanning - there's no sense in wrapping methods in methods. just use pimpl-friend paradigm instead. Someone could rework all the other methods in the future. --- .../move_group_interface.h | 17 ++++- .../src/move_group_interface.cpp | 74 ++++++++++++------- 2 files changed, 62 insertions(+), 29 deletions(-) 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 fa447f2ec7..8838bcc411 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 @@ -782,13 +782,22 @@ class 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::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 4e6700f043..c1ceab384b 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 @@ -92,6 +92,8 @@ enum ActiveTargetType class MoveGroupInterface::MoveGroupInterfaceImpl { + friend MoveGroupInterface; + public: MoveGroupInterfaceImpl(const Options& opt, const std::shared_ptr& tf_buffer, const ros::WallDuration& wait_for_servers) @@ -119,8 +121,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 @@ -1052,29 +1056,6 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return allowed_planning_time_; } - void allowLooking(bool flag) - { - can_look_ = flag; - ROS_INFO_NAMED(LOGNAME, "Looking around: %s", can_look_ ? "yes" : "no"); - } - - void allowReplanning(bool flag) - { - can_replan_ = flag; - ROS_INFO_NAMED(LOGNAME, "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::MotionPlanRequest& request) const { request.group_name = opt_.group_name_; @@ -1324,7 +1305,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 @@ -2207,12 +2190,53 @@ void MoveGroupInterface::forgetJointValues(const std::string& name) void MoveGroupInterface::allowLooking(bool flag) { - impl_->allowLooking(flag); + impl_->can_look_ = flag; + ROS_DEBUG_NAMED(LOGNAME, "Looking around: %s", flag ? "yes" : "no"); +} + +void MoveGroupInterface::setLookAroundAttempts(int32_t attempts) +{ + if (attempts < 0) + { + ROS_ERROR_NAMED(LOGNAME, "Tried to set negative number of look-around attempts"); + } + else + { + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Look around attempts: " << attempts); + impl_->look_around_attempts_ = attempts; + } } void MoveGroupInterface::allowReplanning(bool flag) { - impl_->allowReplanning(flag); + impl_->can_replan_ = flag; + ROS_DEBUG_NAMED(LOGNAME, "Replanning: %s", flag ? "yes" : "no"); +} + +void MoveGroupInterface::setReplanAttempts(int32_t attempts) +{ + if (attempts < 0) + { + ROS_ERROR_NAMED(LOGNAME, "Tried to set negative number of replan attempts"); + } + else + { + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Replan Attempts: " << attempts); + impl_->replan_attempts_ = attempts; + } +} + +void MoveGroupInterface::setReplanDelay(double delay) +{ + if (delay < 0.0) + { + ROS_ERROR_NAMED(LOGNAME, "Tried to set negative replan delay"); + } + else + { + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Replan Delay: " << delay); + impl_->replan_delay_ = delay; + } } std::vector MoveGroupInterface::getKnownConstraints() const From d57721cf8503fe893347196cc1e1f9249ec57777 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Tue, 5 Oct 2021 21:27:28 +0200 Subject: [PATCH 011/229] fix uninitialized orientation in default shape pose (#2896) --- moveit_core/planning_scene/src/planning_scene.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 740d018478..43639be742 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -1757,8 +1757,12 @@ 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::Pose()); // Empty shape pose => Identity + { + // Empty shape pose => Identity + geometry_msgs::Pose identity; + identity.orientation.w = 1.0; + append(shapes::constructShapeFromMsg(shape_vector[i]), identity); + } else append(shapes::constructShapeFromMsg(shape_vector[i]), shape_poses_vector[i]); } From 56ffde6780fdcce86af3c6596a88442ec0290c50 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Mon, 18 Oct 2021 05:24:35 +0200 Subject: [PATCH 012/229] RobotState: write to correct array (#2909) Not an actual bug because both arrays share the same memory. As mentioned in https://github.com/ros-planning/moveit2/pull/683#pullrequestreview-780447848 --- moveit_core/robot_state/src/robot_state.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index e6fcda859f..f3de5936bb 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -470,7 +470,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, From 38b3010cb38c6928d0286f990b3aeb8862bc6861 Mon Sep 17 00:00:00 2001 From: cambel Date: Tue, 19 Oct 2021 18:05:00 +0900 Subject: [PATCH 013/229] Add test for pilz planner with attached objects (#2878) * Add test case for #2824 Co-authored-by: Cristian Beltran Co-authored-by: Joachim Schleicher Co-authored-by: jschleicher --- .../CMakeLists.txt | 2 + .../test/python_move_group_planning.py | 118 ++++++++++++++++++ .../test/python_move_group_planning.test | 9 ++ 3 files changed, 129 insertions(+) create mode 100755 moveit_planners/pilz_industrial_motion_planner/test/python_move_group_planning.py create mode 100644 moveit_planners/pilz_industrial_motion_planner/test/python_move_group_planning.test diff --git a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt index a8edc26b45..744ecb71e6 100644 --- a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt @@ -308,6 +308,8 @@ if(CATKIN_ENABLE_TESTING) ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} ) + add_rostest(test/python_move_group_planning.test) + ################## ####Unit Tests#### ################## diff --git a/moveit_planners/pilz_industrial_motion_planner/test/python_move_group_planning.py b/moveit_planners/pilz_industrial_motion_planner/test/python_move_group_planning.py new file mode 100755 index 0000000000..93adb0d585 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/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/python_move_group_planning.test b/moveit_planners/pilz_industrial_motion_planner/test/python_move_group_planning.test new file mode 100644 index 0000000000..c6b286de4c --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/python_move_group_planning.test @@ -0,0 +1,9 @@ + + + + + + + + From a0ee2020c4a40d03a48044d71753ed23853a665d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Leroy=20R=C3=BCgemer?= Date: Tue, 19 Oct 2021 11:10:15 +0200 Subject: [PATCH 014/229] Remove '-W*' options from cmake files (#2903) --- .../pilz_industrial_motion_planner/CMakeLists.txt | 3 --- .../src/trajectory_generator_ptp.cpp | 2 +- .../pilz_industrial_motion_planner/test/test_utils.cpp | 5 +++-- .../pilz_industrial_motion_planner_testutils/CMakeLists.txt | 4 ---- 4 files changed, 4 insertions(+), 10 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt index 744ecb71e6..4341fe5874 100644 --- a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt @@ -3,9 +3,6 @@ project(pilz_industrial_motion_planner) ## Add support for C++14, supported in ROS Melodic and newer add_definitions(-std=c++14) -add_definitions(-Wall) -add_definitions(-Wextra) -add_definitions(-Wno-unused-parameter) find_package(catkin REQUIRED COMPONENTS joint_limits_interface 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 85216bcdd6..639058ebd0 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 @@ -257,7 +257,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::JointTrajectory& joint_trajectory) { 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 7aaa2ffe56..56b08c410d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -219,7 +219,7 @@ bool testutils::checkCartesianLinearity(const moveit::core::RobotModelConstPtr& const trajectory_msgs::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; @@ -1102,7 +1102,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_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt index b1650ea49a..0c1dfb654e 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt @@ -3,10 +3,6 @@ project(pilz_industrial_motion_planner_testutils) ## Add support for C++11, supported in ROS Kinetic and newer add_definitions(-std=c++11) -add_definitions(-Wall) -add_definitions(-Wextra) -add_definitions(-Wno-unused-parameter) -add_definitions(-Werror) find_package(catkin REQUIRED COMPONENTS tf2_eigen From a28ae89649280b471a61f36ebd0135188300dc32 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 19 Oct 2021 16:20:15 +0200 Subject: [PATCH 015/229] moveit_build_options() Declare common build options like CMAKE_CXX_STANDARD, CMAKE_BUILD_TYPE, and compiler options (namely warning flags) once. Each package depending on moveit_core can use these via moveit_build_options(). --- .github/workflows/ci.yaml | 5 ++-- moveit_core/CMakeLists.txt | 29 ++++--------------- .../{CMakeModules => cmake}/FindBullet.cmake | 0 moveit_core/cmake/moveit.cmake | 22 ++++++++++++++ .../collision_distance_field/CMakeLists.txt | 5 ---- moveit_core/robot_state/CMakeLists.txt | 5 ---- moveit_kinematics/CMakeLists.txt | 11 +------ .../templates/CMakeLists.txt | 8 ++--- .../chomp/chomp_interface/CMakeLists.txt | 7 +---- .../chomp/chomp_motion_planner/CMakeLists.txt | 7 +---- .../chomp_optimizer_adapter/CMakeLists.txt | 9 ++---- moveit_planners/ompl/CMakeLists.txt | 14 ++------- moveit_planners/trajopt/CMakeLists.txt | 11 +------ .../CMakeLists.txt | 15 ++-------- .../CMakeLists.txt | 8 ++--- .../CMakeLists.txt | 15 ++-------- moveit_ros/benchmarks/CMakeLists.txt | 11 +------ moveit_ros/manipulation/CMakeLists.txt | 12 ++------ moveit_ros/move_group/CMakeLists.txt | 11 +------ moveit_ros/moveit_servo/CMakeLists.txt | 11 +------ .../occupancy_map_monitor/CMakeLists.txt | 11 +------ moveit_ros/perception/CMakeLists.txt | 11 +------ moveit_ros/planning/CMakeLists.txt | 11 +------ .../robot_model_loader/CMakeLists.txt | 4 --- moveit_ros/planning_interface/CMakeLists.txt | 11 +------ moveit_ros/robot_interaction/CMakeLists.txt | 11 +------ moveit_ros/visualization/CMakeLists.txt | 19 ++---------- moveit_ros/warehouse/CMakeLists.txt | 11 +------ moveit_setup_assistant/CMakeLists.txt | 13 ++------- 29 files changed, 63 insertions(+), 255 deletions(-) rename moveit_core/{CMakeModules => cmake}/FindBullet.cmake (100%) create mode 100644 moveit_core/cmake/moveit.cmake diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index caf9cc450a..6717f30b1f 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -27,7 +27,6 @@ jobs: - IMAGE: master-ci # ROS Melodic with all dependencies required for master CATKIN_LINT: true env: - CXXFLAGS: "-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-deprecated-copy" DOCKER_IMAGE: moveit/moveit:${{ matrix.env.IMAGE }} UPSTREAM_WORKSPACE: .github/workflows/upstream.rosinstall TARGET_WORKSPACE: $TARGET_REPO_PATH github:ros-planning/moveit_resources#master @@ -38,8 +37,8 @@ jobs: AFTER_RUN_TARGET_TEST: ${{ matrix.env.CCOV && './.ci.prepare_codecov' || '' }} TARGET_CMAKE_ARGS: > -DCMAKE_BUILD_TYPE=${{ matrix.env.CCOV && 'RelWithDebInfo' || 'Release'}} - -DCMAKE_CXX_FLAGS="-Werror $CXXFLAGS ${{ matrix.env.CCOV && '--coverage'}}" - DOWNSTREAM_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-Werror $CXXFLAGS" + -DCMAKE_CXX_FLAGS="-Werror ${{ matrix.env.CCOV && '--coverage'}}" + DOWNSTREAM_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-Wall -Wextra" CCACHE_DIR: ${{ github.workspace }}/.ccache BASEDIR: ${{ github.workspace }}/.work CLANG_TIDY_BASE_REF: ${{ github.base_ref || github.ref }} diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index f555734a6d..fd2b944e4a 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -1,35 +1,14 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_core) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -if(NOT CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") - # Enable warnings - add_compile_options(-Wall -Wextra - -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wcast-qual - -Wno-unused-parameter -Wno-unused-function) -endif() -if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") - # This too often has false-positives - add_compile_options(-Wno-maybe-uninitialized) -elseif(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") -endif() - -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - message("${PROJECT_NAME}: You did not request a specific build type: Choosing 'Release' for maximum performance") - set(CMAKE_BUILD_TYPE Release) -endif() +include(cmake/moveit.cmake) +moveit_build_options() find_package(Boost REQUIRED system filesystem date_time thread iostreams regex ${EXTRA_BOOST_COMPONENTS}) find_package(Eigen3 REQUIRED) -list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules") - # TODO: Move collision detection into separate packages +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") find_package(Bullet 2.87) # TODO(j-petit): Version check can be dropped when Xenial reaches end-of-life @@ -187,6 +166,8 @@ catkin_package( urdfdom urdfdom_headers ${BULLET_ENABLE} + CFG_EXTRAS + moveit.cmake ) if (WIN32) diff --git a/moveit_core/CMakeModules/FindBullet.cmake b/moveit_core/cmake/FindBullet.cmake similarity index 100% rename from moveit_core/CMakeModules/FindBullet.cmake rename to moveit_core/cmake/FindBullet.cmake diff --git a/moveit_core/cmake/moveit.cmake b/moveit_core/cmake/moveit.cmake new file mode 100644 index 0000000000..831c218aef --- /dev/null +++ b/moveit_core/cmake/moveit.cmake @@ -0,0 +1,22 @@ +macro(moveit_build_options) + if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) + endif() + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) + + if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) + message("${PROJECT_NAME}: You did not request a specific build type: Choosing 'Release' for maximum performance") + set(CMAKE_BUILD_TYPE Release) + endif() + + option(MOVEIT_CI_WARNINGS "Enable all warnings used by CI" ON) + if(MOVEIT_CI_WARNINGS) + if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + add_compile_options(-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wcast-qual) + endif() + if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang") + add_compile_options(-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wcast-qual) + endif() + endif() +endmacro() diff --git a/moveit_core/collision_distance_field/CMakeLists.txt b/moveit_core/collision_distance_field/CMakeLists.txt index 2c7696896d..9a684f6b5e 100644 --- a/moveit_core/collision_distance_field/CMakeLists.txt +++ b/moveit_core/collision_distance_field/CMakeLists.txt @@ -1,10 +1,5 @@ set(MOVEIT_LIB_NAME moveit_collision_distance_field) -if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") - # clang is picky about new virtual functions hiding existing ones - add_compile_options(-Wno-overloaded-virtual) -endif() - add_library(${MOVEIT_LIB_NAME} src/collision_distance_field_types.cpp src/collision_common_distance_field.cpp diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt index aaef8c7c41..26d7f8d54f 100644 --- a/moveit_core/robot_state/CMakeLists.txt +++ b/moveit_core/robot_state/CMakeLists.txt @@ -1,10 +1,5 @@ set(MOVEIT_LIB_NAME moveit_robot_state) -if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") - # clang is picky about typeid(*solver) - add_compile_options(-Wno-potentially-evaluated-expression) -endif() - add_library(${MOVEIT_LIB_NAME} src/attached_body.cpp src/conversions.cpp diff --git a/moveit_kinematics/CMakeLists.txt b/moveit_kinematics/CMakeLists.txt index dfe9e7342d..05d74e67e8 100644 --- a/moveit_kinematics/CMakeLists.txt +++ b/moveit_kinematics/CMakeLists.txt @@ -1,16 +1,6 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_kinematics) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - find_package(Boost REQUIRED) find_package(Eigen3 REQUIRED) find_package(orocos_kdl REQUIRED) @@ -21,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS tf2 tf2_kdl ) +moveit_build_options() set(THIS_PACKAGE_INCLUDE_DIRS kdl_kinematics_plugin/include diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt b/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt index 3489b88797..31f210180f 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt @@ -1,12 +1,6 @@ cmake_minimum_required(VERSION 3.1.3) project(_PACKAGE_NAME_) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(catkin REQUIRED COMPONENTS moveit_core pluginlib @@ -14,6 +8,8 @@ find_package(catkin REQUIRED COMPONENTS tf2_kdl tf2_eigen ) +moveit_build_options() + find_package(LAPACK REQUIRED) include_directories(include) diff --git a/moveit_planners/chomp/chomp_interface/CMakeLists.txt b/moveit_planners/chomp/chomp_interface/CMakeLists.txt index bacee69953..63c741773b 100644 --- a/moveit_planners/chomp/chomp_interface/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_interface/CMakeLists.txt @@ -1,12 +1,6 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_planners_chomp) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - # find catkin in isolation so that CATKIN_ENABLE_TESTING is defined find_package(catkin REQUIRED) if (CATKIN_ENABLE_TESTING) @@ -22,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS chomp_motion_planner ${CHOMP_TEST_DEPS} ) +moveit_build_options() find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED) diff --git a/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt b/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt index ed698c3336..2d887603e1 100644 --- a/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt @@ -1,16 +1,11 @@ cmake_minimum_required(VERSION 3.1.3) project(chomp_motion_planner) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(catkin REQUIRED COMPONENTS roscpp moveit_core ) +moveit_build_options() catkin_package( INCLUDE_DIRS include diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt b/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt index d17f10a72a..7886b10ed2 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt @@ -1,17 +1,12 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_chomp_optimizer_adapter) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(catkin REQUIRED COMPONENTS moveit_core chomp_motion_planner pluginlib - ) +) +moveit_build_options() catkin_package() diff --git a/moveit_planners/ompl/CMakeLists.txt b/moveit_planners/ompl/CMakeLists.txt index e5c6345d1a..340fbf8bbb 100644 --- a/moveit_planners/ompl/CMakeLists.txt +++ b/moveit_planners/ompl/CMakeLists.txt @@ -1,17 +1,6 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_planners_ompl) -# At least C++11 required for OMPL -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - find_package(Boost REQUIRED system filesystem date_time thread serialization) find_package(catkin REQUIRED COMPONENTS moveit_core @@ -21,7 +10,8 @@ find_package(catkin REQUIRED COMPONENTS pluginlib tf2 dynamic_reconfigure - ) +) +moveit_build_options() find_package(ompl REQUIRED) diff --git a/moveit_planners/trajopt/CMakeLists.txt b/moveit_planners/trajopt/CMakeLists.txt index 4d7d8f54e1..536bf4e85a 100644 --- a/moveit_planners/trajopt/CMakeLists.txt +++ b/moveit_planners/trajopt/CMakeLists.txt @@ -1,16 +1,6 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_planners_trajopt) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - find_package(catkin REQUIRED COMPONENTS moveit_core @@ -22,6 +12,7 @@ find_package(catkin REQUIRED rosparam_shortcuts trajopt ) +moveit_build_options() catkin_package( INCLUDE_DIRS include diff --git a/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt b/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt index 14ac5ea714..095a0fa5a6 100644 --- a/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt +++ b/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt @@ -1,23 +1,14 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_fake_controller_manager) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - find_package(Boost REQUIRED thread) -find_package(catkin COMPONENTS +find_package(catkin REQUIRED COMPONENTS moveit_core moveit_ros_planning pluginlib roscpp - REQUIRED) +) +moveit_build_options() include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) diff --git a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt index 8408cc163a..ca3c2657c6 100644 --- a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt +++ b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt @@ -1,12 +1,6 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_control_interface) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(catkin REQUIRED COMPONENTS actionlib controller_manager_msgs @@ -15,6 +9,8 @@ find_package(catkin REQUIRED COMPONENTS pluginlib trajectory_msgs ) +moveit_build_options() + ## System dependencies are found with CMake's conventions find_package(Boost REQUIRED COMPONENTS system thread) diff --git a/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt b/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt index b89705f3ac..0055715311 100644 --- a/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt +++ b/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt @@ -1,25 +1,16 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_simple_controller_manager) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - find_package(Boost REQUIRED thread) -find_package(catkin COMPONENTS +find_package(catkin REQUIRED COMPONENTS moveit_core pluginlib actionlib roscpp control_msgs - REQUIRED) +) +moveit_build_options() include_directories(include) include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) diff --git a/moveit_ros/benchmarks/CMakeLists.txt b/moveit_ros/benchmarks/CMakeLists.txt index f64a99da75..45e013373e 100644 --- a/moveit_ros/benchmarks/CMakeLists.txt +++ b/moveit_ros/benchmarks/CMakeLists.txt @@ -3,16 +3,6 @@ project(moveit_ros_benchmarks) set(MOVEIT_LIB_NAME moveit_ros_benchmarks) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - find_package(Boost REQUIRED filesystem) find_package(catkin REQUIRED COMPONENTS @@ -22,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp pluginlib ) +moveit_build_options() catkin_package( LIBRARIES ${MOVEIT_LIB_NAME} diff --git a/moveit_ros/manipulation/CMakeLists.txt b/moveit_ros/manipulation/CMakeLists.txt index 5a6d452a53..4af616d463 100644 --- a/moveit_ros/manipulation/CMakeLists.txt +++ b/moveit_ros/manipulation/CMakeLists.txt @@ -1,16 +1,6 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_manipulation) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - find_package(catkin REQUIRED COMPONENTS moveit_core moveit_msgs @@ -23,6 +13,8 @@ find_package(catkin REQUIRED COMPONENTS pluginlib actionlib ) +moveit_build_options() + find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED thread system filesystem date_time program_options) diff --git a/moveit_ros/move_group/CMakeLists.txt b/moveit_ros/move_group/CMakeLists.txt index a8c2098417..6f7da8c58d 100644 --- a/moveit_ros/move_group/CMakeLists.txt +++ b/moveit_ros/move_group/CMakeLists.txt @@ -1,16 +1,6 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_move_group) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - find_package(Boost REQUIRED system filesystem date_time program_options thread) find_package(catkin REQUIRED COMPONENTS moveit_core @@ -23,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS tf2_geometry_msgs tf2_ros ) +moveit_build_options() catkin_package( LIBRARIES diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index 96791050e7..83d0a44b1a 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -1,18 +1,8 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_servo) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - set(SERVO_LIB_NAME moveit_servo_cpp_api) -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - find_package(Boost REQUIRED) find_package(Eigen3 REQUIRED) find_package(catkin REQUIRED COMPONENTS @@ -28,6 +18,7 @@ find_package(catkin REQUIRED COMPONENTS tf2_eigen trajectory_msgs ) +moveit_build_options() catkin_package( INCLUDE_DIRS diff --git a/moveit_ros/occupancy_map_monitor/CMakeLists.txt b/moveit_ros/occupancy_map_monitor/CMakeLists.txt index 8bb7d0ce61..e02d4fb7e1 100644 --- a/moveit_ros/occupancy_map_monitor/CMakeLists.txt +++ b/moveit_ros/occupancy_map_monitor/CMakeLists.txt @@ -2,16 +2,6 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_occupancy_map_monitor) set(MOVEIT_LIB_NAME ${PROJECT_NAME}) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - find_package(Boost REQUIRED thread) if(APPLE) @@ -25,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS pluginlib tf2_ros ) +moveit_build_options() find_package(Eigen3 REQUIRED) find_package(octomap REQUIRED) diff --git a/moveit_ros/perception/CMakeLists.txt b/moveit_ros/perception/CMakeLists.txt index 4a91111c30..e2e3dc26e0 100644 --- a/moveit_ros/perception/CMakeLists.txt +++ b/moveit_ros/perception/CMakeLists.txt @@ -1,18 +1,8 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_perception) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - option(WITH_OPENGL "Build the parts that depend on OpenGL" ON) -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - find_package(Boost REQUIRED thread) if(WITH_OPENGL) @@ -52,6 +42,7 @@ find_package(catkin REQUIRED COMPONENTS moveit_ros_planning nodelet ) +moveit_build_options() find_package(Eigen3 REQUIRED) find_package(OpenMP REQUIRED) diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt index 9783e1e1bb..2f73370cbd 100644 --- a/moveit_ros/planning/CMakeLists.txt +++ b/moveit_ros/planning/CMakeLists.txt @@ -1,16 +1,6 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_planning) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - find_package(Boost REQUIRED system filesystem date_time program_options thread chrono) find_package(catkin REQUIRED COMPONENTS moveit_core @@ -30,6 +20,7 @@ find_package(catkin REQUIRED COMPONENTS tf2_msgs tf2_ros ) +moveit_build_options() find_package(Eigen3 REQUIRED) diff --git a/moveit_ros/planning/robot_model_loader/CMakeLists.txt b/moveit_ros/planning/robot_model_loader/CMakeLists.txt index 9790d1bcc3..a9df47b5ea 100644 --- a/moveit_ros/planning/robot_model_loader/CMakeLists.txt +++ b/moveit_ros/planning/robot_model_loader/CMakeLists.txt @@ -1,9 +1,5 @@ set(MOVEIT_LIB_NAME moveit_robot_model_loader) -if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") - add_compile_options(-Wno-potentially-evaluated-expression) -endif() - add_library(${MOVEIT_LIB_NAME} src/robot_model_loader.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} moveit_rdf_loader moveit_kinematics_plugin_loader ${catkin_LIBRARIES}) diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index 9f19cc1441..73f1aa3ea4 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -1,16 +1,6 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_planning_interface) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - find_package(catkin REQUIRED COMPONENTS moveit_msgs moveit_ros_planning @@ -27,6 +17,7 @@ find_package(catkin REQUIRED COMPONENTS rospy rosconsole ) +moveit_build_options() find_package(PythonInterp REQUIRED) find_package(PythonLibs "${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}" REQUIRED) diff --git a/moveit_ros/robot_interaction/CMakeLists.txt b/moveit_ros/robot_interaction/CMakeLists.txt index 498b8e1c5d..8ec6a589a5 100644 --- a/moveit_ros/robot_interaction/CMakeLists.txt +++ b/moveit_ros/robot_interaction/CMakeLists.txt @@ -1,16 +1,6 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_robot_interaction) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - find_package(Boost REQUIRED thread) find_package(catkin REQUIRED COMPONENTS moveit_ros_planning @@ -21,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS tf2_ros roscpp ) +moveit_build_options() set(MOVEIT_LIB_NAME moveit_robot_interaction) diff --git a/moveit_ros/visualization/CMakeLists.txt b/moveit_ros/visualization/CMakeLists.txt index 395b35c3e6..bba6034ea8 100644 --- a/moveit_ros/visualization/CMakeLists.txt +++ b/moveit_ros/visualization/CMakeLists.txt @@ -1,24 +1,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_visualization) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") - # suppress warning in Ogre - add_compile_options(-Wno-deprecated-register) -endif() - # definition needed for boost/math/constants/constants.hpp included by Ogre to compile add_definitions(-DBOOST_MATH_DISABLE_FLOAT128) -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - find_package(Boost REQUIRED thread date_time system filesystem) find_package(catkin REQUIRED COMPONENTS class_loader @@ -39,8 +24,10 @@ find_package(catkin REQUIRED COMPONENTS rosconsole object_recognition_msgs ) +moveit_build_options() + # TODO: Remove when Kinetic support is dropped -if(rviz_VERSION VERSION_LESS 1.13.1) # Does rviz supports TF2? +if(rviz_VERSION VERSION_LESS 1.13.1) # Does rviz support TF2? add_definitions(-DRVIZ_TF1) endif() diff --git a/moveit_ros/warehouse/CMakeLists.txt b/moveit_ros/warehouse/CMakeLists.txt index d974831537..801fa71ed1 100644 --- a/moveit_ros/warehouse/CMakeLists.txt +++ b/moveit_ros/warehouse/CMakeLists.txt @@ -1,16 +1,6 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_warehouse) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - find_package(Boost REQUIRED thread system filesystem regex date_time program_options) find_package(catkin REQUIRED COMPONENTS @@ -21,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS tf2_eigen tf2_ros ) +moveit_build_options() catkin_package( LIBRARIES diff --git a/moveit_setup_assistant/CMakeLists.txt b/moveit_setup_assistant/CMakeLists.txt index c39d7cfe23..58909804de 100644 --- a/moveit_setup_assistant/CMakeLists.txt +++ b/moveit_setup_assistant/CMakeLists.txt @@ -1,17 +1,6 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_setup_assistant) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") - # suppress warning in Ogre - add_compile_options(-Wno-deprecated-register) -endif() - # definition needed for boost/math/constants/constants.hpp included by Ogre to compile add_definitions(-DBOOST_MATH_DISABLE_FLOAT128) @@ -30,6 +19,8 @@ find_package(catkin REQUIRED COMPONENTS urdf srdfdom ) +moveit_build_options() + find_package(ompl REQUIRED) include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${OMPL_INCLUDE_DIRS}) include_directories(include) From ad07d3e0e42386abd6831083bed08d3e48525e53 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 20 Oct 2021 22:49:41 +0200 Subject: [PATCH 016/229] Uppercase step names --- .github/workflows/ci.yaml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index caf9cc450a..1b315301d5 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -66,7 +66,7 @@ jobs: sudo rm -rf /usr/local df -h - uses: actions/checkout@v2 - - name: cache upstream workspace + - name: Cache upstream workspace uses: pat-s/always-upload-cache@v2.1.5 with: path: ${{ env.BASEDIR }}/upstream_ws @@ -74,7 +74,7 @@ jobs: restore-keys: ${{ env.CACHE_PREFIX }} env: CACHE_PREFIX: upstream_ws-${{ matrix.env.IMAGE }}-${{ hashFiles('.github/workflows/upstream.rosinstall', '.github/workflows/ci.yaml') }} - - name: cache downstream workspace + - name: Cache downstream workspace uses: pat-s/always-upload-cache@v2.1.5 with: path: ${{ env.BASEDIR }}/downstream_ws @@ -84,7 +84,7 @@ jobs: CACHE_PREFIX: downstream_ws-${{ matrix.env.IMAGE }}-${{ hashFiles('.github/workflows/downstream.rosinstall', '.github/workflows/ci.yaml') }} # The target directory cache doesn't include the source directory because # that comes from the checkout. See "prepare target_ws for cache" task below - - name: cache target workspace + - name: Cache target workspace if: ${{ ! matrix.env.CCOV }} uses: pat-s/always-upload-cache@v2.1.5 with: @@ -93,7 +93,7 @@ jobs: restore-keys: ${{ env.CACHE_PREFIX }} env: CACHE_PREFIX: target_ws${{ matrix.env.CCOV && '-ccov' || '' }}-${{ matrix.env.IMAGE }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml', '.github/workflows/ci.yaml') }} - - name: cache ccache + - name: Cache ccache uses: pat-s/always-upload-cache@v2.1.5 with: path: ${{ env.CCACHE_DIR }} @@ -104,7 +104,7 @@ jobs: env: CACHE_PREFIX: ccache-${{ matrix.env.IMAGE }}${{ matrix.env.CCOV && '-ccov' || '' }} - - name: generate ikfast packages + - name: Generate ikfast packages if: ${{ matrix.env.IKFAST_TEST }} run: | moveit_kinematics/test/test_ikfast_plugins.sh @@ -112,18 +112,18 @@ jobs: uses: ros-industrial/industrial_ci@master env: ${{ matrix.env }} - - name: upload test artifacts (on failure) + - name: Upload test artifacts (on failure) uses: actions/upload-artifact@v2 if: steps.industrial_ci.outcome != 'success' with: name: test-results-${{ matrix.env.IMAGE }} path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml - - name: upload codecov report + - name: Upload codecov report uses: codecov/codecov-action@v1 if: ${{ matrix.env.CCOV }} with: files: ${{ env.BASEDIR }}/coverage.info - - name: prepare target_ws for cache + - name: Prepare target_ws for cache if: ${{ always() && ! matrix.env.CCOV }} run: | du -sh ${{ env.BASEDIR }}/target_ws From 3983f5622462bba16621475d3e4cdcda33a2528f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 20 Oct 2021 14:12:10 +0200 Subject: [PATCH 017/229] clang-tidy: auto-fix unused-parameters --- .clang-tidy | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.clang-tidy b/.clang-tidy index 8b8aaae3bb..f356d15242 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -9,6 +9,7 @@ Checks: '-*, modernize-loop-convert, modernize-make-shared, modernize-make-unique, + misc-unused-parameters, readability-named-parameter, readability-redundant-smartptr-get, readability-redundant-string-cstr, @@ -23,6 +24,8 @@ CheckOptions: value: '10' - key: llvm-namespace-comment.SpacesBeforeComments value: '2' + - key: misc-unused-parameters.StrictMode + value: '1' - key: readability-braces-around-statements.ShortStatementLines value: '2' # type names From 716ea7db91735738702a4ace6d2561e689db8d9b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 21 Oct 2021 01:13:44 +0200 Subject: [PATCH 018/229] clang-tidy: fix unused parameter (uncritical cases) These parameters aren't used for an obvious reason. --- .../src/allvalid/collision_env_allvalid.cpp | 36 ++++++------- .../collision_detection/src/collision_env.cpp | 2 +- .../bullet_integration/bullet_utils.h | 4 +- .../bullet_cast_bvh_manager.cpp | 2 +- .../src/bullet_integration/bullet_utils.cpp | 9 ++-- .../src/collision_env_bullet.cpp | 8 +-- .../src/collision_env_fcl.cpp | 16 +++--- .../src/collision_distance_field_types.cpp | 4 +- .../src/collision_env_distance_field.cpp | 14 ++--- .../union_constraint_sampler.h | 4 +- .../test/pr2_arm_kinematics_plugin.cpp | 51 ++++++++++--------- .../test/test_constraint_samplers.cpp | 4 +- .../kinematics_base/src/kinematics_base.cpp | 12 ++--- .../test/test_multi_threaded.cpp | 2 +- .../moveit/python/pybind_rosmsg_typecasters.h | 2 +- .../robot_model/src/fixed_joint_model.cpp | 24 +++++---- moveit_core/robot_model/src/joint_model.cpp | 2 +- .../robot_model/src/revolute_joint_model.cpp | 2 +- moveit_core/robot_model/src/robot_model.cpp | 2 +- moveit_core/robot_state/src/robot_state.cpp | 4 +- .../test/test_cartesian_interpolator.cpp | 2 +- .../test/test_time_parameterization.cpp | 4 +- moveit_core/version/version.cpp | 2 +- 23 files changed, 111 insertions(+), 101 deletions(-) diff --git a/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp b/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp index 5f6de7dfbb..224ef00735 100644 --- a/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp +++ b/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp @@ -62,7 +62,7 @@ CollisionEnvAllValid::CollisionEnvAllValid(const CollisionEnv& other, const Worl } void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state) const + const moveit::core::RobotState& /*state*/) const { res.collision = false; if (req.verbose) @@ -70,8 +70,8 @@ void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, Coll } void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state, - const AllowedCollisionMatrix& acm) const + const moveit::core::RobotState& /*state*/, + const AllowedCollisionMatrix& /*acm*/) const { res.collision = false; if (req.verbose) @@ -79,8 +79,8 @@ void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, Coll } void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state1, - const moveit::core::RobotState& state2) const + const moveit::core::RobotState& /*state1*/, + const moveit::core::RobotState& /*state2*/) const { res.collision = false; if (req.verbose) @@ -88,34 +88,34 @@ void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, Coll } void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state1, - const moveit::core::RobotState& state2, - const AllowedCollisionMatrix& acm) const + const moveit::core::RobotState& /*state1*/, + const moveit::core::RobotState& /*state2*/, + const AllowedCollisionMatrix& /*acm*/) const { res.collision = false; if (req.verbose) ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); } -void CollisionEnvAllValid::distanceRobot(const DistanceRequest& req, DistanceResult& res, - const moveit::core::RobotState& state) const +void CollisionEnvAllValid::distanceRobot(const DistanceRequest& /*req*/, DistanceResult& res, + const moveit::core::RobotState& /*state*/) const { res.collision = false; } -double CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& state) const +double CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& /*state*/) const { return 0.0; } -double CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& state, - const AllowedCollisionMatrix& acm) const +double CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& /*state*/, + const AllowedCollisionMatrix& /*acm*/) const { return 0.0; } void CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state) const + const moveit::core::RobotState& /*state*/) const { res.collision = false; if (req.verbose) @@ -123,16 +123,16 @@ void CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, Colli } void CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state, - const AllowedCollisionMatrix& acm) const + const moveit::core::RobotState& /*state*/, + const AllowedCollisionMatrix& /*acm*/) const { res.collision = false; if (req.verbose) ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); } -void CollisionEnvAllValid::distanceSelf(const DistanceRequest& req, DistanceResult& res, - const moveit::core::RobotState& state) const +void CollisionEnvAllValid::distanceSelf(const DistanceRequest& /*req*/, DistanceResult& res, + const moveit::core::RobotState& /*state*/) const { res.collision = false; } diff --git a/moveit_core/collision_detection/src/collision_env.cpp b/moveit_core/collision_detection/src/collision_env.cpp index 16b195e625..d9f1f45cf8 100644 --- a/moveit_core/collision_detection/src/collision_env.cpp +++ b/moveit_core/collision_detection/src/collision_env.cpp @@ -275,7 +275,7 @@ void CollisionEnv::getScale(std::vector& scale) const } } -void CollisionEnv::updatedPaddingOrScaling(const std::vector& links) +void CollisionEnv::updatedPaddingOrScaling(const std::vector& /*links*/) { } diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h index b4f82f00a5..f0afe785ff 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h @@ -601,8 +601,8 @@ struct BroadphaseContactResultCallback } /** \brief This callback is used after btManifoldResult processed a collision result. */ - btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, - const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) + btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int /*partId0*/, + int index0, const btCollisionObjectWrapper* colObj1Wrap, int /*partId1*/, int index1) { if (cp.m_distance1 > static_cast(contact_distance_)) { diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp index 64e9df962c..5deb590591 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp @@ -131,7 +131,7 @@ void BulletCastBVHManager::setCastCollisionObjectsTransform(const std::string& n void BulletCastBVHManager::contactTest(collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req, - const collision_detection::AllowedCollisionMatrix* acm, bool self = false) + const collision_detection::AllowedCollisionMatrix* acm, bool /*self*/ = false) { ContactTestData cdata(active_, contact_distance_, collisions, req); broadphase_->calculateOverlappingPairs(dispatcher_.get()); diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp index 2f2b771964..12fad66cb9 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp @@ -44,7 +44,7 @@ namespace collision_detection_bullet { -btCollisionShape* createShapePrimitive(const shapes::Box* geom, const CollisionObjectType& collision_object_type) +btCollisionShape* createShapePrimitive(const shapes::Box* geom, const CollisionObjectType& /*collision_object_type*/) { assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE); const double* size = geom->size; @@ -55,13 +55,14 @@ btCollisionShape* createShapePrimitive(const shapes::Box* geom, const CollisionO return (new btBoxShape(btVector3(a, b, c))); } -btCollisionShape* createShapePrimitive(const shapes::Sphere* geom, const CollisionObjectType& collision_object_type) +btCollisionShape* createShapePrimitive(const shapes::Sphere* geom, const CollisionObjectType& /*collision_object_type*/) { assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE); return (new btSphereShape(static_cast(geom->radius))); } -btCollisionShape* createShapePrimitive(const shapes::Cylinder* geom, const CollisionObjectType& collision_object_type) +btCollisionShape* createShapePrimitive(const shapes::Cylinder* geom, + const CollisionObjectType& /*collision_object_type*/) { assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE); btScalar r = static_cast(geom->radius); @@ -69,7 +70,7 @@ btCollisionShape* createShapePrimitive(const shapes::Cylinder* geom, const Colli return (new btCylinderShapeZ(btVector3(r, r, l))); } -btCollisionShape* createShapePrimitive(const shapes::Cone* geom, const CollisionObjectType& collision_object_type) +btCollisionShape* createShapePrimitive(const shapes::Cone* geom, const CollisionObjectType& /*collision_object_type*/) { assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE); btScalar r = static_cast(geom->radius); diff --git a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp index bba49f6b41..3885f721dc 100644 --- a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp +++ b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp @@ -235,14 +235,14 @@ void CollisionEnvBullet::checkRobotCollisionHelperCCD(const CollisionRequest& re } } -void CollisionEnvBullet::distanceSelf(const DistanceRequest& req, DistanceResult& res, - const moveit::core::RobotState& state) const +void CollisionEnvBullet::distanceSelf(const DistanceRequest& /*req*/, DistanceResult& /*res*/, + const moveit::core::RobotState& /*state*/) const { ROS_INFO_NAMED(LOGNAME, "distanceSelf is not implemented for Bullet."); } -void CollisionEnvBullet::distanceRobot(const DistanceRequest& req, DistanceResult& res, - const moveit::core::RobotState& state) const +void CollisionEnvBullet::distanceRobot(const DistanceRequest& /*req*/, DistanceResult& /*res*/, + const moveit::core::RobotState& /*state*/) const { ROS_INFO_NAMED(LOGNAME, "distanceRobot is not implemented for Bullet."); } diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp index 7820e56754..4895b27baf 100644 --- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp @@ -67,6 +67,8 @@ void checkFCLCapabilities(const DistanceRequest& req) "to at least 0.6.0.", FCL_MAJOR_VERSION, FCL_MINOR_VERSION, FCL_PATCH_VERSION); } +#else + (void)(req); // silent -Wunused-parameter #endif } } // namespace @@ -285,17 +287,17 @@ void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, Collision checkRobotCollisionHelper(req, res, state, &acm); } -void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state1, - const moveit::core::RobotState& state2) const +void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& /*req*/, CollisionResult& /*res*/, + const moveit::core::RobotState& /*state1*/, + const moveit::core::RobotState& /*state2*/) const { ROS_ERROR_NAMED(LOGNAME, "Continuous collision not implemented"); } -void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state1, - const moveit::core::RobotState& state2, - const AllowedCollisionMatrix& acm) const +void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& /*req*/, CollisionResult& /*res*/, + const moveit::core::RobotState& /*state1*/, + const moveit::core::RobotState& /*state2*/, + const AllowedCollisionMatrix& /*acm*/) const { ROS_ERROR_NAMED(LOGNAME, "Not implemented"); } diff --git a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp index f9be5ccc4a..2f80e15e38 100644 --- a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp +++ b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp @@ -435,7 +435,7 @@ void collision_detection::getCollisionSphereMarkers( } void collision_detection::getProximityGradientMarkers( - const std::string& frame_id, const std::string& ns, const ros::Duration& dur, + const std::string& frame_id, const std::string& ns, const ros::Duration& /*dur*/, const std::vector& posed_decompositions, const std::vector& posed_vector_decompositions, const std::vector& gradients, visualization_msgs::MarkerArray& arr) @@ -538,7 +538,7 @@ void collision_detection::getProximityGradientMarkers( } void collision_detection::getCollisionMarkers( - const std::string& frame_id, const std::string& ns, const ros::Duration& dur, + const std::string& frame_id, const std::string& ns, const ros::Duration& /*dur*/, const std::vector& posed_decompositions, const std::vector& posed_vector_decompositions, const std::vector& gradients, visualization_msgs::MarkerArray& arr) diff --git a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp index 2be752d1e8..d25cbb8978 100644 --- a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp @@ -1509,17 +1509,17 @@ void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, // checkRobotCollisionHelper(req, res, robot, state, &acm); } -void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state1, - const moveit::core::RobotState& state2, - const AllowedCollisionMatrix& acm) const +void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& /*req*/, CollisionResult& /*res*/, + const moveit::core::RobotState& /*state1*/, + const moveit::core::RobotState& /*state2*/, + const AllowedCollisionMatrix& /*acm*/) const { ROS_ERROR_NAMED("collision_detection.distance", "Continuous collision checking not implemented"); } -void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state1, - const moveit::core::RobotState& state2) const +void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& /*req*/, CollisionResult& /*res*/, + const moveit::core::RobotState& /*state1*/, + const moveit::core::RobotState& /*state2*/) const { ROS_ERROR_NAMED("collision_detection.distance", "Continuous collision checking not implemented"); } diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h index 9c46104e76..b6c620654c 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h @@ -116,7 +116,7 @@ class UnionConstraintSampler : public ConstraintSampler * * @return Always true */ - bool configure(const moveit_msgs::Constraints& constr) override + bool configure(const moveit_msgs::Constraints& /*constr*/) override { return true; } @@ -128,7 +128,7 @@ class UnionConstraintSampler : public ConstraintSampler * * @return Always true */ - virtual bool canService(const moveit_msgs::Constraints& constr) const + virtual bool canService(const moveit_msgs::Constraints& /*constr*/) const { return true; } diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp index 22d91f7e64..03d66e86ca 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -313,9 +313,11 @@ bool PR2ArmKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_mo return active_; } -bool PR2ArmKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, - std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options) const +bool PR2ArmKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& /*ik_pose*/, + const std::vector& /*ik_seed_state*/, + std::vector& /*solution*/, + moveit_msgs::MoveItErrorCodes& /*error_code*/, + const kinematics::KinematicsQueryOptions& /*options*/) const { return false; } @@ -323,7 +325,7 @@ bool PR2ArmKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, c bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options) const + const kinematics::KinematicsQueryOptions& /*options*/) const { if (!active_) { @@ -368,37 +370,40 @@ bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose } } -bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, - const std::vector& ik_seed_state, double timeout, - const std::vector& consistency_limit, - std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options) const +bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& /*ik_pose*/, + const std::vector& /*ik_seed_state*/, double /*timeout*/, + const std::vector& /*consistency_limit*/, + std::vector& /*solution*/, + moveit_msgs::MoveItErrorCodes& /*error_code*/, + const kinematics::KinematicsQueryOptions& /*options*/) const { return false; } -bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, - const std::vector& ik_seed_state, double timeout, - std::vector& solution, const IKCallbackFn& solution_callback, - moveit_msgs::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options) const +bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& /*ik_pose*/, + const std::vector& /*ik_seed_state*/, double /*timeout*/, + std::vector& /*solution*/, + const IKCallbackFn& /*solution_callback*/, + moveit_msgs::MoveItErrorCodes& /*error_code*/, + const kinematics::KinematicsQueryOptions& /*options*/) const { return false; } -bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, - const std::vector& ik_seed_state, double timeout, - const std::vector& consistency_limit, - std::vector& solution, const IKCallbackFn& solution_callback, - moveit_msgs::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options) const +bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& /*ik_pose*/, + const std::vector& /*ik_seed_state*/, double /*timeout*/, + const std::vector& /*consistency_limit*/, + std::vector& /*solution*/, + const IKCallbackFn& /*solution_callback*/, + moveit_msgs::MoveItErrorCodes& /*error_code*/, + const kinematics::KinematicsQueryOptions& /*options*/) const { return false; } -bool PR2ArmKinematicsPlugin::getPositionFK(const std::vector& link_names, - const std::vector& joint_angles, - std::vector& poses) const +bool PR2ArmKinematicsPlugin::getPositionFK(const std::vector& /*link_names*/, + const std::vector& /*joint_angles*/, + std::vector& /*poses*/) const { return false; } diff --git a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp index f43d021850..3cfd3a957f 100644 --- a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp @@ -56,14 +56,14 @@ class LoadPlanningModelsPr2 : public testing::Test { protected: - kinematics::KinematicsBasePtr getKinematicsSolverRightArm(const moveit::core::JointModelGroup* jmg) + kinematics::KinematicsBasePtr getKinematicsSolverRightArm(const moveit::core::JointModelGroup* /*jmg*/) { { return pr2_kinematics_plugin_right_arm_; } } - kinematics::KinematicsBasePtr getKinematicsSolverLeftArm(const moveit::core::JointModelGroup* jmg) + kinematics::KinematicsBasePtr getKinematicsSolverLeftArm(const moveit::core::JointModelGroup* /*jmg*/) { { return pr2_kinematics_plugin_left_arm_; diff --git a/moveit_core/kinematics_base/src/kinematics_base.cpp b/moveit_core/kinematics_base/src/kinematics_base.cpp index fe26075128..e7cfcf7821 100644 --- a/moveit_core/kinematics_base/src/kinematics_base.cpp +++ b/moveit_core/kinematics_base/src/kinematics_base.cpp @@ -95,9 +95,9 @@ void KinematicsBase::setValues(const std::string& robot_description, const std:: setValues(robot_description, group_name, base_frame, std::vector({ tip_frame }), search_discretization); } -bool KinematicsBase::initialize(const std::string& robot_description, const std::string& group_name, - const std::string& base_frame, const std::string& tip_frame, - double search_discretization) +bool KinematicsBase::initialize(const std::string& /*robot_description*/, const std::string& /*group_name*/, + const std::string& /*base_frame*/, const std::string& /*tip_frame*/, + double /*search_discretization*/) { return false; // default implementation returns false } @@ -116,9 +116,9 @@ bool KinematicsBase::initialize(const std::string& robot_description, const std: return false; } -bool KinematicsBase::initialize(const moveit::core::RobotModel& robot_model, const std::string& group_name, - const std::string& base_frame, const std::vector& tip_frames, - double search_discretization) +bool KinematicsBase::initialize(const moveit::core::RobotModel& /*robot_model*/, const std::string& group_name, + const std::string& /*base_frame*/, const std::vector& /*tip_frames*/, + double /*search_discretization*/) { ROS_WARN_NAMED(LOGNAME, "IK plugin for group '%s' relies on deprecated API. " diff --git a/moveit_core/planning_scene/test/test_multi_threaded.cpp b/moveit_core/planning_scene/test/test_multi_threaded.cpp index acc9f1ce2f..6a95fab9f0 100644 --- a/moveit_core/planning_scene/test/test_multi_threaded.cpp +++ b/moveit_core/planning_scene/test/test_multi_threaded.cpp @@ -47,7 +47,7 @@ const int TRIALS = 1000; const int THREADS = 2; -bool runCollisionDetection(unsigned int id, unsigned int trials, const planning_scene::PlanningScene* scene, +bool runCollisionDetection(unsigned int /*id*/, unsigned int trials, const planning_scene::PlanningScene* scene, const moveit::core::RobotState* state) { collision_detection::CollisionRequest req; diff --git a/moveit_core/python/tools/include/moveit/python/pybind_rosmsg_typecasters.h b/moveit_core/python/tools/include/moveit/python/pybind_rosmsg_typecasters.h index e613144421..9b71586b8e 100644 --- a/moveit_core/python/tools/include/moveit/python/pybind_rosmsg_typecasters.h +++ b/moveit_core/python/tools/include/moveit/python/pybind_rosmsg_typecasters.h @@ -114,7 +114,7 @@ struct type_caster::value>> } // Python -> C++ - bool load(handle src, bool convert) + bool load(handle src, bool /*convert*/) { if (!moveit::python::convertible(src, ros::message_traits::DataType::value())) return false; diff --git a/moveit_core/robot_model/src/fixed_joint_model.cpp b/moveit_core/robot_model/src/fixed_joint_model.cpp index 270f96234b..7204724539 100644 --- a/moveit_core/robot_model/src/fixed_joint_model.cpp +++ b/moveit_core/robot_model/src/fixed_joint_model.cpp @@ -50,42 +50,44 @@ unsigned int FixedJointModel::getStateSpaceDimension() const return 0; } -void FixedJointModel::getVariableDefaultPositions(double* values, const Bounds& bounds) const +void FixedJointModel::getVariableDefaultPositions(double* /*values*/, const Bounds& /*bounds*/) const { } -void FixedJointModel::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, - const Bounds& bounds) const +void FixedJointModel::getVariableRandomPositions(random_numbers::RandomNumberGenerator& /*rng*/, double* /*values*/, + const Bounds& /*bounds*/) const { } -void FixedJointModel::getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, - const Bounds& bounds, const double* seed, - const double distance) const +void FixedJointModel::getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& /*rng*/, + double* /*values*/, const Bounds& /*bounds*/, + const double* /*seed*/, const double /*distance*/) const { } -bool FixedJointModel::enforcePositionBounds(double* values, const Bounds& bounds) const +bool FixedJointModel::enforcePositionBounds(double* /*values*/, const Bounds& /*bounds*/) const { return false; } -bool FixedJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds, double margin) const +bool FixedJointModel::satisfiesPositionBounds(const double* /*values*/, const Bounds& /*bounds*/, + double /*margin*/) const { return true; } -double FixedJointModel::distance(const double* values1, const double* values2) const +double FixedJointModel::distance(const double* /*values1*/, const double* /*values2*/) const { return 0.0; } -double FixedJointModel::getMaximumExtent(const Bounds& other_bounds) const +double FixedJointModel::getMaximumExtent(const Bounds& /*other_bounds*/) const { return 0.0; } -void FixedJointModel::interpolate(const double* from, const double* to, const double t, double* state) const +void FixedJointModel::interpolate(const double* /*from*/, const double* /*to*/, const double /*t*/, + double* /*state*/) const { } diff --git a/moveit_core/robot_model/src/joint_model.cpp b/moveit_core/robot_model/src/joint_model.cpp index 323a9e53af..79d9ee91f2 100644 --- a/moveit_core/robot_model/src/joint_model.cpp +++ b/moveit_core/robot_model/src/joint_model.cpp @@ -90,7 +90,7 @@ int JointModel::getLocalVariableIndex(const std::string& variable) const return it->second; } -bool JointModel::harmonizePosition(double* values, const Bounds& other_bounds) const +bool JointModel::harmonizePosition(double* /*values*/, const Bounds& /*other_bounds*/) const { return false; } diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp index 5fe8a76636..222a2e30ed 100644 --- a/moveit_core/robot_model/src/revolute_joint_model.cpp +++ b/moveit_core/robot_model/src/revolute_joint_model.cpp @@ -88,7 +88,7 @@ void RevoluteJointModel::setContinuous(bool flag) computeVariableBoundsMsg(); } -double RevoluteJointModel::getMaximumExtent(const Bounds& other_bounds) const +double RevoluteJointModel::getMaximumExtent(const Bounds& /*other_bounds*/) const { return variable_bounds_[0].max_position_ - variable_bounds_[0].min_position_; } diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 0a10bec21b..bf28eb344b 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -552,7 +552,7 @@ void RobotModel::buildGroups(const srdf::Model& srdf_model) buildGroupsInfoEndEffectors(srdf_model); } -void RobotModel::buildGroupsInfoSubgroups(const srdf::Model& srdf_model) +void RobotModel::buildGroupsInfoSubgroups(const srdf::Model& /*srdf_model*/) { // compute subgroups for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index f3de5936bb..ea38aab829 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1786,7 +1786,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: const std::vector& tips_in, const std::vector >& consistency_limits, double timeout, const GroupStateValidityCallbackFn& constraint, - const kinematics::KinematicsQueryOptions& options) + const kinematics::KinematicsQueryOptions& /*options*/) { // Assume we have already ran setFromIK() and those checks @@ -2203,7 +2203,7 @@ void RobotState::printTransforms(std::ostream& out) const } } -std::string RobotState::getStateTreeString(const std::string& prefix) const +std::string RobotState::getStateTreeString(const std::string& /*prefix*/) const { std::stringstream ss; ss << "ROBOT: " << robot_model_->getName() << std::endl; diff --git a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp index 01753e99fa..0cc6ab6d7a 100644 --- a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp +++ b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp @@ -244,7 +244,7 @@ class OneRobot : public testing::Test std::size_t generateTestTraj(std::vector>& traj, const moveit::core::RobotModelConstPtr& robot_model_, - const moveit::core::JointModelGroup* joint_model_group) + const moveit::core::JointModelGroup* /*joint_model_group*/) { traj.clear(); diff --git a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp index d3cba00ecb..cb318306ce 100644 --- a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp +++ b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp @@ -76,8 +76,8 @@ int initRepeatedPointTrajectory(robot_trajectory::RobotTrajectory& trajectory) // Initialize one-joint, straight-line trajectory // Can specify init/final velocity/acceleration, // but not all time parameterization methods may accept it. -int initStraightTrajectory(robot_trajectory::RobotTrajectory& trajectory, double vel_i = 0.0, double vel_f = 0.0, - double acc_i = 0.0, double acc_f = 0.0) +int initStraightTrajectory(robot_trajectory::RobotTrajectory& trajectory, double /*vel_i*/ = 0.0, + double /*vel_f*/ = 0.0, double /*acc_i*/ = 0.0, double /*acc_f*/ = 0.0) { const int num = 10; const double max = 2.0; diff --git a/moveit_core/version/version.cpp b/moveit_core/version/version.cpp index 0bbb8638dc..75ce900180 100644 --- a/moveit_core/version/version.cpp +++ b/moveit_core/version/version.cpp @@ -37,7 +37,7 @@ #include #include -int main(int argc, char** argv) +int main(int /*argc*/, char** /*argv*/) { printf("%s\n", MOVEIT_VERSION); return 0; From ab64d84d82da70f5f491c724aacd628b123347b7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 21 Oct 2021 01:15:43 +0200 Subject: [PATCH 019/229] clang-tidy: fix unused parameter (critical cases) This warnings should be considered in more detail (TODO). Not using these arguments might be an actual bug. --- .../collision_detection_fcl/src/collision_common.cpp | 2 +- .../src/collision_env_distance_field.cpp | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp index 8268147356..94fcb6c281 100644 --- a/moveit_core/collision_detection_fcl/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp @@ -408,7 +408,7 @@ struct FCLShapeCache unsigned int clean_count_; }; -bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data, double& min_dist) +bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data, double& /*min_dist*/) { DistanceData* cdata = reinterpret_cast(data); diff --git a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp index d25cbb8978..dee29b010b 100644 --- a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp @@ -58,7 +58,7 @@ CollisionEnvDistanceField::CollisionEnvDistanceField( const moveit::core::RobotModelConstPtr& robot_model, const std::map>& link_body_decompositions, double size_x, double size_y, double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, - double collision_tolerance, double max_propogation_distance, double padding, double scale) + double collision_tolerance, double max_propogation_distance, double /*padding*/, double /*scale*/) : CollisionEnv(robot_model) { initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), origin, use_signed_distance_field, @@ -1085,7 +1085,8 @@ void CollisionEnvDistanceField::addLinkBodyDecompositions( } PosedBodySphereDecompositionPtr -CollisionEnvDistanceField::getPosedLinkBodySphereDecomposition(const moveit::core::LinkModel* ls, unsigned int ind) const +CollisionEnvDistanceField::getPosedLinkBodySphereDecomposition(const moveit::core::LinkModel* /*ls*/, + unsigned int ind) const { PosedBodySphereDecompositionPtr ret; ret = std::make_shared(link_body_decomposition_vector_.at(ind)); @@ -1524,7 +1525,7 @@ void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& /*re ROS_ERROR_NAMED("collision_detection.distance", "Continuous collision checking not implemented"); } -void CollisionEnvDistanceField::getCollisionGradients(const CollisionRequest& req, CollisionResult& res, +void CollisionEnvDistanceField::getCollisionGradients(const CollisionRequest& req, CollisionResult& /*res*/, const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const From f790e9d423a01755a6e9747db4d848358ad0b941 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 21 Oct 2021 01:16:05 +0200 Subject: [PATCH 020/229] Simplify API: Remove obviously unused arguments --- .../include/moveit/robot_model/robot_model.h | 2 +- moveit_core/robot_model/src/robot_model.cpp | 4 ++-- .../include/moveit/robot_state/robot_state.h | 2 +- moveit_core/robot_state/src/robot_state.cpp | 2 +- .../test/test_cartesian_interpolator.cpp | 21 +++++++++---------- .../test/test_time_parameterization.cpp | 5 +---- 6 files changed, 16 insertions(+), 20 deletions(-) diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h index 4187510804..2b339dc7ba 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h @@ -604,7 +604,7 @@ class RobotModel void buildGroups(const srdf::Model& srdf_model); /** \brief Compute helpful information about groups (that can be queried later) */ - void buildGroupsInfoSubgroups(const srdf::Model& srdf_model); + void buildGroupsInfoSubgroups(); /** \brief Compute helpful information about groups (that can be queried later) */ void buildGroupsInfoEndEffectors(const srdf::Model& srdf_model); diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index bf28eb344b..ab666581e1 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -548,11 +548,11 @@ void RobotModel::buildGroups(const srdf::Model& srdf_model) joint_model_group_names_.push_back(joint_model_group->getName()); } - buildGroupsInfoSubgroups(srdf_model); + buildGroupsInfoSubgroups(); buildGroupsInfoEndEffectors(srdf_model); } -void RobotModel::buildGroupsInfoSubgroups(const srdf::Model& /*srdf_model*/) +void RobotModel::buildGroupsInfoSubgroups() { // compute subgroups for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it) diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index 25870ebf6b..c53cc2d743 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -1826,7 +1826,7 @@ class RobotState void printDirtyInfo(std::ostream& out = std::cout) const; - std::string getStateTreeString(const std::string& prefix = "") const; + std::string getStateTreeString() const; /** * \brief Transform pose from the robot model's base frame to the reference frame of the IK solver diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index ea38aab829..662cf5e752 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -2203,7 +2203,7 @@ void RobotState::printTransforms(std::ostream& out) const } } -std::string RobotState::getStateTreeString(const std::string& /*prefix*/) const +std::string RobotState::getStateTreeString() const { std::stringstream ss; ss << "ROBOT: " << robot_model_->getName() << std::endl; diff --git a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp index 0cc6ab6d7a..abadc23314 100644 --- a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp +++ b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp @@ -243,8 +243,7 @@ class OneRobot : public testing::Test }; std::size_t generateTestTraj(std::vector>& traj, - const moveit::core::RobotModelConstPtr& robot_model_, - const moveit::core::JointModelGroup* /*joint_model_group*/) + const moveit::core::RobotModelConstPtr& robot_model_) { traj.clear(); @@ -293,7 +292,7 @@ TEST_F(OneRobot, testGenerateTrajectory) const std::size_t expected_full_traj_len = 7; // Generate a test trajectory - std::size_t full_traj_len = generateTestTraj(traj, robot_model_, joint_model_group); + std::size_t full_traj_len = generateTestTraj(traj, robot_model_); // Test the generateTestTraj still generates a trajectory of length 7 EXPECT_EQ(full_traj_len, expected_full_traj_len); // full traj should be 7 waypoints long @@ -309,7 +308,7 @@ TEST_F(OneRobot, checkAbsoluteJointSpaceJump) const std::size_t expected_prismatic_jump_traj_len = 5; // Pre-compute expected results for tests - std::size_t full_traj_len = generateTestTraj(traj, robot_model_, joint_model_group); + std::size_t full_traj_len = generateTestTraj(traj, robot_model_); const double expected_revolute_jump_fraction = (double)expected_revolute_jump_traj_len / (double)full_traj_len; const double expected_prismatic_jump_fraction = (double)expected_prismatic_jump_traj_len / (double)full_traj_len; @@ -322,28 +321,28 @@ TEST_F(OneRobot, checkAbsoluteJointSpaceJump) EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01); // Indirect call using checkJointSpaceJumps - generateTestTraj(traj, robot_model_, joint_model_group); + generateTestTraj(traj, robot_model_); fraction = moveit::core::CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, moveit::core::JumpThreshold(1.0, 1.0)); EXPECT_EQ(expected_revolute_jump_traj_len, traj.size()); // traj should be cut before the revolute jump EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01); // Test revolute joints - generateTestTraj(traj, robot_model_, joint_model_group); + generateTestTraj(traj, robot_model_); fraction = moveit::core::CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, moveit::core::JumpThreshold(1.0, 0.0)); EXPECT_EQ(expected_revolute_jump_traj_len, traj.size()); // traj should be cut before the revolute jump EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01); // Test prismatic joints - generateTestTraj(traj, robot_model_, joint_model_group); + generateTestTraj(traj, robot_model_); fraction = moveit::core::CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, moveit::core::JumpThreshold(0.0, 1.0)); EXPECT_EQ(expected_prismatic_jump_traj_len, traj.size()); // traj should be cut before the prismatic jump EXPECT_NEAR(expected_prismatic_jump_fraction, fraction, 0.01); // Ignore all absolute jumps - generateTestTraj(traj, robot_model_, joint_model_group); + generateTestTraj(traj, robot_model_); fraction = moveit::core::CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, moveit::core::JumpThreshold(0.0, 0.0)); EXPECT_EQ(full_traj_len, traj.size()); // traj should not be cut @@ -359,7 +358,7 @@ TEST_F(OneRobot, checkRelativeJointSpaceJump) const std::size_t expected_relative_jump_traj_len = 4; // Pre-compute expected results for tests - std::size_t full_traj_len = generateTestTraj(traj, robot_model_, joint_model_group); + std::size_t full_traj_len = generateTestTraj(traj, robot_model_); const double expected_relative_jump_fraction = (double)expected_relative_jump_traj_len / (double)full_traj_len; // Container for results @@ -371,14 +370,14 @@ TEST_F(OneRobot, checkRelativeJointSpaceJump) EXPECT_NEAR(expected_relative_jump_fraction, fraction, 0.01); // Indirect call of relative version using checkJointSpaceJumps - generateTestTraj(traj, robot_model_, joint_model_group); + generateTestTraj(traj, robot_model_); fraction = moveit::core::CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, moveit::core::JumpThreshold(2.97)); EXPECT_EQ(expected_relative_jump_traj_len, traj.size()); // traj should be cut before the first jump of 1.01 EXPECT_NEAR(expected_relative_jump_fraction, fraction, 0.01); // Trajectory should not be cut: 1.01 < 2.98 * (0.01 * 2 + 1.01 * 2)/6. - generateTestTraj(traj, robot_model_, joint_model_group); + generateTestTraj(traj, robot_model_); fraction = moveit::core::CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, moveit::core::JumpThreshold(2.98)); EXPECT_EQ(full_traj_len, traj.size()); // traj should not be cut diff --git a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp index cb318306ce..ba5bacf914 100644 --- a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp +++ b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp @@ -74,10 +74,7 @@ int initRepeatedPointTrajectory(robot_trajectory::RobotTrajectory& trajectory) } // Initialize one-joint, straight-line trajectory -// Can specify init/final velocity/acceleration, -// but not all time parameterization methods may accept it. -int initStraightTrajectory(robot_trajectory::RobotTrajectory& trajectory, double /*vel_i*/ = 0.0, - double /*vel_f*/ = 0.0, double /*acc_i*/ = 0.0, double /*acc_f*/ = 0.0) +int initStraightTrajectory(robot_trajectory::RobotTrajectory& trajectory) { const int num = 10; const double max = 2.0; From 157794cd69d0dba9f733187c52e3c34e3d1efc60 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 21 Oct 2021 07:33:10 +0200 Subject: [PATCH 021/229] Remove unused arguments from global_adjustment_factor() Looks like, dt and x were passed originally to call fit_cubic_spline() inside that function. However, later it was assumed that fit_cubic_spline() was already called, rendering these parameters superfluous. --- .../src/iterative_spline_parameterization.cpp | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp index 855c3a7929..a9b7c370b6 100644 --- a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp +++ b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp @@ -51,9 +51,9 @@ static int fit_spline_and_adjust_times(const int n, double dt[], const double x[ const double max_velocity, const double min_velocity, const double max_acceleration, const double min_acceleration, const double tfactor); -static double global_adjustment_factor(const int n, double dt[], const double x[], double x1[], double x2[], - const double max_velocity, const double min_velocity, - const double max_acceleration, const double min_acceleration); +static double global_adjustment_factor(const int n, double x1[], double x2[], const double max_velocity, + const double min_velocity, const double max_acceleration, + const double min_acceleration); // The path of a single joint: positions, velocities, and accelerations struct SingleJointTrajectory @@ -537,15 +537,13 @@ static int fit_spline_and_adjust_times(const int n, double dt[], const double x[ // to force within bounds. // Assumes that the spline is already fit // (fit_cubic_spline must have been called before this). -static double global_adjustment_factor(const int n, double dt[], const double x[], double x1[], double x2[], - const double max_velocity, const double min_velocity, - const double max_acceleration, const double min_acceleration) +static double global_adjustment_factor(const int n, double x1[], double x2[], const double max_velocity, + const double min_velocity, const double max_acceleration, + const double min_acceleration) { int i; double tfactor2 = 1.00; - // fit_cubic_spline(n, dt, x, x1, x2); - for (i = 0; i < n; i++) { double tfactor; @@ -581,9 +579,8 @@ void globalAdjustment(std::vector& t2, int num_joints, co for (int j = 0; j < num_joints; j++) { double tfactor; - tfactor = global_adjustment_factor(num_points, &time_diff[0], &t2[j].positions_[0], &t2[j].velocities_[0], - &t2[j].accelerations_[0], t2[j].max_velocity_, t2[j].min_velocity_, - t2[j].max_acceleration_, t2[j].min_acceleration_); + tfactor = global_adjustment_factor(num_points, &t2[j].velocities_[0], &t2[j].accelerations_[0], t2[j].max_velocity_, + t2[j].min_velocity_, t2[j].max_acceleration_, t2[j].min_acceleration_); if (tfactor > gtfactor) gtfactor = tfactor; } From 1987073ed89f50c953db61dcf7726fd7fc984de1 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 21 Oct 2021 01:35:28 +0200 Subject: [PATCH 022/229] Disable clang-tidy for ikfast plugins We cannot tidy the ikfast-generated code. Thus disable clang-tidy for those packages by removing their compile_commands.json. Future versions of clang-tidy (14) allow adapting the parent's folder config via InheritParentConfig. --- .github/workflows/ci.yaml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 6717f30b1f..76833e55b3 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -42,7 +42,11 @@ jobs: CCACHE_DIR: ${{ github.workspace }}/.ccache BASEDIR: ${{ github.workspace }}/.work CLANG_TIDY_BASE_REF: ${{ github.base_ref || github.ref }} - BEFORE_CLANG_TIDY_CHECKS: (cd $TARGET_REPO_PATH; clang-tidy --list-checks) + BEFORE_CLANG_TIDY_CHECKS: | + # Show list of applied checks + (cd $TARGET_REPO_PATH; clang-tidy --list-checks) + # Disable clang-tidy for ikfast plugins as we cannot fix the generated code + find $BASEDIR/target_ws/build -iwholename "*_ikfast_plugin/compile_commands.json" -exec rm {} \; BUILDER: ${{ matrix.env.BUILDER || 'catkin_tools' }} CC: ${{ matrix.env.CLANG_TIDY && 'clang' }} CXX: ${{ matrix.env.CLANG_TIDY && 'clang++' }} From b427510039196c46f8e4557d6e08d6e3dd5ffee7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 21 Oct 2021 06:38:35 +0200 Subject: [PATCH 023/229] Silent unused-function warnings --- moveit_core/robot_state/test/robot_state_test.cpp | 3 +++ .../src/iterative_spline_parameterization.cpp | 6 ++---- .../src/iterative_time_parameterization.cpp | 2 ++ .../src/time_optimal_trajectory_generation.cpp | 5 ----- 4 files changed, 7 insertions(+), 9 deletions(-) diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index 1be16e4386..6482ef324c 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -49,6 +49,7 @@ namespace constexpr double EPSILON{ 1.e-9 }; } +#if 0 // unused function static bool sameStringIgnoringWS(const std::string& s1, const std::string& s2) { unsigned int i1 = 0; @@ -73,6 +74,8 @@ static bool sameStringIgnoringWS(const std::string& s1, const std::string& s2) } return i1 == s1.size() && i2 == s2.size(); } +#endif + static void expect_near(const Eigen::MatrixXd& x, const Eigen::MatrixXd& y, double eps = std::numeric_limits::epsilon()) { diff --git a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp index a9b7c370b6..bd82bec2a9 100644 --- a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp +++ b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp @@ -47,10 +47,6 @@ static void fit_cubic_spline(const int n, const double dt[], const double x[], d static void adjust_two_positions(const int n, const double dt[], double x[], double x1[], double x2[], const double x2_i, const double x2_f); static void init_times(const int n, double dt[], const double x[], const double max_velocity, const double min_velocity); -static int fit_spline_and_adjust_times(const int n, double dt[], const double x[], double x1[], double x2[], - const double max_velocity, const double min_velocity, - const double max_acceleration, const double min_acceleration, - const double tfactor); static double global_adjustment_factor(const int n, double x1[], double x2[], const double max_velocity, const double min_velocity, const double max_acceleration, const double min_acceleration); @@ -475,6 +471,7 @@ static void init_times(const int n, double dt[], const double x[], const double } } +#if 0 // unused function /* Fit a spline, then check each interval to see if bounds are met. If all bounds met (no time adjustments made), return 0. @@ -532,6 +529,7 @@ static int fit_spline_and_adjust_times(const int n, double dt[], const double x[ return ret; } +#endif // return global expansion multiplicative factor required // to force within bounds. diff --git a/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp b/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp index afef388e49..d300e0b9a5 100644 --- a/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp +++ b/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp @@ -50,6 +50,7 @@ IterativeParabolicTimeParameterization::IterativeParabolicTimeParameterization(u { } +#if 0 // unused functions namespace { void printPoint(const trajectory_msgs::JointTrajectoryPoint& point, std::size_t i) @@ -94,6 +95,7 @@ void printStats(const trajectory_msgs::JointTrajectory& trajectory, const std::v printPoint(trajectory.points[i], i); } } // namespace +#endif // Applies velocity void IterativeParabolicTimeParameterization::applyVelocityConstraints(robot_trajectory::RobotTrajectory& rob_trajectory, 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 03cf56dd9a..ff1f0e8308 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -306,11 +306,6 @@ std::list> Path::getSwitchingPoints() const return switching_points_; } -static double squared(double d) -{ - return d * d; -} - Trajectory::Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, const Eigen::VectorXd& max_acceleration, double time_step) : path_(path) From 0e988cdd132807cf2c3577f7e11a0b2bb610714d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 21 Oct 2021 06:41:23 +0200 Subject: [PATCH 024/229] Fix unused-variable warning --- moveit_core/robot_state/test/test_cartesian_interpolator.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp index abadc23314..4dfd852d14 100644 --- a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp +++ b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp @@ -285,7 +285,6 @@ std::size_t generateTestTraj(std::vectorgetJointModelGroup("arm"); std::vector> traj; // The full trajectory should be of length 7 From cf5664e97318ee0081546105b571bb12a2dd7f73 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 21 Oct 2021 06:48:05 +0200 Subject: [PATCH 025/229] Silent clang-tidy's -Wpotentially-evaluated-expression https://stackoverflow.com/questions/46494928/clang-warning-on-expression-side-effects --- .../planning/robot_model_loader/src/robot_model_loader.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp index 846112b277..2ce6c2f652 100644 --- a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp +++ b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp @@ -207,8 +207,9 @@ void RobotModelLoader::loadKinematicsSolvers(const kinematics_plugin_loader::Kin } else { - ROS_ERROR("Kinematics solver %s does not support joint group %s. Error: %s", typeid(*solver).name(), - group.c_str(), error_msg.c_str()); + const auto& s = *solver; // avoid clang-tidy's -Wpotentially-evaluated-expression + ROS_ERROR("Kinematics solver %s does not support joint group %s. Error: %s", typeid(s).name(), group.c_str(), + error_msg.c_str()); } } else From b49d2739fbd9058b1eb7fe01371e11c4e42ae62f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 21 Oct 2021 08:38:54 +0200 Subject: [PATCH 026/229] Don't fail on -Wmaybe-uninitialized. Needs more analysis! --- moveit_core/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index fd2b944e4a..96f3b52f38 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -1,6 +1,10 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_core) +if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + # Don't fail on this warning, but don't silent it. TODO: Needs more detailed analysis. + add_compile_options(-Wno-error=maybe-uninitialized) +endif() include(cmake/moveit.cmake) moveit_build_options() From 9c946c2914dbd940fff0dc011237571a840464c3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 20 Oct 2021 22:52:16 +0200 Subject: [PATCH 027/229] More specific run conditions Relies on https://github.com/ros-industrial/industrial_ci/pull/755 --- .github/workflows/ci.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 1b315301d5..238da0c08e 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -108,23 +108,23 @@ jobs: if: ${{ matrix.env.IKFAST_TEST }} run: | moveit_kinematics/test/test_ikfast_plugins.sh - - id: industrial_ci + - id: ici uses: ros-industrial/industrial_ci@master env: ${{ matrix.env }} - name: Upload test artifacts (on failure) uses: actions/upload-artifact@v2 - if: steps.industrial_ci.outcome != 'success' + if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results) with: name: test-results-${{ matrix.env.IMAGE }} path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml - name: Upload codecov report uses: codecov/codecov-action@v1 - if: ${{ matrix.env.CCOV }} + if: always() && matrix.env.CCOV && steps.ici.outputs.target_test_results == '0' with: files: ${{ env.BASEDIR }}/coverage.info - name: Prepare target_ws for cache - if: ${{ always() && ! matrix.env.CCOV }} + if: always() && ! matrix.env.CCOV run: | du -sh ${{ env.BASEDIR }}/target_ws sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete From 40fb8e0f2feba7d8490bf7471f3b42919572f572 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 19 Oct 2021 18:34:56 +0200 Subject: [PATCH 028/229] Upload clang-tidy patch as artifact --- .github/workflows/ci.yaml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 238da0c08e..70acfb102c 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -28,6 +28,7 @@ jobs: CATKIN_LINT: true env: CXXFLAGS: "-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-deprecated-copy" + CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file DOCKER_IMAGE: moveit/moveit:${{ matrix.env.IMAGE }} UPSTREAM_WORKSPACE: .github/workflows/upstream.rosinstall TARGET_WORKSPACE: $TARGET_REPO_PATH github:ros-planning/moveit_resources#master @@ -123,6 +124,12 @@ jobs: if: always() && matrix.env.CCOV && steps.ici.outputs.target_test_results == '0' with: files: ${{ env.BASEDIR }}/coverage.info + - name: Upload clang-tidy changes + uses: rhaschke/upload-git-patch-action@main + if: always() && matrix.env.CLANG_TIDY + with: + name: clang-tidy + path: ${{ env.BASEDIR }}/target_ws/src/$(basename $(pwd)) - name: Prepare target_ws for cache if: always() && ! matrix.env.CCOV run: | From 84cbc251ce6cfbbb1ff937ef14c3a755b4db9576 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 19 Oct 2021 20:02:59 +0200 Subject: [PATCH 029/229] Upload pre-commit changes --- .github/workflows/format.yaml | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml index b3770ce961..544b4613e1 100644 --- a/.github/workflows/format.yaml +++ b/.github/workflows/format.yaml @@ -17,15 +17,11 @@ jobs: - uses: actions/setup-python@v2 - name: Install clang-format-10 run: sudo apt-get install clang-format-10 - - name: Install catkin-lint - run: | - lsb_release -sc - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 - sudo apt-get -q update - sudo apt-get -q install python3-rosdep - sudo rosdep init - rosdep update - sudo apt-get -q install catkin-lint - export ROS_DISTRO=noetic + - uses: rhaschke/install-catkin_lint-action@v1.0 - uses: pre-commit/action@v2.0.0 + id: precommit + - name: Upload pre-commit changes + if: failure() && steps.precommit.outcome == 'failure' + uses: rhaschke/upload-git-patch-action@main + with: + name: pre-commit From aa2ab714b6d6001105f5e9c4e41e7baeab60e90c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 20 Oct 2021 15:16:51 +0200 Subject: [PATCH 030/229] Remove -Werror for pedantic clang-tidy builds 1. -Werror should only be used for the target (but not downstream) workspace 2. For target workspace, a pedantic clang-tidy build replaces -Werror: It fails on any warnings as well but applies automatic fixes where possible. --- .github/workflows/ci.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 70acfb102c..a4ce90cba9 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -39,8 +39,8 @@ jobs: AFTER_RUN_TARGET_TEST: ${{ matrix.env.CCOV && './.ci.prepare_codecov' || '' }} TARGET_CMAKE_ARGS: > -DCMAKE_BUILD_TYPE=${{ matrix.env.CCOV && 'RelWithDebInfo' || 'Release'}} - -DCMAKE_CXX_FLAGS="-Werror $CXXFLAGS ${{ matrix.env.CCOV && '--coverage'}}" - DOWNSTREAM_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-Werror $CXXFLAGS" + -DCMAKE_CXX_FLAGS="${{ matrix.env.CLANG_TIDY != 'pedantic' && '-Werror ' || '' }}$CXXFLAGS${{ matrix.env.CCOV && ' --coverage'}}" + DOWNSTREAM_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-Wall -Wextra" CCACHE_DIR: ${{ github.workspace }}/.ccache BASEDIR: ${{ github.workspace }}/.work CLANG_TIDY_BASE_REF: ${{ github.base_ref || github.ref }} From 07ff21d84f5a91b396065b53d587724df8912ac3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 20 Oct 2021 19:02:01 +0200 Subject: [PATCH 031/229] clang-tidy: diffs only by default, full when manually triggered Trigger full clang-tidy run when workflow was manually triggered. By default, only commit diffs are checked. --- .github/workflows/ci.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index a4ce90cba9..90292a7f24 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -43,13 +43,13 @@ jobs: DOWNSTREAM_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-Wall -Wextra" CCACHE_DIR: ${{ github.workspace }}/.ccache BASEDIR: ${{ github.workspace }}/.work - CLANG_TIDY_BASE_REF: ${{ github.base_ref || github.ref }} + CLANG_TIDY_BASE_REF: ${{ github.event_name != 'workflow_dispatch' && (github.base_ref || github.ref) || '' }} BEFORE_CLANG_TIDY_CHECKS: (cd $TARGET_REPO_PATH; clang-tidy --list-checks) BUILDER: ${{ matrix.env.BUILDER || 'catkin_tools' }} CC: ${{ matrix.env.CLANG_TIDY && 'clang' }} CXX: ${{ matrix.env.CLANG_TIDY && 'clang++' }} - name: "${{ matrix.env.IMAGE }}${{ matrix.env.CATKIN_LINT && ' + catkin_lint' || ''}}${{ matrix.env.CCOV && ' + ccov' || ''}}${{ matrix.env.IKFAST_TEST && ' + ikfast' || ''}}${{ matrix.env.CLANG_TIDY && ' + clang-tidy' || '' }}" + name: "${{ matrix.env.IMAGE }}${{ matrix.env.CATKIN_LINT && ' + catkin_lint' || ''}}${{ matrix.env.CCOV && ' + ccov' || ''}}${{ matrix.env.IKFAST_TEST && ' + ikfast' || ''}}${{ matrix.env.CLANG_TIDY && (github.event_name != 'workflow_dispatch' && ' + clang-tidy (delta)' || ' + clang-tidy (all)') || '' }}" runs-on: ubuntu-latest steps: - name: "Free up disk space" From aa0f7bb4ccf17c1865219ecf9d2eb4f4957237ee Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 21 Oct 2021 08:48:21 +0200 Subject: [PATCH 032/229] Completely silent -Wmaybe-uninitialized --- moveit_core/CMakeLists.txt | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 96f3b52f38..0785a6b1c8 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -2,8 +2,9 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_core) if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") - # Don't fail on this warning, but don't silent it. TODO: Needs more detailed analysis. - add_compile_options(-Wno-error=maybe-uninitialized) + # Silent these warnings. They are too often false-positives. + message("TODO: Analyse and fix gcc's maybe-uninitialized warnings") + add_compile_options(-Wno-maybe-uninitialized) endif() include(cmake/moveit.cmake) moveit_build_options() From 98288b13ce8e84f5745bc93a31b63982537cb50a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 21 Oct 2021 12:11:12 +0200 Subject: [PATCH 033/229] Modernize: std::make_shared --- .../src/planning_scene_display.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) 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 bea40fafa0..44cc719a6f 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 @@ -170,8 +170,7 @@ PlanningSceneDisplay::~PlanningSceneDisplay() planning_scene_render_.reset(); if (context_ && context_->getSceneManager() && planning_scene_node_) context_->getSceneManager()->destroySceneNode(planning_scene_node_); - if (planning_scene_robot_) - planning_scene_robot_.reset(); + planning_scene_robot_.reset(); planning_scene_monitor_.reset(); } @@ -193,8 +192,8 @@ void PlanningSceneDisplay::onInitialize() if (robot_category_) { - planning_scene_robot_.reset( - new RobotStateVisualization(planning_scene_node_, context_, "Planning Scene", robot_category_)); + planning_scene_robot_ = + std::make_shared(planning_scene_node_, context_, "Planning Scene", robot_category_); planning_scene_robot_->setVisible(true); planning_scene_robot_->setVisualVisible(scene_robot_visual_enabled_property_->getBool()); planning_scene_robot_->setCollisionVisible(scene_robot_collision_enabled_property_->getBool()); From 27e5b32804b856999f33f6a3a2064ed62ffde46f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 21 Oct 2021 12:23:52 +0200 Subject: [PATCH 034/229] Fix ClassLoader: SEVERE WARNING Clear all references to RobotModel before destroying the corresponding RobotModelLoader. --- .../motion_planning_display.h | 1 + .../motion_planning_frame.h | 1 + .../motion_planning_frame_joints_widget.h | 1 + .../src/motion_planning_display.cpp | 15 +++++++++++++++ .../src/motion_planning_frame.cpp | 7 +++++++ .../src/motion_planning_frame_joints_widget.cpp | 12 ++++++++---- .../planning_scene_display.h | 2 +- .../src/planning_scene_display.cpp | 4 ++-- 8 files changed, 36 insertions(+), 7 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h index 0fd36a4088..22bea0bf54 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h @@ -196,6 +196,7 @@ private Q_SLOTS: OUTSIDE_BOUNDS_LINK }; + void clearRobotModel() override; void onRobotModelLoaded() override; void onNewPlanningSceneState() override; void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) override; diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index 455568c14f..4c58e2fefd 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -105,6 +105,7 @@ class MotionPlanningFrame : public QWidget MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz::DisplayContext* context, QWidget* parent = nullptr); ~MotionPlanningFrame() override; + void clearRobotModel(); void changePlanningGroup(); void enable(); void disable(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h index e2de501104..679cf7d1a8 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h @@ -126,6 +126,7 @@ class MotionPlanningFrameJointsWidget : public QWidget MotionPlanningFrameJointsWidget(MotionPlanningDisplay* display, QWidget* parent = nullptr); ~MotionPlanningFrameJointsWidget() override; + void clearRobotModel(); void changePlanningGroup(const std::string& group_name, const robot_interaction::InteractionHandlerPtr& start_state_handler, const robot_interaction::InteractionHandlerPtr& goal_state_handler); 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 dc16f4c7ac..9a9d65fecc 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 @@ -35,6 +35,7 @@ /* Author: Ioan Sucan, Dave Coleman, Adam Leeper, Sachin Chitta */ #include +#include #include #include #include @@ -1139,6 +1140,20 @@ void MotionPlanningDisplay::populateMenuHandler(std::shared_ptrclearRobotModel(); + previous_state_.reset(); + query_start_state_.reset(); + query_goal_state_.reset(); + kinematics_metrics_.reset(); + robot_interaction_.reset(); + // ... before calling the parent's method, which finally destroys the creating RobotModelLoader. + PlanningSceneDisplay::clearRobotModel(); +} + void MotionPlanningDisplay::onRobotModelLoaded() { PlanningSceneDisplay::onRobotModelLoaded(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 34082ce602..2006089f07 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -425,6 +425,13 @@ void MotionPlanningFrame::changePlanningGroupHelper() } } +void MotionPlanningFrame::clearRobotModel() +{ + ui_->planner_param_treeview->setMoveGroup(moveit::planning_interface::MoveGroupInterfacePtr()); + joints_tab_->clearRobotModel(); + move_group_.reset(); +} + void MotionPlanningFrame::changePlanningGroup() { planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::changePlanningGroupHelper, this), diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp index 652c2e0e30..fdd7f0fe9f 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp @@ -198,15 +198,19 @@ MotionPlanningFrameJointsWidget::~MotionPlanningFrameJointsWidget() delete ui_; } -void MotionPlanningFrameJointsWidget::changePlanningGroup( - const std::string& group_name, const robot_interaction::InteractionHandlerPtr& start_state_handler, - const robot_interaction::InteractionHandlerPtr& goal_state_handler) +void MotionPlanningFrameJointsWidget::clearRobotModel() { - // release previous models (if any) ui_->joints_view_->setModel(nullptr); start_state_model_.reset(); goal_state_model_.reset(); +} +void MotionPlanningFrameJointsWidget::changePlanningGroup( + const std::string& group_name, const robot_interaction::InteractionHandlerPtr& start_state_handler, + const robot_interaction::InteractionHandlerPtr& goal_state_handler) +{ + // release previous models (if any) + clearRobotModel(); // create new models start_state_handler_ = start_state_handler; goal_state_handler_ = goal_state_handler; diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h index 94cc27ff32..959aca2d4f 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h @@ -142,7 +142,7 @@ protected Q_SLOTS: /// This function is used by loadRobotModel() and should only be called in the MainLoop /// You probably should not call this function directly - void clearRobotModel(); + virtual void clearRobotModel(); /// This function constructs a new planning scene. Probably this should be called in a background thread /// as it may take some time to complete its execution 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 44cc719a6f..8aa6630e7b 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 @@ -516,8 +516,8 @@ planning_scene_monitor::PlanningSceneMonitorPtr PlanningSceneDisplay::createPlan void PlanningSceneDisplay::clearRobotModel() { planning_scene_render_.reset(); - planning_scene_monitor_.reset(); // this so that the destructor of the PlanningSceneMonitor gets called before a new - // instance of a scene monitor is constructed + // Ensure old PSM is destroyed before we attempt to create a new one + planning_scene_monitor_.reset(); } void PlanningSceneDisplay::loadRobotModel() From f3ac6070497da90da33551fc1dc3a68938340413 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 22 Oct 2021 10:46:36 +0200 Subject: [PATCH 035/229] Normalize incoming transforms (#2920) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Normalize incoming transforms * fixup: adapt comment according to review suggestion Co-authored-by: Michael Görner --- moveit_core/transforms/src/transforms.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/moveit_core/transforms/src/transforms.cpp b/moveit_core/transforms/src/transforms.cpp index 005bc2e07f..91cdd66541 100644 --- a/moveit_core/transforms/src/transforms.cpp +++ b/moveit_core/transforms/src/transforms.cpp @@ -130,8 +130,15 @@ void Transforms::setTransform(const geometry_msgs::TransformStamped& transform) { if (sameFrame(transform.child_frame_id, target_frame_)) { - Eigen::Isometry3d t = tf2::transformToEigen(transform.transform); - setTransform(t, transform.header.frame_id); + // convert message manually to ensure correct normalization for double (error < 1e-12) + // tf2 only enforces float normalization (error < 1e-5) + const auto& trans = transform.transform.translation; + const auto& rot = transform.transform.rotation; + Eigen::Translation3d translation(trans.x, trans.y, trans.z); + Eigen::Quaterniond rotation(rot.w, rot.x, rot.y, rot.z); + rotation.normalize(); + + setTransform(translation * rotation, transform.header.frame_id); } else { From 0d006508180f72ad6d3354b72dc3eaba65720ca8 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 20 Oct 2021 20:50:01 +0200 Subject: [PATCH 036/229] Fix MotionPlanningFrame's namespace handling (#2922) * waitForAction(): remove NodeHandle argument * The NodeHandle was just for NodeHandle::ok(), which can be handled by ros::ok() as well. * Fix initialization of params, etc. that depend on MoveGroupNS * When the MoveGroupNS has changed, we should re-initialize all these params, subscribers, and topics. Thus having them in a central place is helpful ;-) * Fix namespaces as pointed out by @v4hn * Simplify nh_ naming * update comments --- .../motion_planning_frame.h | 13 ++-- .../src/motion_planning_display.cpp | 9 --- .../src/motion_planning_frame.cpp | 69 ++++++++++++------- .../motion_planning_frame_manipulation.cpp | 2 +- 4 files changed, 53 insertions(+), 40 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index 455568c14f..33334c24e4 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -114,6 +114,7 @@ class MotionPlanningFrame : public QWidget static const int ITEM_TYPE_SCENE = 1; static const int ITEM_TYPE_QUERY = 2; + void initFromMoveGroupNS(); void constructPlanningRequest(moveit_msgs::MotionPlanRequest& mreq); void updateSceneMarkers(float wall_dt, float ros_dt); @@ -293,8 +294,7 @@ private Q_SLOTS: std::unique_ptr > object_recognition_client_; template - void waitForAction(const T& action, const ros::NodeHandle& node_handle, const ros::Duration& wait_for_server, - const std::string& name); + void waitForAction(const T& action, const ros::Duration& wait_for_server, const std::string& name); void listenDetectedObjects(const object_recognition_msgs::RecognizedObjectArrayPtr& msg); ros::Subscriber object_recognition_subscriber_; @@ -321,7 +321,7 @@ private Q_SLOTS: /* Selects or unselects a item in a list by the item name */ void setItemSelectionInList(const std::string& item_name, bool selection, QListWidget* list); - ros::NodeHandle nh_; + ros::NodeHandle nh_; // node handle with the namespace of the connected move_group node ros::Publisher planning_scene_publisher_; ros::Publisher planning_scene_world_publisher_; @@ -337,8 +337,7 @@ private Q_SLOTS: // \todo THIS IS REALLY BAD. NEED TO MOVE THIS AND RELATED FUNCTIONALITY OUT OF HERE template -void MotionPlanningFrame::waitForAction(const T& action, const ros::NodeHandle& node_handle, - const ros::Duration& wait_for_server, const std::string& name) +void MotionPlanningFrame::waitForAction(const T& action, const ros::Duration& wait_for_server, const std::string& name) { ROS_DEBUG("Waiting for MoveGroup action server (%s)...", name.c_str()); @@ -354,7 +353,7 @@ void MotionPlanningFrame::waitForAction(const T& action, const ros::NodeHandle& if (wait_for_server == ros::Duration(0, 0)) { // wait forever until action server connects - while (node_handle.ok() && !action->isServerConnected()) + while (ros::ok() && !action->isServerConnected()) { ros::WallDuration(0.02).sleep(); ros::spinOnce(); @@ -364,7 +363,7 @@ void MotionPlanningFrame::waitForAction(const T& action, const ros::NodeHandle& { // wait for a limited amount of non-simulated time ros::WallTime final_time = ros::WallTime::now() + ros::WallDuration(wait_for_server.toSec()); - while (node_handle.ok() && !action->isServerConnected() && final_time > ros::WallTime::now()) + while (ros::ok() && !action->isServerConnected() && final_time > ros::WallTime::now()) { ros::WallDuration(0.02).sleep(); ros::spinOnce(); 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 dc16f4c7ac..b68d565d50 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 @@ -229,15 +229,6 @@ void MotionPlanningDisplay::onInitialize() rviz::WindowManagerInterface* window_context = context_->getWindowManager(); frame_ = new MotionPlanningFrame(this, context_, window_context ? window_context->getParentWindow() : nullptr); - ros::NodeHandle move_group_nh(ros::names::append(getMoveGroupNS(), "move_group")); - std::string param_name; - std::string host_param; - int port; - if (move_group_nh.searchParam("warehouse_host", param_name) && move_group_nh.getParam(param_name, host_param)) - frame_->ui_->database_host->setText(QString::fromStdString(host_param)); - if (move_group_nh.searchParam("warehouse_port", param_name) && move_group_nh.getParam(param_name, port)) - frame_->ui_->database_port->setValue(port); - connect(frame_, SIGNAL(configChanged()), this->getModel(), SIGNAL(configChanged())); resetStatusTextColor(); addStatusText("Initialized."); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 34082ce602..8c627d9345 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -59,12 +59,7 @@ namespace moveit_rviz_plugin { MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz::DisplayContext* context, QWidget* parent) - : QWidget(parent) - , planning_display_(pdisplay) - , context_(context) - , ui_(new Ui::MotionPlanningUI()) - , first_time_(true) - , clear_octomap_service_client_(nh_.serviceClient(move_group::CLEAR_OCTOMAP_SERVICE_NAME)) + : QWidget(parent), planning_display_(pdisplay), context_(context), ui_(new Ui::MotionPlanningUI()), first_time_(true) { // set up the GUI ui_->setupUi(this); @@ -82,17 +77,6 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: connect(planning_display_, SIGNAL(queryStartStateChanged()), joints_tab_, SLOT(queryStartStateChanged())); connect(planning_display_, SIGNAL(queryGoalStateChanged()), joints_tab_, SLOT(queryGoalStateChanged())); - // Set initial velocity and acceleration scaling factors from ROS parameters - double factor; - nh_.param("robot_description_planning/default_velocity_scaling_factor", factor, 0.1); - ui_->velocity_scaling_factor->setValue(factor); - nh_.param("robot_description_planning/default_acceleration_scaling_factor", factor, 0.1); - ui_->acceleration_scaling_factor->setValue(factor); - - // Query default planning pipeline id - nh_.getParam(planning_display_->getMoveGroupNS() + "/move_group/default_planning_pipeline", - default_planning_pipeline_); - // connect bottons to actions; each action usually registers the function pointer for the actual computation, // to keep the GUI more responsive (using the background job processing) connect(ui_->plan_button, SIGNAL(clicked()), this, SLOT(planButtonClicked())); @@ -197,21 +181,17 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: known_collision_objects_version_ = 0; - planning_scene_publisher_ = nh_.advertise("planning_scene", 1); - planning_scene_world_publisher_ = nh_.advertise("planning_scene_world", 1); + initFromMoveGroupNS(); - // object_recognition_trigger_publisher_ = nh_.advertise("recognize_objects_switch", 1); object_recognition_client_ = std::make_unique>( OBJECT_RECOGNITION_ACTION, false); - object_recognition_subscriber_ = - nh_.subscribe("recognized_object_array", 1, &MotionPlanningFrame::listenDetectedObjects, this); if (object_recognition_client_) { try { - waitForAction(object_recognition_client_, nh_, ros::Duration(3.0), OBJECT_RECOGNITION_ACTION); + waitForAction(object_recognition_client_, ros::Duration(3.0), OBJECT_RECOGNITION_ACTION); } catch (std::exception& ex) { @@ -576,11 +556,54 @@ void MotionPlanningFrame::enable() ui_->library_label->setStyleSheet("QLabel { color : red; font: bold }"); ui_->object_status->setText(""); + const std::string new_ns = ros::names::resolve(planning_display_->getMoveGroupNS()); + if (nh_.getNamespace() != new_ns) + { + ROS_INFO("MoveGroup namespace changed: %s -> %s. Reloading params.", nh_.getNamespace().c_str(), new_ns.c_str()); + initFromMoveGroupNS(); + } + // activate the frame if (parentWidget()) parentWidget()->show(); } +// (re)initialize after MotionPlanningDisplay::changedMoveGroupNS() +// Should be called from constructor and enable() only +void MotionPlanningFrame::initFromMoveGroupNS() +{ + nh_ = ros::NodeHandle(planning_display_->getMoveGroupNS()); // / + + // Create namespace-dependent services, topics, and subscribers + clear_octomap_service_client_ = nh_.serviceClient(move_group::CLEAR_OCTOMAP_SERVICE_NAME); + + object_recognition_subscriber_ = + nh_.subscribe("recognized_object_array", 1, &MotionPlanningFrame::listenDetectedObjects, this); + + planning_scene_publisher_ = nh_.advertise("planning_scene", 1); + planning_scene_world_publisher_ = nh_.advertise("planning_scene_world", 1); + + // Set initial velocity and acceleration scaling factors from ROS parameters + double factor; + nh_.param("robot_description_planning/default_velocity_scaling_factor", factor, 0.1); + ui_->velocity_scaling_factor->setValue(factor); + nh_.param("robot_description_planning/default_acceleration_scaling_factor", factor, 0.1); + ui_->acceleration_scaling_factor->setValue(factor); + + // Fetch parameters from private move_group sub space + ros::NodeHandle nh_mg("move_group"); // //move_group + std::string param_name; + std::string host_param; + int port; + if (nh_mg.searchParam("warehouse_host", param_name) && nh_mg.getParam(param_name, host_param)) + ui_->database_host->setText(QString::fromStdString(host_param)); + if (nh_mg.searchParam("warehouse_port", param_name) && nh_mg.getParam(param_name, port)) + ui_->database_port->setValue(port); + + // Get default planning pipeline id + nh_mg.param("default_planning_pipeline", default_planning_pipeline_, ""); +} + void MotionPlanningFrame::disable() { move_group_.reset(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp index a002f27c7b..76e4c0d6da 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp @@ -143,7 +143,7 @@ void MotionPlanningFrame::triggerObjectDetection() OBJECT_RECOGNITION_ACTION, false); try { - waitForAction(object_recognition_client_, nh_, ros::Duration(3.0), OBJECT_RECOGNITION_ACTION); + waitForAction(object_recognition_client_, ros::Duration(3.0), OBJECT_RECOGNITION_ACTION); } catch (std::exception& ex) { From e763f768e3cd252ecb933a26678c95d718de37ba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Fri, 22 Oct 2021 11:25:48 +0200 Subject: [PATCH 037/229] import exception with the same style throughout the package (#2912) There's no hard reason to do it this way or the other, but a user reported unverified issues with the global include. --- moveit_commander/src/moveit_commander/conversions.py | 2 +- moveit_commander/src/moveit_commander/robot.py | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/moveit_commander/src/moveit_commander/conversions.py b/moveit_commander/src/moveit_commander/conversions.py index dcf884aa47..1fd154104c 100644 --- a/moveit_commander/src/moveit_commander/conversions.py +++ b/moveit_commander/src/moveit_commander/conversions.py @@ -39,7 +39,7 @@ # Use Python 3.x behaviour as fallback and choose the non-unicode version from io import BytesIO as StringIO -from moveit_commander import MoveItCommanderException +from .exception import MoveItCommanderException from geometry_msgs.msg import Pose, PoseStamped, Transform import rospy import tf diff --git a/moveit_commander/src/moveit_commander/robot.py b/moveit_commander/src/moveit_commander/robot.py index 413f6d1774..a095b6b310 100644 --- a/moveit_commander/src/moveit_commander/robot.py +++ b/moveit_commander/src/moveit_commander/robot.py @@ -32,7 +32,8 @@ # # Author: Ioan Sucan -from moveit_commander import MoveGroupCommander, MoveItCommanderException +from moveit_commander import MoveGroupCommander +from .exception import MoveItCommanderException from moveit_ros_planning_interface import _moveit_robot_interface from moveit_msgs.msg import RobotState from visualization_msgs.msg import MarkerArray From 4b08ced93b0445738dd72271e61ecf1cc5a5a2b3 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 20 Oct 2021 12:02:15 +0200 Subject: [PATCH 038/229] consistent parameter names for AttachedBody constructor "attach_posture" is plain wrong. I don't see why clang-tidy did not find this before. --- .../robot_state/include/moveit/robot_state/attached_body.h | 5 +++-- moveit_core/robot_state/src/attached_body.cpp | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h index 6582a697c1..dec578efc2 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h +++ b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h @@ -61,10 +61,11 @@ class AttachedBody * * The name of this body is \e id and it consists of \e shapes that attach to the link by the transforms * \e shape_poses. The set of links that are allowed to be touched by this object is specified by \e touch_links. + * detach_posture may describe a detach motion for the gripper when placing the object. * The shape and subframe poses are relative to the \e pose, and \e pose is relative to the parent link. */ - AttachedBody(const LinkModel* link, const std::string& id, const Eigen::Isometry3d& pose, + AttachedBody(const LinkModel* parent, const std::string& id, const Eigen::Isometry3d& pose, const std::vector& shapes, const EigenSTL::vector_Isometry3d& shape_poses, - const std::set& touch_links, const trajectory_msgs::JointTrajectory& attach_posture, + const std::set& touch_links, const trajectory_msgs::JointTrajectory& detach_posture, const moveit::core::FixedTransformsMap& subframe_poses = moveit::core::FixedTransformsMap()); ~AttachedBody(); diff --git a/moveit_core/robot_state/src/attached_body.cpp b/moveit_core/robot_state/src/attached_body.cpp index d0a0fa58fa..cefccb6da5 100644 --- a/moveit_core/robot_state/src/attached_body.cpp +++ b/moveit_core/robot_state/src/attached_body.cpp @@ -43,12 +43,12 @@ namespace moveit { namespace core { -AttachedBody::AttachedBody(const LinkModel* parent_link_model, const std::string& id, const Eigen::Isometry3d& pose, +AttachedBody::AttachedBody(const LinkModel* parent, const std::string& id, const Eigen::Isometry3d& pose, const std::vector& shapes, const EigenSTL::vector_Isometry3d& shape_poses, const std::set& touch_links, const trajectory_msgs::JointTrajectory& detach_posture, const FixedTransformsMap& subframe_poses) - : parent_link_model_(parent_link_model) + : parent_link_model_(parent) , id_(id) , pose_(pose) , shapes_(shapes) From 265cb19288240ec9d347ba8c7cd7eefd7e06bbcc Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 20 Oct 2021 12:08:24 +0200 Subject: [PATCH 039/229] add RS::getRigidlyConnectedParentLinkModel to resolve links for attached objects as well --- .../include/moveit/robot_state/robot_state.h | 7 ++++ moveit_core/robot_state/src/robot_state.cpp | 25 ++++++++++++++ .../robot_state/test/robot_state_test.cpp | 33 +++++++++++++++++++ 3 files changed, 65 insertions(+) diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index c53cc2d743..dd8859dbad 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -1376,6 +1376,13 @@ class RobotState /** \brief Update the state after setting a particular link to the input global transform pose.*/ void updateStateWithLinkAt(const LinkModel* link, const Eigen::Isometry3d& transform, bool backward = false); + /** \brief Get the latest link upwards the kinematic tree which is only connected via fixed joints. + * + * This behaves the same as RobotModel::getRigidlyConnectedParentLinkModel, + * but can additionally resolve parents for attached objects / subframes. + */ + const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const std::string& frame) const; + /** \brief Get the link transform w.r.t. the root link (model frame) of the RobotModel. * This is typically the root link of the URDF unless a virtual joint is present. * Checks the cache and if there are any dirty (non-updated) transforms, first updates them as needed. diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 662cf5e752..c60cd509cc 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -797,6 +797,31 @@ void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isome it->second->computeTransform(global_link_transforms_[it->second->getAttachedLink()->getLinkIndex()]); } +const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::string& frame) const +{ + const moveit::core::LinkModel* link{ nullptr }; + + size_t idx = 0; + if ((idx = frame.find('/')) != std::string::npos) + { // resolve sub frame + std::string object{ frame.substr(0, idx) }; + if (!hasAttachedBody(object)) + return nullptr; + auto body{ getAttachedBody(object) }; + if (!body->hasSubframeTransform(frame)) + return nullptr; + link = body->getAttachedLink(); + } + else if (hasAttachedBody(frame)) + { + link = getAttachedBody(frame)->getAttachedLink(); + } + else if (getRobotModel()->hasLinkModel(frame)) + link = getLinkModel(frame); + + return getRobotModel()->getRigidlyConnectedParentLinkModel(link); +} + bool RobotState::satisfiesBounds(double margin) const { const std::vector& jm = robot_model_->getActiveJointModels(); diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index 6482ef324c..7313a94aed 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -676,6 +676,39 @@ TEST_F(OneRobot, testInterpolation) EXPECT_TRUE(nan_exception) << "NaN interpolation parameter did not create expected exception."; } +TEST_F(OneRobot, rigidlyConnectedParent) +{ + // link_e is its own rigidly-connected parent + const moveit::core::LinkModel* link_e{ robot_model_->getLinkModel("link_e") }; + EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_e), link_e); + + // link_b is rigidly connected to its parent link_a + const moveit::core::LinkModel* link_a{ robot_model_->getLinkModel("link_a") }; + const moveit::core::LinkModel* link_b{ robot_model_->getLinkModel("link_b") }; + EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_b), link_a); + + moveit::core::RobotState state(robot_model_); + + EXPECT_EQ(state.getRigidlyConnectedParentLinkModel("link_b"), link_a); + + // attach "object" with "subframe" to link_b + state.attachBody(new moveit::core::AttachedBody( + link_b, "object", Eigen::Isometry3d::Identity(), std::vector{}, + EigenSTL::vector_Isometry3d{}, std::set{}, trajectory_msgs::JointTrajectory{}, + moveit::core::FixedTransformsMap{ { "subframe", Eigen::Isometry3d::Identity() } })); + + // RobotState's version should resolve these too + EXPECT_EQ(link_a, state.getRigidlyConnectedParentLinkModel("object")); + EXPECT_EQ(link_a, state.getRigidlyConnectedParentLinkModel("object/subframe")); + + // test failure cases + EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("no_object")); + EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("object/no_subframe")); + EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("")); + EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("object/")); + EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("/")); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From a2a29614634ea17ad03b381dc685655706fd1795 Mon Sep 17 00:00:00 2001 From: Wolf Vollprecht Date: Fri, 22 Oct 2021 13:39:53 +0200 Subject: [PATCH 040/229] CI: Pin versions of dependencies of cross-platform build (#2929) --- .github/workflows/cross_platform_ci.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/cross_platform_ci.yml b/.github/workflows/cross_platform_ci.yml index 9fe7eb6440..c43ed62bd7 100644 --- a/.github/workflows/cross_platform_ci.yml +++ b/.github/workflows/cross_platform_ci.yml @@ -21,10 +21,10 @@ jobs: - uses: actions/checkout@v2 - name: Set up Build Dependencies - uses: mamba-org/provision-with-micromamba@main + uses: mamba-org/provision-with-micromamba@v10 with: environment-file: .github/ci_cross_platform_env.yml - micromamba-version: latest + micromamba-version: 0.17.0 - name: Set up MoveIt Core Dependencies on Unix if: runner.os == 'Linux' || runner.os == 'macOS' From 61d18f2f073aa4c8a13c2278c41a63591d401c4a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 22 Oct 2021 14:27:39 +0200 Subject: [PATCH 041/229] moveit_controller_manager.launch: pass execution_type via pass_all_args (#2928) While we need to pass execution_type to fake_moveit_controller_manager.launch, the controller_manager.launch files of real-robot shouldn't be required to define this argument. However, if they don't roslaunch fails with an `unused args` exception (see #2786). Passing arguments via pass_all_args should solve that issue. --- .../launch/trajectory_execution.launch.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml index 2a269a075c..1efc010d48 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml @@ -17,8 +17,8 @@ - - - + + From a24bd54a835848be45d5f54398e2f49790574658 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 22 Oct 2021 14:12:40 +0200 Subject: [PATCH 042/229] Fix Debug build: re-add seemingly unused arguments --- .../src/bullet_integration/bullet_utils.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp index 12fad66cb9..bea182e7ab 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp @@ -44,8 +44,9 @@ namespace collision_detection_bullet { -btCollisionShape* createShapePrimitive(const shapes::Box* geom, const CollisionObjectType& /*collision_object_type*/) +btCollisionShape* createShapePrimitive(const shapes::Box* geom, const CollisionObjectType& collision_object_type) { + (void)(collision_object_type); assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE); const double* size = geom->size; btScalar a = static_cast(size[0] / 2); @@ -55,23 +56,25 @@ btCollisionShape* createShapePrimitive(const shapes::Box* geom, const CollisionO return (new btBoxShape(btVector3(a, b, c))); } -btCollisionShape* createShapePrimitive(const shapes::Sphere* geom, const CollisionObjectType& /*collision_object_type*/) +btCollisionShape* createShapePrimitive(const shapes::Sphere* geom, const CollisionObjectType& collision_object_type) { + (void)(collision_object_type); assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE); return (new btSphereShape(static_cast(geom->radius))); } -btCollisionShape* createShapePrimitive(const shapes::Cylinder* geom, - const CollisionObjectType& /*collision_object_type*/) +btCollisionShape* createShapePrimitive(const shapes::Cylinder* geom, const CollisionObjectType& collision_object_type) { + (void)(collision_object_type); assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE); btScalar r = static_cast(geom->radius); btScalar l = static_cast(geom->length / 2); return (new btCylinderShapeZ(btVector3(r, r, l))); } -btCollisionShape* createShapePrimitive(const shapes::Cone* geom, const CollisionObjectType& /*collision_object_type*/) +btCollisionShape* createShapePrimitive(const shapes::Cone* geom, const CollisionObjectType& collision_object_type) { + (void)(collision_object_type); assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE); btScalar r = static_cast(geom->radius); btScalar l = static_cast(geom->length); From e089c957ddd463e406df573f40ff183d3e40e91d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 24 Oct 2021 22:20:31 +0200 Subject: [PATCH 043/229] CI: Improve code coverage generation (#2931) * Switch CCOV to build type Debug RelWithDebInfo was using -DNDEBUG, thus ignoring all assertions. Also use -fno-omit-frame-pointer to have better coverage analysis. * Use lcov-action to generate coverage info * Unify syntax of `if` conditions --- .ci.prepare_codecov | 21 --------------------- .github/workflows/ci.yaml | 28 +++++++++++++++++----------- 2 files changed, 17 insertions(+), 32 deletions(-) delete mode 100755 .ci.prepare_codecov diff --git a/.ci.prepare_codecov b/.ci.prepare_codecov deleted file mode 100755 index 91119f5dc1..0000000000 --- a/.ci.prepare_codecov +++ /dev/null @@ -1,21 +0,0 @@ -#!/bin/bash -pushd $BASEDIR - -BLUE='\033[0;34m' -NOCOLOR='\033[0m' - -apt-get install -qq lcov - -echo -e "${BLUE}Capture coverage info${NOCOLOR}" -lcov --capture --directory target_ws --output-file coverage.info - -echo -e "${BLUE}Extract repository files${NOCOLOR}" -lcov --extract coverage.info "$BASEDIR/target_ws/src/$TARGET_REPO_NAME/*" --output-file coverage.info - -echo -e "${BLUE}Filter out test files${NOCOLOR}" -lcov --remove coverage.info '*/test/*' --output-file coverage.info - -echo -e "${BLUE}Output coverage data for debugging${NOCOLOR}" -lcov --list coverage.info - -popd diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index d5fb9a10ce..72b08dab3b 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -35,10 +35,9 @@ jobs: # Pull any updates to the upstream workspace (after restoring it from cache) AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src AFTER_SETUP_DOWNSTREAM_WORKSPACE: vcs pull $BASEDIR/downstream_ws/src - AFTER_RUN_TARGET_TEST: ${{ matrix.env.CCOV && './.ci.prepare_codecov' || '' }} TARGET_CMAKE_ARGS: > - -DCMAKE_BUILD_TYPE=${{ matrix.env.CCOV && 'RelWithDebInfo' || 'Release'}} - -DCMAKE_CXX_FLAGS="${{ matrix.env.CLANG_TIDY != 'pedantic' && '-Werror ' || '' }}${{ matrix.env.CCOV && ' --coverage'}}" + -DCMAKE_BUILD_TYPE=${{ matrix.env.CCOV && 'Debug' || 'Release'}} + -DCMAKE_CXX_FLAGS="${{ matrix.env.CLANG_TIDY != 'pedantic' && '-Werror ' || '' }}${{ matrix.env.CCOV && ' --coverage -O2 -fno-omit-frame-pointer'}}" DOWNSTREAM_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-Wall -Wextra" CCACHE_DIR: ${{ github.workspace }}/.ccache BASEDIR: ${{ github.workspace }}/.work @@ -56,7 +55,7 @@ jobs: runs-on: ubuntu-latest steps: - name: "Free up disk space" - if: ${{ matrix.env.CCOV }} + if: matrix.env.CCOV run: | sudo apt-get -qq purge build-essential "ghc*" sudo apt-get clean @@ -89,7 +88,7 @@ jobs: # The target directory cache doesn't include the source directory because # that comes from the checkout. See "prepare target_ws for cache" task below - name: Cache target workspace - if: ${{ ! matrix.env.CCOV }} + if: "!matrix.env.CCOV" uses: pat-s/always-upload-cache@v2.1.5 with: path: ${{ env.BASEDIR }}/target_ws @@ -109,7 +108,7 @@ jobs: CACHE_PREFIX: ccache-${{ matrix.env.IMAGE }}${{ matrix.env.CCOV && '-ccov' || '' }} - name: Generate ikfast packages - if: ${{ matrix.env.IKFAST_TEST }} + if: matrix.env.IKFAST_TEST run: | moveit_kinematics/test/test_ikfast_plugins.sh - id: ici @@ -123,19 +122,26 @@ jobs: with: name: test-results-${{ matrix.env.IMAGE }} path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml + - name: Generate codecov report + uses: rhaschke/lcov-action@main + if: matrix.env.CCOV && steps.ici.outputs.target_test_results == '0' + with: + docker: $DOCKER_IMAGE + workdir: ${{ env.BASEDIR }}/target_ws + ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"' - name: Upload codecov report - uses: codecov/codecov-action@v1 - if: always() && matrix.env.CCOV && steps.ici.outputs.target_test_results == '0' + uses: codecov/codecov-action@v2 + if: matrix.env.CCOV && steps.ici.outputs.target_test_results == '0' with: - files: ${{ env.BASEDIR }}/coverage.info + files: ${{ env.BASEDIR }}/target_ws/coverage.info - name: Upload clang-tidy changes uses: rhaschke/upload-git-patch-action@main - if: always() && matrix.env.CLANG_TIDY + if: matrix.env.CLANG_TIDY with: name: clang-tidy path: ${{ env.BASEDIR }}/target_ws/src/$(basename $(pwd)) - name: Prepare target_ws for cache - if: always() && ! matrix.env.CCOV + if: "!matrix.env.CCOV" run: | du -sh ${{ env.BASEDIR }}/target_ws sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete From 27b4290f1c828192c98bc64ba31329c23834518a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 26 Oct 2021 17:37:49 +0200 Subject: [PATCH 044/229] CI: minor updates --- .github/workflows/ci.yaml | 3 +-- .github/workflows/format.yaml | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 72b08dab3b..05cc19aadb 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -109,8 +109,7 @@ jobs: - name: Generate ikfast packages if: matrix.env.IKFAST_TEST - run: | - moveit_kinematics/test/test_ikfast_plugins.sh + run: moveit_kinematics/test/test_ikfast_plugins.sh - id: ici name: Run industrial_ci uses: ros-industrial/industrial_ci@master diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml index 544b4613e1..e49de3638a 100644 --- a/.github/workflows/format.yaml +++ b/.github/workflows/format.yaml @@ -18,7 +18,7 @@ jobs: - name: Install clang-format-10 run: sudo apt-get install clang-format-10 - uses: rhaschke/install-catkin_lint-action@v1.0 - - uses: pre-commit/action@v2.0.0 + - uses: pre-commit/action@v2.0.3 id: precommit - name: Upload pre-commit changes if: failure() && steps.precommit.outcome == 'failure' From d1465a7dd55ffead03b35f9d0d3d878fc4757eeb Mon Sep 17 00:00:00 2001 From: Michael Wiznitzer <31753650+mechwiz@users.noreply.github.com> Date: Wed, 27 Oct 2021 09:36:41 -0400 Subject: [PATCH 045/229] Servo: sync position limit enforcement with MoveIt2 (#2898) * fix enforce position bug * remove unnecessary variable * make clang tidy happy * Update my comment * implement same logic as in the moveit2! repo * fix copy-pase error Co-authored-by: Michael Wiznitzer Co-authored-by: AndyZe --- .../include/moveit_servo/servo_calcs.h | 2 +- moveit_ros/moveit_servo/src/servo_calcs.cpp | 14 +++++++++----- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index dddf2ef8bc..a154a9943d 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -148,7 +148,7 @@ class ServoCalcs void enforceVelLimits(Eigen::ArrayXd& delta_theta); /** \brief Avoid overshooting joint limits */ - bool enforcePositionLimits(); + bool enforcePositionLimits(sensor_msgs::JointState& joint_state); /** \brief Possibly calculate a velocity scaling factor, due to proximity of * singularity and direction of motion diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index 0305fb2211..8b8cd34d01 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -580,7 +580,7 @@ bool ServoCalcs::convertDeltasToOutgoingCmd(trajectory_msgs::JointTrajectory& jo composeJointTrajMessage(internal_joint_state_, joint_trajectory); - if (!enforcePositionLimits()) + if (!enforcePositionLimits(internal_joint_state_)) { suddenHalt(joint_trajectory); status_ = StatusCode::JOINT_BOUND; @@ -779,7 +779,7 @@ void ServoCalcs::enforceVelLimits(Eigen::ArrayXd& delta_theta) delta_theta = velocity_scaling_factor * velocity * parameters_.publish_period; } -bool ServoCalcs::enforcePositionLimits() +bool ServoCalcs::enforcePositionLimits(sensor_msgs::JointState& joint_state) { bool halting = false; @@ -797,14 +797,18 @@ bool ServoCalcs::enforcePositionLimits() } if (!current_state_->satisfiesPositionBounds(joint, -parameters_.joint_limit_margin)) { - const std::vector limits = joint->getVariableBoundsMsg(); + const std::vector& limits = joint->getVariableBoundsMsg(); // Joint limits are not defined for some joints. Skip them. if (!limits.empty()) { - if ((current_state_->getJointVelocities(joint)[0] < 0 && + // Check if pending velocity command is moving in the right direction + auto joint_itr = std::find(joint_state.name.begin(), joint_state.name.end(), joint->getName()); + auto joint_idx = std::distance(joint_state.name.begin(), joint_itr); + + if ((joint_state.velocity.at(joint_idx) < 0 && (joint_angle < (limits[0].min_position + parameters_.joint_limit_margin))) || - (current_state_->getJointVelocities(joint)[0] > 0 && + (joint_state.velocity.at(joint_idx) > 0 && (joint_angle > (limits[0].max_position - parameters_.joint_limit_margin)))) { ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, From c65260dbee4b81fd0405f2f84947520de3666a01 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 2 Nov 2021 21:54:38 +0100 Subject: [PATCH 046/229] Improve error messages - Downgrade ERROR to WARN - Report affected joint name - Quote (possibly empty) planner id --- .../src/joint_limits_aggregator.cpp | 8 ++++---- .../src/pilz_industrial_motion_planner.cpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp index 5c800573b5..e17a33f1a4 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp @@ -105,7 +105,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updatePositionLimitF { // LCOV_EXCL_START case 0: - ROS_ERROR_STREAM("no bounds set for joint " << joint_model->getName()); + ROS_WARN_STREAM("no bounds set for joint " << joint_model->getName()); break; // LCOV_EXCL_STOP case 1: @@ -115,7 +115,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updatePositionLimitF break; // LCOV_EXCL_START default: - ROS_ERROR_STREAM("Multi-DOF-Joints not supported. The robot won't move. " << joint_model->getName()); + ROS_WARN_STREAM("Multi-DOF-Joint '" << joint_model->getName() << "' not supported."); joint_limit.has_position_limits = true; joint_limit.min_position = 0; joint_limit.max_position = 0; @@ -134,7 +134,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updateVelocityLimitF { // LCOV_EXCL_START case 0: - ROS_ERROR_STREAM("no bounds set for joint " << joint_model->getName()); + ROS_WARN_STREAM("no bounds set for joint " << joint_model->getName()); break; // LCOV_EXCL_STOP case 1: @@ -143,7 +143,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updateVelocityLimitF break; // LCOV_EXCL_START default: - ROS_ERROR_STREAM("Multi-DOF-Joints not supported. The robot won't move."); + ROS_WARN_STREAM("Multi-DOF-Joint '" << joint_model->getName() << "' not supported."); joint_limit.has_velocity_limits = true; joint_limit.max_velocity = 0; break; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp index d6e3b43294..9d830544fc 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp @@ -127,7 +127,7 @@ CommandPlanner::getPlanningContext(const planning_scene::PlanningSceneConstPtr& // Check that a loaded for this request exists if (!canServiceRequest(req)) { - ROS_ERROR_STREAM("No ContextLoader for planner_id " << req.planner_id.c_str() << " found. Planning not possible."); + ROS_ERROR_STREAM("No ContextLoader for planner_id '" << req.planner_id.c_str() << "' found. Planning not possible."); return nullptr; } From ed78706bb77d6f583b0d63058b68a9a93ac0f86d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 2 Nov 2021 22:20:36 +0100 Subject: [PATCH 047/229] Avoid duplicate error messages --- .../src/trajectory_generator_ptp.cpp | 15 +++------------ 1 file changed, 3 insertions(+), 12 deletions(-) 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 639058ebd0..3649916b3e 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 @@ -69,20 +69,11 @@ TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const robot_model::RobotModelCons JointLimit most_strict_limit = joint_limits_.getCommonLimit(active_joints); if (!most_strict_limit.has_velocity_limits) - { - ROS_ERROR_STREAM("velocity limit not set for group " << jmg->getName()); - throw TrajectoryGeneratorInvalidLimitsException("velocity limit not set for group " + jmg->getName()); - } + throw TrajectoryGeneratorInvalidLimitsException("velocity limit not set for group " + group_name); if (!most_strict_limit.has_acceleration_limits) - { - ROS_ERROR_STREAM("acceleration limit not set for group " << jmg->getName()); - throw TrajectoryGeneratorInvalidLimitsException("acceleration limit not set for group " + jmg->getName()); - } + throw TrajectoryGeneratorInvalidLimitsException("acceleration limit not set for group " + group_name); if (!most_strict_limit.has_deceleration_limits) - { - ROS_ERROR_STREAM("deceleration limit not set for group " << jmg->getName()); - throw TrajectoryGeneratorInvalidLimitsException("deceleration limit not set for group " + jmg->getName()); - } + throw TrajectoryGeneratorInvalidLimitsException("deceleration limit not set for group " + group_name); most_strict_limits_.insert(std::pair(jmg->getName(), most_strict_limit)); } From a6ec352cec488c1433a97c38e12a3ab6716bf34c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 2 Nov 2021 22:31:13 +0100 Subject: [PATCH 048/229] Don't complain about missing limits for irrelevant JMGs When planning an arm motion, Pilz's PTP planner shouldn't complain (and bail out) on missing joint limits of hand joints! --- .../planning_context_base.h | 2 +- .../trajectory_generator_circ.h | 3 +- .../trajectory_generator_lin.h | 3 +- .../trajectory_generator_ptp.h | 12 +++--- .../src/trajectory_generator_circ.cpp | 3 +- .../src/trajectory_generator_lin.cpp | 2 +- .../src/trajectory_generator_ptp.cpp | 38 +++++++++---------- ...t_trajectory_blender_transition_window.cpp | 2 +- .../unittest_trajectory_generator_circ.cpp | 5 ++- .../unittest_trajectory_generator_common.cpp | 3 +- .../unittest_trajectory_generator_lin.cpp | 4 +- .../unittest_trajectory_generator_ptp.cpp | 17 +++++---- 12 files changed, 49 insertions(+), 45 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h index 86fb36dbaf..7357c8bd4d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h @@ -63,7 +63,7 @@ class PlanningContextBase : public planning_interface::PlanningContext , terminated_(false) , model_(model) , limits_(limits) - , generator_(model, limits_) + , generator_(model, limits_, group) { } diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h index 13dda37739..e0adf32196 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h @@ -79,7 +79,8 @@ class TrajectoryGeneratorCIRC : public TrajectoryGenerator * */ TrajectoryGeneratorCIRC(const robot_model::RobotModelConstPtr& robot_model, - const pilz_industrial_motion_planner::LimitsContainer& planner_limits); + const pilz_industrial_motion_planner::LimitsContainer& planner_limits, + const std::string& group_name); private: void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const override; diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h index 269c9c0151..ffc881deb9 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h @@ -68,7 +68,8 @@ class TrajectoryGeneratorLIN : public TrajectoryGenerator * @param planner_limits: limits in joint and Cartesian spaces */ TrajectoryGeneratorLIN(const robot_model::RobotModelConstPtr& robot_model, - const pilz_industrial_motion_planner::LimitsContainer& planner_limits); + const pilz_industrial_motion_planner::LimitsContainer& planner_limits, + const std::string& group_name); private: void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h index 2ec0097ffb..3df3cd1ab9 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h @@ -63,7 +63,8 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator * @param model: a map of joint limits information */ TrajectoryGeneratorPTP(const robot_model::RobotModelConstPtr& robot_model, - const pilz_industrial_motion_planner::LimitsContainer& planner_limits); + const pilz_industrial_motion_planner::LimitsContainer& planner_limits, + const std::string& group_name); private: void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, @@ -80,9 +81,8 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator * @param sampling_time */ void planPTP(const std::map& start_pos, const std::map& goal_pos, - trajectory_msgs::JointTrajectory& joint_trajectory, const std::string& group_name, - const double& velocity_scaling_factor, const double& acceleration_scaling_factor, - const double& sampling_time); + trajectory_msgs::JointTrajectory& joint_trajectory, const double& velocity_scaling_factor, + const double& acceleration_scaling_factor, const double& sampling_time); void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, const double& sampling_time, @@ -91,8 +91,8 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator private: const double MIN_MOVEMENT = 0.001; pilz_industrial_motion_planner::JointLimitsContainer joint_limits_; - // most strict joint limits for each group - std::map most_strict_limits_; + // most strict joint limits + JointLimit most_strict_limit_; }; } // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index f062677502..44115f6e2e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -51,7 +51,8 @@ namespace pilz_industrial_motion_planner { TrajectoryGeneratorCIRC::TrajectoryGeneratorCIRC(const moveit::core::RobotModelConstPtr& robot_model, - const LimitsContainer& planner_limits) + const LimitsContainer& planner_limits, + const std::string& /*group_name*/) : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) { if (!planner_limits_.hasFullCartesianLimits()) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index 40eadac63b..dede65781f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -52,7 +52,7 @@ namespace pilz_industrial_motion_planner { TrajectoryGeneratorLIN::TrajectoryGeneratorLIN(const moveit::core::RobotModelConstPtr& robot_model, - const LimitsContainer& planner_limits) + const LimitsContainer& planner_limits, const std::string& /*group_name*/) : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) { if (!planner_limits_.hasFullCartesianLimits()) 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 3649916b3e..d1dc58f1c2 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 @@ -45,7 +45,7 @@ namespace pilz_industrial_motion_planner { TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const robot_model::RobotModelConstPtr& robot_model, - const LimitsContainer& planner_limits) + const LimitsContainer& planner_limits, const std::string& group_name) : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) { if (!planner_limits_.hasJointLimits()) @@ -56,26 +56,23 @@ TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const robot_model::RobotModelCons joint_limits_ = planner_limits_.getJointLimitContainer(); // collect most strict joint limits for each group in robot model - for (const auto& jmg : robot_model->getJointModelGroups()) - { - const auto& active_joints = jmg->getActiveJointModelNames(); + const auto* jmg = robot_model->getJointModelGroup(group_name); + if (!jmg) + throw TrajectoryGeneratorInvalidLimitsException("invalid group: " + group_name); - // no active joints - if (active_joints.empty()) - { - continue; - } + const auto& active_joints = jmg->getActiveJointModelNames(); - JointLimit most_strict_limit = joint_limits_.getCommonLimit(active_joints); + // no active joints + if (!active_joints.empty()) + { + most_strict_limit_ = joint_limits_.getCommonLimit(active_joints); - if (!most_strict_limit.has_velocity_limits) + if (!most_strict_limit_.has_velocity_limits) throw TrajectoryGeneratorInvalidLimitsException("velocity limit not set for group " + group_name); - if (!most_strict_limit.has_acceleration_limits) + if (!most_strict_limit_.has_acceleration_limits) throw TrajectoryGeneratorInvalidLimitsException("acceleration limit not set for group " + group_name); - if (!most_strict_limit.has_deceleration_limits) + if (!most_strict_limit_.has_deceleration_limits) throw TrajectoryGeneratorInvalidLimitsException("deceleration limit not set for group " + group_name); - - most_strict_limits_.insert(std::pair(jmg->getName(), most_strict_limit)); } ROS_INFO("Initialized Point-to-Point Trajectory Generator."); @@ -83,7 +80,7 @@ TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const robot_model::RobotModelCons void TrajectoryGeneratorPTP::planPTP(const std::map& start_pos, const std::map& goal_pos, - trajectory_msgs::JointTrajectory& joint_trajectory, const std::string& group_name, + trajectory_msgs::JointTrajectory& joint_trajectory, const double& velocity_scaling_factor, const double& acceleration_scaling_factor, const double& sampling_time) { @@ -130,10 +127,9 @@ void TrajectoryGeneratorPTP::planPTP(const std::map& start_ { // create vecocity profile if necessary velocity_profile.insert(std::make_pair( - joint_name, - VelocityProfileATrap(velocity_scaling_factor * most_strict_limits_.at(group_name).max_velocity, - acceleration_scaling_factor * most_strict_limits_.at(group_name).max_acceleration, - acceleration_scaling_factor * most_strict_limits_.at(group_name).max_deceleration))); + joint_name, VelocityProfileATrap(velocity_scaling_factor * most_strict_limit_.max_velocity, + acceleration_scaling_factor * most_strict_limit_.max_acceleration, + acceleration_scaling_factor * most_strict_limit_.max_deceleration))); velocity_profile.at(joint_name).SetProfile(start_pos.at(joint_name), goal_pos.at(joint_name)); if (velocity_profile.at(joint_name).Duration() > max_duration) @@ -253,7 +249,7 @@ void TrajectoryGeneratorPTP::plan(const planning_scene::PlanningSceneConstPtr& / const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) { // plan the ptp trajectory - planPTP(plan_info.start_joint_position, plan_info.goal_joint_position, joint_trajectory, plan_info.group_name, + planPTP(plan_info.start_joint_position, plan_info.goal_joint_position, joint_trajectory, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, sampling_time); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp index 4c97843b9f..3a685790e0 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp @@ -137,7 +137,7 @@ void TrajectoryBlenderTransitionWindowTest::SetUp() planner_limits_.setCartesianLimits(cart_limits); // initialize trajectory generators and blender - lin_generator_ = std::make_unique(robot_model_, planner_limits_); + lin_generator_ = std::make_unique(robot_model_, planner_limits_, planning_group_); ASSERT_NE(nullptr, lin_generator_) << "failed to create LIN trajectory generator"; blender_ = std::make_unique(planner_limits_); ASSERT_NE(nullptr, blender_) << "failed to create trajectory blender"; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp index 5b0924c270..9783bf36d7 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp @@ -141,7 +141,7 @@ void TrajectoryGeneratorCIRCTest::SetUp() planner_limits_.setCartesianLimits(cart_limits); // initialize the LIN trajectory generator - circ_ = std::make_unique(robot_model_, planner_limits_); + circ_ = std::make_unique(robot_model_, planner_limits_, planning_group_); ASSERT_NE(nullptr, circ_) << "failed to create CIRC trajectory generator"; } @@ -284,7 +284,8 @@ INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryGeneratorCIRCTest, TEST_P(TrajectoryGeneratorCIRCTest, noLimits) { LimitsContainer planner_limits; - EXPECT_THROW(TrajectoryGeneratorCIRC(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException); + EXPECT_THROW(TrajectoryGeneratorCIRC(this->robot_model_, planner_limits, planning_group_), + TrajectoryGeneratorInvalidLimitsException); } /** diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp index 7a50c654f6..8350ef6fc2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp @@ -119,7 +119,8 @@ class TrajectoryGeneratorCommonTest : public ::testing::Test planner_limits.setCartesianLimits(cart_limits); // create planner instance - trajectory_generator_ = std::unique_ptr(new typename T::Type_(robot_model_, planner_limits)); + trajectory_generator_ = + std::unique_ptr(new typename T::Type_(robot_model_, planner_limits, planning_group_)); ASSERT_NE(nullptr, trajectory_generator_) << "failed to create trajectory generator"; // create a valid motion plan request with goal in joint space as basis for diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp index 4b8edf69a6..fb12a97f04 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp @@ -142,7 +142,7 @@ void TrajectoryGeneratorLINTest::SetUp() planner_limits_.setCartesianLimits(cart_limits); // initialize the LIN trajectory generator - lin_ = std::make_unique(robot_model_, planner_limits_); + lin_ = std::make_unique(robot_model_, planner_limits_, planning_group_); ASSERT_NE(nullptr, lin_) << "Failed to create LIN trajectory generator."; } @@ -394,7 +394,7 @@ TEST_P(TrajectoryGeneratorLINTest, CtorNoLimits) { pilz_industrial_motion_planner::LimitsContainer planner_limits; - EXPECT_THROW(pilz_industrial_motion_planner::TrajectoryGeneratorLIN(robot_model_, planner_limits), + EXPECT_THROW(pilz_industrial_motion_planner::TrajectoryGeneratorLIN(robot_model_, planner_limits, planning_group_), pilz_industrial_motion_planner::TrajectoryGeneratorInvalidLimitsException); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp index 80a16e7577..2ab940c7ae 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp @@ -127,7 +127,7 @@ void TrajectoryGeneratorPTPTest::SetUp() // create the trajectory generator planner_limits_.setJointLimits(joint_limits); - ptp_ = std::make_unique(robot_model_, planner_limits_); + ptp_ = std::make_unique(robot_model_, planner_limits_, planning_group_); ASSERT_NE(nullptr, ptp_); } @@ -169,7 +169,8 @@ INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryGeneratorPTPTest, TEST_P(TrajectoryGeneratorPTPTest, noLimits) { LimitsContainer planner_limits; - EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException); + EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits, planning_group_), + TrajectoryGeneratorInvalidLimitsException); } /** @@ -221,7 +222,8 @@ TEST_P(TrajectoryGeneratorPTPTest, missingVelocityLimits) } planner_limits.setJointLimits(joint_limits); - EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException); + EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits, planning_group_), + TrajectoryGeneratorInvalidLimitsException); } /** @@ -244,7 +246,8 @@ TEST_P(TrajectoryGeneratorPTPTest, missingDecelerationimits) } planner_limits.setJointLimits(joint_limits); - EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException); + EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits, planning_group_), + TrajectoryGeneratorInvalidLimitsException); } /** @@ -287,7 +290,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testInsufficientLimit) EXPECT_THROW( { std::unique_ptr ptp_error( - new TrajectoryGeneratorPTP(robot_model_, insufficient_planner_limits)); + new TrajectoryGeneratorPTP(robot_model_, insufficient_planner_limits, planning_group_)); }, TrajectoryGeneratorInvalidLimitsException); @@ -326,7 +329,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testInsufficientLimit) EXPECT_NO_THROW({ std::unique_ptr ptp_no_error( - new TrajectoryGeneratorPTP(robot_model_, sufficient_planner_limits)); + new TrajectoryGeneratorPTP(robot_model_, sufficient_planner_limits, planning_group_)); }); } @@ -517,7 +520,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testScalingFactor) planner_limits.setJointLimits(joint_limits); // create the generator with new limits - ptp_ = std::make_unique(robot_model_, planner_limits); + ptp_ = std::make_unique(robot_model_, planner_limits, planning_group_); planning_interface::MotionPlanResponse res; planning_interface::MotionPlanRequest req; From 38e93f783312d74594dc51bf6c224e72d452a634 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 3 Nov 2021 14:22:49 +0100 Subject: [PATCH 049/229] Fix unittests by providing a valid JMG --- .../test/unittest_pilz_industrial_motion_planner.cpp | 1 + .../test/unittest_planning_context.cpp | 2 +- .../test/unittest_planning_context_loaders.cpp | 5 +++-- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.cpp index cb8ac155c7..b7e8d72af2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.cpp @@ -203,6 +203,7 @@ TEST_P(CommandPlannerTest, CheckPlanningContextRequest) moveit_msgs::MotionPlanRequest req; moveit_msgs::MoveItErrorCodes error_code; + req.group_name = "manipulator"; // Check for the algorithms std::vector algs; planner_instance_->getPlanningAlgorithms(algs); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp index 016c612e0a..48b959960b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp @@ -113,7 +113,7 @@ class PlanningContextTest : public ::testing::Test limits.setCartesianLimits(cartesian_limit); planning_context_ = std::unique_ptr( - new typename T::Type_("TestPlanningContext", "TestGroup", robot_model_, limits)); + new typename T::Type_("TestPlanningContext", planning_group_, robot_model_, limits)); // Define and set the current scene planning_scene::PlanningScenePtr scene(new planning_scene::PlanningScene(robot_model_)); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.cpp index c53c5c2242..885eda3000 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.cpp @@ -139,9 +139,10 @@ TEST_P(PlanningContextLoadersTest, GetAlgorithm) TEST_P(PlanningContextLoadersTest, LoadContext) { planning_interface::PlanningContextPtr planning_context; + const std::string& group_name = "manipulator"; // Without limits should return false - bool res = planning_context_loader_->loadContext(planning_context, "test", "test"); + bool res = planning_context_loader_->loadContext(planning_context, "test", group_name); EXPECT_EQ(false, res) << "Context returned even when no limits where set"; // After setting the limits this should work @@ -161,7 +162,7 @@ TEST_P(PlanningContextLoadersTest, LoadContext) try { - res = planning_context_loader_->loadContext(planning_context, "test", "test"); + res = planning_context_loader_->loadContext(planning_context, "test", group_name); } catch (std::exception& ex) { From 8f61377b43603d578740a81dffb7a125572264fe Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 3 Nov 2021 14:23:10 +0100 Subject: [PATCH 050/229] Remove deprecated xacro --inorder --- .../test/test_robots/prbt/launch/test_context.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch index 49ac94b65d..aa1ef5a82e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch @@ -10,10 +10,10 @@ + command="$(find xacro)/xacro $(find moveit_resources_prbt_support)/urdf/prbt.xacro gripper:=$(arg gripper)"/> - From 18cfe72775d07f3ccf115b440fe9cf721cac2eec Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 3 Nov 2021 14:24:11 +0100 Subject: [PATCH 051/229] Fix typo: demangel -> demangle --- .../test/test_utils.h | 2 +- .../test/unittest_planning_context.cpp | 30 +++++++++---------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h index dd5d06e6b9..e9de944e41 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h @@ -89,7 +89,7 @@ inline std::string getJointName(size_t joint_number, const std::string& joint_pr */ pilz_industrial_motion_planner::JointLimitsContainer createFakeLimits(const std::vector& joint_names); -inline std::string demangel(char const* name) +inline std::string demangle(char const* name) { return boost::core::demangle(name); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp index 48b959960b..fecbcc27bd 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp @@ -199,9 +199,9 @@ TYPED_TEST(PlanningContextTest, NoRequest) planning_interface::MotionPlanResponse res; bool result = this->planning_context_->solve(res); - EXPECT_FALSE(result) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_FALSE(result) << testutils::demangle(typeid(TypeParam).name()); EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN, res.error_code_.val) - << testutils::demangel(typeid(TypeParam).name()); + << testutils::demangle(typeid(TypeParam).name()); } /** @@ -210,23 +210,23 @@ TYPED_TEST(PlanningContextTest, NoRequest) TYPED_TEST(PlanningContextTest, SolveValidRequest) { planning_interface::MotionPlanResponse res; - planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangel(typeid(TypeParam).name())); + planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangle(typeid(TypeParam).name())); this->planning_context_->setMotionPlanRequest(req); // TODO Formulate valid request bool result = this->planning_context_->solve(res); - EXPECT_TRUE(result) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_TRUE(result) << testutils::demangle(typeid(TypeParam).name()); EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, res.error_code_.val) - << testutils::demangel(typeid(TypeParam).name()); + << testutils::demangle(typeid(TypeParam).name()); planning_interface::MotionPlanDetailedResponse res_detailed; bool result_detailed = this->planning_context_->solve(res_detailed); - EXPECT_TRUE(result_detailed) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_TRUE(result_detailed) << testutils::demangle(typeid(TypeParam).name()); EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, res.error_code_.val) - << testutils::demangel(typeid(TypeParam).name()); + << testutils::demangle(typeid(TypeParam).name()); } /** @@ -235,14 +235,14 @@ TYPED_TEST(PlanningContextTest, SolveValidRequest) TYPED_TEST(PlanningContextTest, SolveValidRequestDetailedResponse) { planning_interface::MotionPlanDetailedResponse res; //<-- Detailed! - planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangel(typeid(TypeParam).name())); + planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangle(typeid(TypeParam).name())); this->planning_context_->setMotionPlanRequest(req); bool result = this->planning_context_->solve(res); - EXPECT_TRUE(result) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_TRUE(result) << testutils::demangle(typeid(TypeParam).name()); EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, res.error_code_.val) - << testutils::demangel(typeid(TypeParam).name()); + << testutils::demangle(typeid(TypeParam).name()); } /** @@ -251,18 +251,18 @@ TYPED_TEST(PlanningContextTest, SolveValidRequestDetailedResponse) TYPED_TEST(PlanningContextTest, SolveOnTerminated) { planning_interface::MotionPlanResponse res; - planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangel(typeid(TypeParam).name())); + planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangle(typeid(TypeParam).name())); this->planning_context_->setMotionPlanRequest(req); bool result_termination = this->planning_context_->terminate(); - EXPECT_TRUE(result_termination) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_TRUE(result_termination) << testutils::demangle(typeid(TypeParam).name()); bool result = this->planning_context_->solve(res); - EXPECT_FALSE(result) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_FALSE(result) << testutils::demangle(typeid(TypeParam).name()); EXPECT_EQ(moveit_msgs::MoveItErrorCodes::PLANNING_FAILED, res.error_code_.val) - << testutils::demangel(typeid(TypeParam).name()); + << testutils::demangle(typeid(TypeParam).name()); } /** @@ -270,7 +270,7 @@ TYPED_TEST(PlanningContextTest, SolveOnTerminated) */ TYPED_TEST(PlanningContextTest, Clear) { - EXPECT_NO_THROW(this->planning_context_->clear()) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_NO_THROW(this->planning_context_->clear()) << testutils::demangle(typeid(TypeParam).name()); } int main(int argc, char** argv) From 0d0a6dd686541177f5baedaca7771e58594670a4 Mon Sep 17 00:00:00 2001 From: Jonathan Grebe Date: Wed, 3 Nov 2021 11:57:28 -0400 Subject: [PATCH 052/229] Drop the minimum velocity/acceleration limits for TOTG (#2937) Just complain about negative / zero values. --- .../src/time_optimal_trajectory_generation.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) 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 ff1f0e8308..299d080905 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -932,17 +932,27 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT max_velocity[j] = 1.0; if (bounds.velocity_bounded_) { + if (bounds.max_velocity_ < std::numeric_limits::epsilon()) + { + ROS_ERROR_NAMED(LOGNAME, "Invalid max_velocity %f specified for '%s', must be greater than 0.0", + bounds.max_velocity_, vars[j].c_str()); + return false; + } 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_) { + if (bounds.max_acceleration_ < std::numeric_limits::epsilon()) + { + ROS_ERROR_NAMED(LOGNAME, "Invalid max_acceleration %f specified for '%s', must be greater than 0.0", + bounds.max_acceleration_, vars[j].c_str()); + return false; + } 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]); } } From 8992f16a779391fbac1c9720bd30d6bbb9d66760 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 3 Nov 2021 15:30:20 +0100 Subject: [PATCH 053/229] CI: Explicitly add always() to if conditions Otherwise, on a failure, the implicitly used success() will skip the step. --- .github/workflows/ci.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 05cc19aadb..9909cc1b62 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -123,24 +123,24 @@ jobs: path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml - name: Generate codecov report uses: rhaschke/lcov-action@main - if: matrix.env.CCOV && steps.ici.outputs.target_test_results == '0' + if: always() && matrix.env.CCOV && steps.ici.outputs.target_test_results == '0' with: docker: $DOCKER_IMAGE workdir: ${{ env.BASEDIR }}/target_ws ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"' - name: Upload codecov report uses: codecov/codecov-action@v2 - if: matrix.env.CCOV && steps.ici.outputs.target_test_results == '0' + if: always() && matrix.env.CCOV && steps.ici.outputs.target_test_results == '0' with: files: ${{ env.BASEDIR }}/target_ws/coverage.info - name: Upload clang-tidy changes uses: rhaschke/upload-git-patch-action@main - if: matrix.env.CLANG_TIDY + if: always() && matrix.env.CLANG_TIDY with: name: clang-tidy path: ${{ env.BASEDIR }}/target_ws/src/$(basename $(pwd)) - name: Prepare target_ws for cache - if: "!matrix.env.CCOV" + if: always() && !matrix.env.CCOV run: | du -sh ${{ env.BASEDIR }}/target_ws sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete From 8d2f96120662730c69b3de2c829e72b6f6259815 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 3 Nov 2021 18:25:53 +0100 Subject: [PATCH 054/229] Reset markers on display_contacts topic after a new planning attempt --- .../planning/planning_pipeline/src/planning_pipeline.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 0558a605ab..07cb9874c1 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -264,6 +264,11 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla ROS_DEBUG_STREAM("Motion planner reported a solution path with " << state_count << " states"); if (check_solution_paths_) { + visualization_msgs::MarkerArray arr; + visualization_msgs::Marker m; + m.action = visualization_msgs::Marker::DELETEALL; + arr.markers.push_back(m); + std::vector index; if (!planning_scene->isPathValid(*res.trajectory_, req.path_constraints, req.group_name, false, &index)) { @@ -301,7 +306,6 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla << private_nh_.resolveName(MOTION_CONTACTS_TOPIC)); // call validity checks in verbose mode for the problematic states - visualization_msgs::MarkerArray arr; for (std::size_t it : index) { // check validity with verbose on @@ -325,8 +329,6 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla } } ROS_ERROR_STREAM("Completed listing of explanations for invalid states."); - if (!arr.markers.empty()) - contacts_publisher_.publish(arr); } } else @@ -335,6 +337,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla } else ROS_DEBUG("Planned path was found to be valid when rechecked"); + contacts_publisher_.publish(arr); } } From 1d2e28ad7864b104ec9b0116afaf2fb47d87e9a1 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 23 Oct 2021 07:46:47 +0200 Subject: [PATCH 055/229] Silent warning about virtual_joint in Gazebo setups Gazebo requires a fixed joint from world to the first robot link. This resembles the virtual_joint of SRDF. However, the RobotModel parser issues the following warning: Skipping virtual joint 'xxx' because its child frame 'xxx' does not match the URDF frame 'world' --- moveit_core/robot_model/src/robot_model.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index ab666581e1..16d0ce7e65 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -922,10 +922,16 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const { if (virtual_joint.child_link_ != child_link->name) { - ROS_WARN_NAMED(LOGNAME, - "Skipping virtual joint '%s' because its child frame '%s' " - "does not match the URDF frame '%s'", - virtual_joint.name_.c_str(), virtual_joint.child_link_.c_str(), child_link->name.c_str()); + if (child_link->name == "world" && virtual_joint.type_ == "fixed" && child_link->collision_array.empty() && + !child_link->collision && child_link->visual_array.empty() && !child_link->visual) + // Gazebo requires a fixed link from a dummy world link to the first robot's link + // Skip warning in this case and create a fixed joint with given name + new_joint_model = new FixedJointModel(virtual_joint.name_); + else + ROS_WARN_NAMED(LOGNAME, + "Skipping virtual joint '%s' because its child frame '%s' " + "does not match the URDF frame '%s'", + virtual_joint.name_.c_str(), virtual_joint.child_link_.c_str(), child_link->name.c_str()); } else if (virtual_joint.parent_frame_.empty()) { From 7194e26222f3d522c7ac6315b6e9e740aaa69e2b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 23 Oct 2021 11:00:58 +0200 Subject: [PATCH 056/229] Remove execution_type argument from real-robot controller_manager.launch --- .../launch/moveit_controller_manager.launch.xml | 3 --- 1 file changed, 3 deletions(-) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_controller_manager.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_controller_manager.launch.xml index bc9b2de0bc..67cb8aa108 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_controller_manager.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_controller_manager.launch.xml @@ -1,8 +1,5 @@ - - - From b4d485aa377ea64e5dc714d1ceac84dceb509b40 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 23 Oct 2021 11:11:19 +0200 Subject: [PATCH 057/229] Fix definition of real-robot moveit_controller_manager Fixes the following error (occurring since 61d18f2f073aa4c8a13c2278c41a63591d401c4a) ``` [FATAL] ros.moveit_ros_planning.trajectory_execution_manager: Exception while loading controller manager 'robot': According to the loaded plugin descriptions the class robot with base class type moveit_controller_manager::MoveItControllerManager does not exist. ``` As we introduced `pass_all_args="true"`, the value of the argument `moveit_controller_manager` was the robot name. --- .../launch/moveit_controller_manager.launch.xml | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_controller_manager.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_controller_manager.launch.xml index 67cb8aa108..b3959b8630 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_controller_manager.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_controller_manager.launch.xml @@ -1,10 +1,7 @@ + + - - - - - + From 19ab016016c34d146e922e1f774c5081f9eaab87 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 09:47:28 +0100 Subject: [PATCH 058/229] Rework moveit_controller_manager handling There are 3 basic MoveIt controller manager plugins: - fake = `moveit_fake_controller_manager::MoveItFakeControllerManager` Used in demo.launch. Doesn't really control the robot, but just interpolates between via points. Allows these execution_types: - via points: just jumps to the via points - interpolate: linearly interpolates between via points (default) - last point: jumps to the final trajectory point (used for fast execution testing) - ros_control = `moveit_ros_control_interface::MoveItControllerManager` Interfaces to ros_control controllers. - simple = `moveit_simple_controller_manager/MoveItSimpleControllerManager` Interfaces to action servers for `FollowJointTrajectory` and/or `GripperCommand` that in turn interface to the low-level robot controllers (typically based on ros_control) However, so far move_group.launch distinguished between `fake` and `robot` only. The argument moveit_controller_manager now allows switching between all 3 variants. Adding more *_moveit_controller_manager.launch files allows for an extension of this scheme. --- .../widgets/configuration_files_widget.cpp | 30 +++++++++++-------- .../launch/demo.launch | 4 ++- .../launch/move_group.launch | 7 ++--- ...ntrol_moveit_controller_manager.launch.xml | 4 +++ ...mple_moveit_controller_manager.launch.xml} | 6 ++-- .../launch/trajectory_execution.launch.xml | 7 ++--- 6 files changed, 34 insertions(+), 24 deletions(-) create mode 100644 moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ros_control_moveit_controller_manager.launch.xml rename moveit_setup_assistant/templates/moveit_config_pkg_template/launch/{moveit_controller_manager.launch.xml => simple_moveit_controller_manager.launch.xml} (55%) diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 5e052dc1ac..43e8e08b7a 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -469,15 +469,6 @@ bool ConfigurationFilesWidget::loadGenFiles() file.write_on_changes = 0; gen_files_.push_back(file); - // robot_moveit_controller_manager.launch ------------------------------------------------------------------ - file.file_name_ = robot_name + "_moveit_controller_manager.launch.xml"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, "moveit_controller_manager.launch.xml"); - file.description_ = "Placeholder for settings specific to the MoveIt controller manager implemented for you robot."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); - file.write_on_changes = 0; - gen_files_.push_back(file); - // robot_moveit_sensor_manager.launch ------------------------------------------------------------------ file.file_name_ = robot_name + "_moveit_sensor_manager.launch.xml"; file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); @@ -495,13 +486,28 @@ bool ConfigurationFilesWidget::loadGenFiles() "trajectory_execution_manager::TrajectoryExecutionManager."; file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); file.write_on_changes = 0; - gen_files_.push_back( - file); // trajectory_execution.launch ------------------------------------------------------------------ + gen_files_.push_back(file); file.file_name_ = "fake_moveit_controller_manager.launch.xml"; file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Loads a fake controller plugin."; + file.description_ = "Loads the fake controller plugin."; + file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.write_on_changes = 0; + gen_files_.push_back(file); + + file.file_name_ = "simple_moveit_controller_manager.launch.xml"; + file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); + template_path = config_data_->appendPaths(template_launch_path, file.file_name_); + file.description_ = "Loads the default controller plugin."; + file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.write_on_changes = 0; + gen_files_.push_back(file); + + file.file_name_ = "ros_control_moveit_controller_manager.launch.xml"; + file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); + template_path = config_data_->appendPaths(template_launch_path, file.file_name_); + file.description_ = "Loads the ros_control controller plugin."; file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); file.write_on_changes = 0; gen_files_.push_back(file); diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch index 56fcd173eb..5b71919cc5 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch @@ -14,6 +14,8 @@ + + @@ -45,7 +47,7 @@ - + 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 bfe6301154..4336184fcf 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 @@ -14,8 +14,8 @@ - - + + @@ -70,8 +70,7 @@ - - + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ros_control_moveit_controller_manager.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ros_control_moveit_controller_manager.launch.xml new file mode 100644 index 0000000000..9ebc91c108 --- /dev/null +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ros_control_moveit_controller_manager.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_controller_manager.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/simple_moveit_controller_manager.launch.xml similarity index 55% rename from moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_controller_manager.launch.xml rename to moveit_setup_assistant/templates/moveit_config_pkg_template/launch/simple_moveit_controller_manager.launch.xml index b3959b8630..dec8cff842 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_controller_manager.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/simple_moveit_controller_manager.launch.xml @@ -1,7 +1,7 @@ - + - - + + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml index 1efc010d48..e6d23c178b 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml @@ -1,7 +1,8 @@ + - - + + @@ -15,8 +16,6 @@ - - From 08fe55dbbbe7eb9e19f6a70d86c6e29bc127e9e2 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 12:12:50 +0100 Subject: [PATCH 059/229] Cleanup generation of ros_controllers.yaml --- .../src/tools/moveit_config_data.cpp | 37 ++++++------------- 1 file changed, 12 insertions(+), 25 deletions(-) diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index e90d555edb..4f4f3fe80b 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -826,16 +826,16 @@ std::vector MoveItConfigData::getOMPLPlanners() // which are published under the namespace of 'controller_list' and other types of controllers. // ****************************************************************************************** void MoveItConfigData::outputFollowJointTrajectoryYAML(YAML::Emitter& emitter, - std::vector& ros_controllers_config_output) + std::vector& ros_controllers_configs) { // Write default controllers emitter << YAML::Key << "controller_list"; emitter << YAML::Value << YAML::BeginSeq; { - for (std::vector::iterator controller_it = ros_controllers_config_output.begin(); - controller_it != ros_controllers_config_output.end();) + for (std::vector::iterator controller_it = ros_controllers_configs.begin(); + controller_it != ros_controllers_configs.end();) { - // Depending on the controller type, fill the required data + // Only process FollowJointTrajectory types if (controller_it->type_ == "FollowJointTrajectory") { emitter << YAML::BeginMap; @@ -849,27 +849,15 @@ void MoveItConfigData::outputFollowJointTrajectoryYAML(YAML::Emitter& emitter, emitter << YAML::Value << controller_it->type_; // Write joints emitter << YAML::Key << "joints"; - { - if (controller_it->joints_.size() != 1) - { - emitter << YAML::Value << YAML::BeginSeq; - // Iterate through the joints - for (std::string& joint : controller_it->joints_) - { - emitter << joint; - } - emitter << YAML::EndSeq; - } - else - { - emitter << YAML::Value << YAML::BeginMap; - emitter << controller_it->joints_[0]; - emitter << YAML::EndMap; - } - } - controller_it = ros_controllers_config_output.erase(controller_it); + emitter << YAML::Value << YAML::BeginSeq; + // Iterate through the joints + for (std::string& joint : controller_it->joints_) + emitter << joint; + emitter << YAML::EndSeq; + emitter << YAML::EndMap; + controller_it = ros_controllers_configs.erase(controller_it); } else { @@ -901,12 +889,12 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) // Cache the joints' names. std::vector> planning_groups; - std::vector group_joints; // We are going to write the joints names many times. // Loop through groups to store the joints names in group_joints vector and reuse is. for (srdf::Model::Group& group : srdf_->groups_) { + std::vector group_joints; // Get list of associated joints const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_); const std::vector& joint_models = joint_model_group->getActiveJointModels(); @@ -920,7 +908,6 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) } // Push all the group joints into planning_groups vector. planning_groups.push_back(group_joints); - group_joints.clear(); } YAML::Emitter emitter; From 6779c14f241a6f44acffd854156b766ab7e6ebaa Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 21:58:28 +0100 Subject: [PATCH 060/229] Fix handling of sensors_3d.yaml - Reading both, the default and the existing package's sensors_3d.yaml into the config, the config file was growing by 2 configs each time. - Not visiting the Perception tab, was writing the default config with 2 entries - Selecting "None" was writing an invalid config: sensors: - {} - {} --- .../tools/moveit_config_data.h | 25 ++-- .../src/tools/moveit_config_data.cpp | 107 +++++------------- .../src/widgets/perception_widget.cpp | 78 ++++++------- .../src/widgets/perception_widget.h | 1 + .../src/widgets/start_screen_widget.cpp | 15 +-- .../test/moveit_config_data_test.cpp | 33 +++--- 6 files changed, 94 insertions(+), 165 deletions(-) diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h index 4601e24d65..c28ea9dbcf 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h @@ -155,15 +155,15 @@ class GenericParameter { comment_ = std::move(comment); }; - std::string getName() + const std::string& getName() const { return name_; }; - std::string getValue() + const std::string& getValue() const { return value_; }; - std::string getComment() + const std::string& getComment() const { return comment_; }; @@ -437,14 +437,10 @@ class MoveItConfigData */ bool inputSetupAssistantYAML(const std::string& file_path); - /** - * Input sensors_3d file - contains 3d sensors config data - * - * @param default_file_path path to sensors_3d yaml file which contains default parameter values - * @param file_path path to sensors_3d yaml file in the config package - * @return true if the file was read correctly - */ - bool input3DSensorsYAML(const std::string& default_file_path, const std::string& file_path = ""); + /// Load perception sensor config (sensors_3d.yaml) into internal data structure + void input3DSensorsYAML(const std::string& file_path); + /// Load perception sensor config + static std::vector> load3DSensorsYAML(const std::string& file_path); /** * Helper Function for joining a file path and a file name, or two file paths, etc, @@ -499,7 +495,10 @@ class MoveItConfigData /** * \brief Used for adding a sensor plugin configuation parameter to the sensor plugin configuration parameter list */ - std::vector > getSensorPluginConfig(); + const std::vector>& getSensorPluginConfig() const + { + return sensors_plugin_config_parameter_list_; + } /** * \brief Helper function to get the default start pose for moveit_sim_hw_interface @@ -526,7 +525,7 @@ class MoveItConfigData // ****************************************************************************************** /// Sensor plugin configuration parameter list, each sensor plugin type is a map - std::vector > sensors_plugin_config_parameter_list_; + std::vector> sensors_plugin_config_parameter_list_; /// Shared kinematic model moveit::core::RobotModelPtr robot_model_; diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index e90d555edb..709acda99d 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -1074,9 +1074,8 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) bool MoveItConfigData::output3DSensorPluginYAML(const std::string& file_path) { YAML::Emitter emitter; - emitter << YAML::BeginMap; - emitter << YAML::Comment("The name of this file shouldn't be changed, or else the Setup Assistant won't detect it"); + emitter << YAML::BeginMap; emitter << YAML::Key << "sensors"; emitter << YAML::Value << YAML::BeginSeq; @@ -1776,66 +1775,28 @@ bool MoveItConfigData::inputSetupAssistantYAML(const std::string& file_path) // ****************************************************************************************** // Input sensors_3d yaml file // ****************************************************************************************** -bool MoveItConfigData::input3DSensorsYAML(const std::string& default_file_path, const std::string& file_path) +void MoveItConfigData::input3DSensorsYAML(const std::string& file_path) { - // Load default parameters file - std::ifstream default_input_stream(default_file_path.c_str()); - if (!default_input_stream.good()) - { - ROS_ERROR_STREAM_NAMED("sensors_3d.yaml", "Unable to open file for reading " << default_file_path); - return false; - } - - // Parse default parameters values - try - { - const YAML::Node& doc = YAML::Load(default_input_stream); - - // Get sensors node - if (const YAML::Node& sensors_node = doc["sensors"]) - { - // Make sue that the sensors are written as a sequence - if (sensors_node.IsSequence()) - { - GenericParameter sensor_param; - std::map sensor_map; - - // Loop over the sensors available in the file - for (const YAML::Node& sensor : sensors_node) - { - if (const YAML::Node& sensor_node = sensor) - { - for (YAML::const_iterator sensor_it = sensor_node.begin(); sensor_it != sensor_node.end(); ++sensor_it) - { - sensor_param.setName(sensor_it->first.as()); - sensor_param.setValue(sensor_it->second.as()); + sensors_plugin_config_parameter_list_ = load3DSensorsYAML(file_path); +} - // Set the key as the parameter name to make accessing it easier - sensor_map[sensor_it->first.as()] = sensor_param; - } - sensors_plugin_config_parameter_list_.push_back(sensor_map); - } - } - } - } - } - catch (YAML::ParserException& e) // Catch errors - { - ROS_ERROR_STREAM("Error parsing default sensors yaml: " << e.what()); - } +// ****************************************************************************************** +// Load sensors_3d.yaml file +// ****************************************************************************************** +std::vector> MoveItConfigData::load3DSensorsYAML(const std::string& file_path) +{ + std::vector> config; // Is there a sensors config in the package? if (file_path.empty()) - { - return true; - } + return config; // Load file std::ifstream input_stream(file_path.c_str()); if (!input_stream.good()) { ROS_ERROR_STREAM_NAMED("sensors_3d.yaml", "Unable to open file for reading " << file_path); - return false; + return config; } // Begin parsing @@ -1848,38 +1809,33 @@ bool MoveItConfigData::input3DSensorsYAML(const std::string& default_file_path, // Make sure that the sensors are written as a sequence if (sensors_node && sensors_node.IsSequence()) { - GenericParameter sensor_param; - std::map sensor_map; - bool empty_node = true; - // Loop over the sensors available in the file for (const YAML::Node& sensor : sensors_node) { - if (const YAML::Node& sensor_node = sensor) + std::map sensor_map; + bool empty_node = true; + for (YAML::const_iterator sensor_it = sensor.begin(); sensor_it != sensor.end(); ++sensor_it) { - for (YAML::const_iterator sensor_it = sensor_node.begin(); sensor_it != sensor_node.end(); ++sensor_it) - { - empty_node = false; - sensor_param.setName(sensor_it->first.as()); - sensor_param.setValue(sensor_it->second.as()); + empty_node = false; + GenericParameter sensor_param; + sensor_param.setName(sensor_it->first.as()); + sensor_param.setValue(sensor_it->second.as()); - // Set the key as the parameter name to make accessing it easier - sensor_map[sensor_it->first.as()] = sensor_param; - } - // Don't push empty nodes - if (!empty_node) - sensors_plugin_config_parameter_list_.push_back(sensor_map); + // Set the key as the parameter name to make accessing it easier + sensor_map[sensor_it->first.as()] = sensor_param; } + // Don't push empty nodes + if (!empty_node) + config.push_back(sensor_map); } } - return true; } catch (YAML::ParserException& e) // Catch errors { ROS_ERROR_STREAM("Error parsing sensors yaml: " << e.what()); } - return false; // if it gets to this point an error has occured + return config; } // ****************************************************************************************** @@ -1992,23 +1948,12 @@ void MoveItConfigData::addGenericParameterToSensorPluginConfig(const std::string sensors_plugin_config_parameter_list_[0][name] = new_parameter; } -// ****************************************************************************************** -// Used to get sensor plugin configuration parameter list -// ****************************************************************************************** -std::vector> MoveItConfigData::getSensorPluginConfig() -{ - return sensors_plugin_config_parameter_list_; -} - // ****************************************************************************************** // Used to clear sensor plugin configuration parameter list // ****************************************************************************************** void MoveItConfigData::clearSensorPluginConfig() { - for (std::map& param_id : sensors_plugin_config_parameter_list_) - { - param_id.clear(); - } + sensors_plugin_config_parameter_list_.clear(); } } // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/perception_widget.cpp b/moveit_setup_assistant/src/widgets/perception_widget.cpp index a907dba20a..9847979a3e 100644 --- a/moveit_setup_assistant/src/widgets/perception_widget.cpp +++ b/moveit_setup_assistant/src/widgets/perception_widget.cpp @@ -94,7 +94,6 @@ PerceptionWidget::PerceptionWidget(QWidget* parent, const MoveItConfigDataPtr& c // Point Cloud Topic point_cloud_topic_field_ = new QLineEdit(this); point_cloud_topic_field_->setMaximumWidth(400); - // point_cloud_topic_field_->setText(QString("/clud topic")); point_cloud_form_layout->addRow("Point Cloud Topic:", point_cloud_topic_field_); // Max Range @@ -284,6 +283,35 @@ void PerceptionWidget::sensorPluginChanged(int index) } } +uint PerceptionWidget::loadConfigIntoWidgets(std::map sensor_plugin_config) +{ + if (sensor_plugin_config["sensor_plugin"].getValue() == "occupancy_map_monitor/PointCloudOctomapUpdater") + { + point_cloud_topic_field_->setText(QString(sensor_plugin_config["point_cloud_topic"].getValue().c_str())); + max_range_field_->setText(QString(sensor_plugin_config["max_range"].getValue().c_str())); + point_subsample_field_->setText(QString(sensor_plugin_config["point_subsample"].getValue().c_str())); + padding_offset_field_->setText(QString(sensor_plugin_config["padding_offset"].getValue().c_str())); + padding_scale_field_->setText(QString(sensor_plugin_config["padding_scale"].getValue().c_str())); + max_update_rate_field_->setText(QString(sensor_plugin_config["max_update_rate"].getValue().c_str())); + filtered_cloud_topic_field_->setText(QString(sensor_plugin_config["filtered_cloud_topic"].getValue().c_str())); + return 1; + } + else if (sensor_plugin_config["sensor_plugin"].getValue() == "occupancy_map_monitor/DepthImageOctomapUpdater") + { + image_topic_field_->setText(QString(sensor_plugin_config["image_topic"].getValue().c_str())); + queue_size_field_->setText(QString(sensor_plugin_config["queue_size"].getValue().c_str())); + near_clipping_field_->setText(QString(sensor_plugin_config["near_clipping_plane_distance"].getValue().c_str())); + far_clipping_field_->setText(QString(sensor_plugin_config["far_clipping_plane_distance"].getValue().c_str())); + shadow_threshold_field_->setText(QString(sensor_plugin_config["shadow_threshold"].getValue().c_str())); + depth_padding_scale_field_->setText(QString(sensor_plugin_config["padding_scale"].getValue().c_str())); + depth_padding_offset_field_->setText(QString(sensor_plugin_config["padding_offset"].getValue().c_str())); + depth_filtered_cloud_topic_field_->setText(QString(sensor_plugin_config["filtered_cloud_topic"].getValue().c_str())); + depth_max_update_rate_field_->setText(QString(sensor_plugin_config["max_update_rate"].getValue().c_str())); + return 2; + } + return 0; +} + void PerceptionWidget::loadSensorPluginsComboBox() { // Only load this combo box once @@ -292,53 +320,25 @@ void PerceptionWidget::loadSensorPluginsComboBox() return; has_loaded = true; - // Remove all old items - sensor_plugin_field_->clear(); - // Add None option, the default sensor_plugin_field_->addItem("None"); + sensor_plugin_field_->setCurrentIndex(0); // Add the two avilable plugins to combo box sensor_plugin_field_->addItem("Point Cloud"); sensor_plugin_field_->addItem("Depth Map"); - // Load deafult config, or use the one in the config package if exists - std::vector > sensors_vec_map = config_data_->getSensorPluginConfig(); - for (std::map& sensor_plugin_config : sensors_vec_map) - { - if (sensor_plugin_config["sensor_plugin"].getValue() == - std::string("occupancy_map_monitor/PointCloudOctomapUpdater")) - { - sensor_plugin_field_->setCurrentIndex(1); - point_cloud_topic_field_->setText(QString(sensor_plugin_config["point_cloud_topic"].getValue().c_str())); - max_range_field_->setText(QString(sensor_plugin_config["max_range"].getValue().c_str())); - point_subsample_field_->setText(QString(sensor_plugin_config["point_subsample"].getValue().c_str())); - padding_offset_field_->setText(QString(sensor_plugin_config["padding_offset"].getValue().c_str())); - padding_scale_field_->setText(QString(sensor_plugin_config["padding_scale"].getValue().c_str())); - max_update_rate_field_->setText(QString(sensor_plugin_config["max_update_rate"].getValue().c_str())); - filtered_cloud_topic_field_->setText(QString(sensor_plugin_config["filtered_cloud_topic"].getValue().c_str())); - } - else if (sensor_plugin_config["sensor_plugin"].getValue() == - std::string("occupancy_map_monitor/DepthImageOctomapUpdater")) - { - sensor_plugin_field_->setCurrentIndex(2); - image_topic_field_->setText(QString(sensor_plugin_config["image_topic"].getValue().c_str())); - queue_size_field_->setText(QString(sensor_plugin_config["queue_size"].getValue().c_str())); - near_clipping_field_->setText(QString(sensor_plugin_config["near_clipping_plane_distance"].getValue().c_str())); - far_clipping_field_->setText(QString(sensor_plugin_config["far_clipping_plane_distance"].getValue().c_str())); - shadow_threshold_field_->setText(QString(sensor_plugin_config["shadow_threshold"].getValue().c_str())); - depth_padding_scale_field_->setText(QString(sensor_plugin_config["padding_scale"].getValue().c_str())); - depth_padding_offset_field_->setText(QString(sensor_plugin_config["padding_offset"].getValue().c_str())); - depth_filtered_cloud_topic_field_->setText( - QString(sensor_plugin_config["filtered_cloud_topic"].getValue().c_str())); - depth_max_update_rate_field_->setText(QString(sensor_plugin_config["max_update_rate"].getValue().c_str())); - } - } + // Load values from default config + auto default_config = MoveItConfigData::load3DSensorsYAML( + config_data_->setup_assistant_path_ + "/templates/moveit_config_pkg_template/config/sensors_3d.yaml"); + for (const auto& sensor_plugin_config : default_config) + loadConfigIntoWidgets(sensor_plugin_config); - // If no sensor config exists, default to None - if (sensors_vec_map.size() == 2) + // Load values from existing config + for (const auto& sensor_plugin_config : config_data_->getSensorPluginConfig()) { - sensor_plugin_field_->setCurrentIndex(0); + uint idx = loadConfigIntoWidgets(sensor_plugin_config); + sensor_plugin_field_->setCurrentIndex(idx); } } diff --git a/moveit_setup_assistant/src/widgets/perception_widget.h b/moveit_setup_assistant/src/widgets/perception_widget.h index 6d23c0d5ba..c67c4ef4d7 100644 --- a/moveit_setup_assistant/src/widgets/perception_widget.h +++ b/moveit_setup_assistant/src/widgets/perception_widget.h @@ -71,6 +71,7 @@ class PerceptionWidget : public SetupScreenWidget /// Populate the combo dropdown box with sensor plugins void loadSensorPluginsComboBox(); + uint loadConfigIntoWidgets(std::map sensor_plugin_config); // ****************************************************************************************** // Qt Components diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp index 941cf320ec..6416f18b8e 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp @@ -738,18 +738,9 @@ bool StartScreenWidget::load3DSensorsFile() fs::path sensors_3d_yaml_path = config_data_->config_pkg_path_; sensors_3d_yaml_path /= "config/sensors_3d.yaml"; - // Default parameters values are always loaded but overridden by values existing in sensors_3d - fs::path default_sensors_3d_yaml_path = "templates/moveit_config_pkg_template/config/sensors_3d.yaml"; - - if (!fs::is_regular_file(sensors_3d_yaml_path)) - { - return config_data_->input3DSensorsYAML(default_sensors_3d_yaml_path.make_preferred().string()); - } - else - { - return config_data_->input3DSensorsYAML(default_sensors_3d_yaml_path.make_preferred().string(), - sensors_3d_yaml_path.make_preferred().string()); - } + if (fs::is_regular_file(sensors_3d_yaml_path)) + config_data_->input3DSensorsYAML(sensors_3d_yaml_path.make_preferred().string()); + return true; } // ****************************************************************************************** diff --git a/moveit_setup_assistant/test/moveit_config_data_test.cpp b/moveit_setup_assistant/test/moveit_config_data_test.cpp index f89316060f..f832eda7d1 100644 --- a/moveit_setup_assistant/test/moveit_config_data_test.cpp +++ b/moveit_setup_assistant/test/moveit_config_data_test.cpp @@ -122,16 +122,15 @@ TEST_F(MoveItConfigData, ReadingSensorsConfig) // Read the file containing the default config parameters config_data->input3DSensorsYAML( (setup_assistant_path / "templates/moveit_config_pkg_template/config/sensors_3d.yaml").string()); + auto configs = config_data->getSensorPluginConfig(); // Default config for the two available sensor plugins // Make sure both are parsed correctly - ASSERT_EQ(config_data->getSensorPluginConfig().size(), 2u); + ASSERT_EQ(configs.size(), 2u); - EXPECT_EQ(config_data->getSensorPluginConfig()[0]["sensor_plugin"].getValue(), - std::string("occupancy_map_monitor/PointCloudOctomapUpdater")); + EXPECT_EQ(configs[0]["sensor_plugin"].getValue(), std::string("occupancy_map_monitor/PointCloudOctomapUpdater")); - EXPECT_EQ(config_data->getSensorPluginConfig()[1]["sensor_plugin"].getValue(), - std::string("occupancy_map_monitor/DepthImageOctomapUpdater")); + EXPECT_EQ(configs[1]["sensor_plugin"].getValue(), std::string("occupancy_map_monitor/DepthImageOctomapUpdater")); } // This tests writing of sensors_3d.yaml @@ -147,33 +146,27 @@ TEST_F(MoveItConfigData, WritingSensorsConfig) // Temporary file used during the test and is deleted when the test is finished char test_file[] = "/tmp/msa_unittest_sensors.yaml"; - // sensors yaml written correctly + // empty sensors.yaml written correctly EXPECT_EQ(config_data->output3DSensorPluginYAML(test_file), true); // Set default file - boost::filesystem::path setup_assistant_path(config_data->setup_assistant_path_); std::string default_file_path = - (setup_assistant_path / "templates/moveit_config_pkg_template/config/sensors_3d.yaml").string(); - - // Read from the written file - config_data = std::make_shared(); - EXPECT_EQ(config_data->input3DSensorsYAML(test_file), true); + config_data->setup_assistant_path_ + "/templates/moveit_config_pkg_template/config/sensors_3d.yaml"; - // Should still have No Sensors - EXPECT_EQ(config_data->getSensorPluginConfig().size(), 0u); + // Read from the written (empty) file + auto config = moveit_setup_assistant::MoveItConfigData::load3DSensorsYAML(test_file); + // Should have No Sensors + EXPECT_EQ(config.size(), 0u); // Now load the default file and write it to a file config_data = std::make_shared(); - EXPECT_EQ(config_data->input3DSensorsYAML(default_file_path), true); + config_data->input3DSensorsYAML(default_file_path); EXPECT_EQ(config_data->getSensorPluginConfig().size(), 2u); EXPECT_EQ(config_data->output3DSensorPluginYAML(test_file), true); // Read from the written file - config_data = std::make_shared(); - EXPECT_EQ(config_data->input3DSensorsYAML(test_file), true); - - // Should now have two sensors - EXPECT_EQ(config_data->getSensorPluginConfig().size(), 2u); + config = moveit_setup_assistant::MoveItConfigData::load3DSensorsYAML(test_file); + EXPECT_EQ(config.size(), 2u); } int main(int argc, char** argv) From 7f30892cbcc95cb5fec1be76edec9d8846b1d6b8 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 12:26:29 +0100 Subject: [PATCH 061/229] Add gazebo_controllers.yaml --- .../widgets/configuration_files_widget.cpp | 26 ++++++++++++------- .../config/gazebo_controllers.yaml | 4 +++ .../launch/gazebo.launch | 5 ++++ 3 files changed, 26 insertions(+), 9 deletions(-) create mode 100644 moveit_setup_assistant/templates/moveit_config_pkg_template/config/gazebo_controllers.yaml diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 5e052dc1ac..431677af5d 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -317,6 +317,14 @@ bool ConfigurationFilesWidget::loadGenFiles() file.write_on_changes = MoveItConfigData::GROUPS; gen_files_.push_back(file); + // gazebo_controllers.yaml ------------------------------------------------------------------ + file.file_name_ = "gazebo_controllers.yaml"; + file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); + template_path = config_data_->appendPaths(config_data_->template_package_path_, file.rel_path_); + file.description_ = "Configuration of Gazebo controllers"; + file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + gen_files_.push_back(file); + // ros_controllers.yaml -------------------------------------------------------------------------------------- file.file_name_ = "ros_controllers.yaml"; file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); @@ -515,15 +523,6 @@ bool ConfigurationFilesWidget::loadGenFiles() file.write_on_changes = 0; gen_files_.push_back(file); - // gazebo.launch ------------------------------------------------------------------ - file.file_name_ = "gazebo.launch"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, "gazebo.launch"); - file.description_ = "Gazebo launch file which also launches ros_controllers and sends robot urdf to param server, " - "then using gazebo_ros pkg the robot is spawned to Gazebo"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); - gen_files_.push_back(file); - // demo_gazebo.launch ------------------------------------------------------------------ file.file_name_ = "demo_gazebo.launch"; file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); @@ -533,6 +532,15 @@ bool ConfigurationFilesWidget::loadGenFiles() file.write_on_changes = 0; gen_files_.push_back(file); + // gazebo.launch ------------------------------------------------------------------ + file.file_name_ = "gazebo.launch"; + file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); + template_path = config_data_->appendPaths(template_launch_path, "gazebo.launch"); + file.description_ = "Gazebo launch file which also launches ros_controllers and sends robot urdf to param server, " + "then using gazebo_ros pkg the robot is spawned to Gazebo"; + file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + gen_files_.push_back(file); + // joystick_control.launch ------------------------------------------------------------------ file.file_name_ = "joystick_control.launch"; file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/config/gazebo_controllers.yaml b/moveit_setup_assistant/templates/moveit_config_pkg_template/config/gazebo_controllers.yaml new file mode 100644 index 0000000000..e4d2eb00c7 --- /dev/null +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/config/gazebo_controllers.yaml @@ -0,0 +1,4 @@ +# Publish joint_states +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gazebo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gazebo.launch index 6213ad3c58..8aca25f1fe 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gazebo.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gazebo.launch @@ -18,6 +18,11 @@ + + + + + From 0216b59edfea1efe009eee0556370cf6a2cd6cf3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 12:59:55 +0100 Subject: [PATCH 062/229] Rework controller config generation We should write separate controller config files for different controller managers: - simple_moveit_controllers.yaml handles everything relevant for SimpleMoveItControllerManager - ros_controllers.yaml handles ros_control config - gazebo_controllers.yaml handles controllers required for Gazebo --- .../tools/moveit_config_data.h | 10 +- .../src/tools/moveit_config_data.cpp | 101 ++++++++---------- .../widgets/configuration_files_widget.cpp | 10 +- 3 files changed, 56 insertions(+), 65 deletions(-) diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h index 4601e24d65..3bd215bcc4 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h @@ -312,15 +312,7 @@ class MoveItConfigData bool outputKinematicsYAML(const std::string& file_path); bool outputJointLimitsYAML(const std::string& file_path); bool outputFakeControllersYAML(const std::string& file_path); - - /** - * Helper function for writing follow joint trajectory ROS controllers to ros_controllers.yaml - * @param YAML Emitter - yaml emitter used to write the config to the ROS controllers yaml file - * @param vector - a copy of ROS controllers config which will be modified in the function - */ - void outputFollowJointTrajectoryYAML(YAML::Emitter& emitter, - std::vector& ros_controllers_config_output); - + bool outputSimpleControllersYAML(const std::string& file_path); bool outputROSControllersYAML(const std::string& file_path); bool output3DSensorPluginYAML(const std::string& file_path); diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 4f4f3fe80b..ad47f2c21b 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -821,51 +821,54 @@ std::vector MoveItConfigData::getOMPLPlanners() } // ****************************************************************************************** -// Helper function to write the FollowJointTrajectory for each planning group to ros_controller.yaml, -// and erases the controller that have been written, to avoid mixing between FollowJointTrajectory -// which are published under the namespace of 'controller_list' and other types of controllers. +// Generate simple_moveit_controllers.yaml config file // ****************************************************************************************** -void MoveItConfigData::outputFollowJointTrajectoryYAML(YAML::Emitter& emitter, - std::vector& ros_controllers_configs) +bool MoveItConfigData::outputSimpleControllersYAML(const std::string& file_path) { - // Write default controllers + YAML::Emitter emitter; + emitter << YAML::BeginMap; emitter << YAML::Key << "controller_list"; emitter << YAML::Value << YAML::BeginSeq; + for (const auto& controller : ros_controllers_config_) { - for (std::vector::iterator controller_it = ros_controllers_configs.begin(); - controller_it != ros_controllers_configs.end();) + // Only process FollowJointTrajectory types + if (controller.type_ == "FollowJointTrajectory" || controller.type_ == "GripperCommand") { - // Only process FollowJointTrajectory types - if (controller_it->type_ == "FollowJointTrajectory") - { - emitter << YAML::BeginMap; - emitter << YAML::Key << "name"; - emitter << YAML::Value << controller_it->name_; - emitter << YAML::Key << "action_ns"; - emitter << YAML::Value << "follow_joint_trajectory"; - emitter << YAML::Key << "default"; - emitter << YAML::Value << "True"; - emitter << YAML::Key << "type"; - emitter << YAML::Value << controller_it->type_; - // Write joints - emitter << YAML::Key << "joints"; - - emitter << YAML::Value << YAML::BeginSeq; - // Iterate through the joints - for (std::string& joint : controller_it->joints_) - emitter << joint; - emitter << YAML::EndSeq; + emitter << YAML::BeginMap; + emitter << YAML::Key << "name"; + emitter << YAML::Value << controller.name_; + emitter << YAML::Key << "action_ns"; + emitter << YAML::Value + << (controller.type_ == "FollowJointTrajectory" ? "follow_joint_trajectory" : "gripper_action"); + emitter << YAML::Key << "type"; + emitter << YAML::Value << controller.type_; + emitter << YAML::Key << "default"; + emitter << YAML::Value << "True"; - emitter << YAML::EndMap; - controller_it = ros_controllers_configs.erase(controller_it); - } - else - { - controller_it++; - } + // Write joints + emitter << YAML::Key << "joints"; + emitter << YAML::Value << YAML::BeginSeq; + // Iterate through the joints + for (const std::string& joint : controller.joints_) + emitter << joint; + emitter << YAML::EndSeq; + + emitter << YAML::EndMap; } - emitter << YAML::EndSeq; } + emitter << YAML::EndSeq; + emitter << YAML::EndMap; + + std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc); + if (!output_stream.good()) + { + ROS_ERROR_STREAM("Unable to open file for writing " << file_path); + return false; + } + output_stream << emitter.c_str(); + output_stream.close(); + + return true; // file created successfully } // ****************************************************************************************** @@ -880,13 +883,10 @@ srdf::Model::GroupState MoveItConfigData::getDefaultStartPose() } // ****************************************************************************************** -// Output controllers config files +// Generate ros_controllers.yaml config file // ****************************************************************************************** bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) { - // Copy ros_control_config_ to a new vector to avoid modifying it - std::vector ros_controllers_config_output(ros_controllers_config_); - // Cache the joints' names. std::vector> planning_groups; @@ -914,6 +914,8 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) emitter << YAML::BeginMap; { +#if 0 // TODO: This is only for fake ROS controllers, which should go into a separate file + // Also replace moveit_sim_controllers with http://wiki.ros.org/fake_joint emitter << YAML::Comment("Simulation settings for using moveit_sim_controllers"); emitter << YAML::Key << "moveit_sim_hw_interface" << YAML::Value << YAML::BeginMap; // MoveIt Simulation Controller settings for setting initial pose @@ -977,23 +979,12 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) emitter << YAML::Newline; emitter << YAML::EndMap; } - // Joint State Controller - emitter << YAML::Comment("Publish all joint states"); - emitter << YAML::Newline << YAML::Comment("Creates the /joint_states topic necessary in ROS"); - emitter << YAML::Key << "joint_state_controller" << YAML::Value << YAML::BeginMap; +#endif + for (const auto& controller : ros_controllers_config_) { - emitter << YAML::Key << "type"; - emitter << YAML::Value << "joint_state_controller/JointStateController"; - emitter << YAML::Key << "publish_rate"; - emitter << YAML::Value << "50"; - emitter << YAML::EndMap; - } + if (controller.type_ == "FollowJointTrajectory" || controller.type_ == "GripperCommand") + continue; // these are handled by outputSimpleControllersYAML() - // Writes Follow Joint Trajectory ROS controllers to ros_controller.yaml - outputFollowJointTrajectoryYAML(emitter, ros_controllers_config_output); - - for (const auto& controller : ros_controllers_config_output) - { emitter << YAML::Key << controller.name_; emitter << YAML::Value << YAML::BeginMap; emitter << YAML::Key << "type"; diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 431677af5d..11665d4986 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -317,6 +317,14 @@ bool ConfigurationFilesWidget::loadGenFiles() file.write_on_changes = MoveItConfigData::GROUPS; gen_files_.push_back(file); + // simple_moveit_controllers.yaml ------------------------------------------------------------------------------- + file.file_name_ = "simple_moveit_controllers.yaml"; + file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); + file.description_ = "Creates controller configuration for SimpleMoveItControllerManager"; + file.gen_func_ = boost::bind(&MoveItConfigData::outputSimpleControllersYAML, config_data_, _1); + file.write_on_changes = MoveItConfigData::GROUPS; + gen_files_.push_back(file); + // gazebo_controllers.yaml ------------------------------------------------------------------ file.file_name_ = "gazebo_controllers.yaml"; file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); @@ -328,7 +336,7 @@ bool ConfigurationFilesWidget::loadGenFiles() // ros_controllers.yaml -------------------------------------------------------------------------------------- file.file_name_ = "ros_controllers.yaml"; file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); - file.description_ = "Creates configurations for ros_controllers."; + file.description_ = "Creates controller configurations for ros_control."; file.gen_func_ = boost::bind(&MoveItConfigData::outputROSControllersYAML, config_data_, _1); file.write_on_changes = MoveItConfigData::GROUPS; gen_files_.push_back(file); From a60571d53947eeb58a0495876f429325b2a3d655 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 22:58:46 +0100 Subject: [PATCH 063/229] Fix ros_controllers.yaml: always handle joints as sequence --- .../src/tools/moveit_config_data.cpp | 23 ++++--------------- 1 file changed, 5 insertions(+), 18 deletions(-) diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index ad47f2c21b..44b56f1717 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -992,25 +992,12 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) // Write joints emitter << YAML::Key << "joints"; - { - if (controller.joints_.size() != 1) - { - emitter << YAML::Value << YAML::BeginSeq; + emitter << YAML::Value << YAML::BeginSeq; + // Iterate through the joints + for (const std::string& joint : controller.joints_) + emitter << joint; + emitter << YAML::EndSeq; - // Iterate through the joints - for (const std::string& joint : controller.joints_) - { - emitter << joint; - } - emitter << YAML::EndSeq; - } - else - { - emitter << YAML::Value << YAML::BeginMap; - emitter << controller.joints_[0]; - emitter << YAML::EndMap; - } - } // Write gains as they are required for vel and effort controllers emitter << YAML::Key << "gains"; emitter << YAML::Value << YAML::BeginMap; From 72cbf756dc3e6d109e9d804f9cc81967950986a0 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 13:08:24 +0100 Subject: [PATCH 064/229] Rename ROSControlConfig -> ControllerConfig --- .../tools/moveit_config_data.h | 14 ++++----- .../src/tools/moveit_config_data.cpp | 22 +++++++------- .../widgets/configuration_files_widget.cpp | 2 +- .../src/widgets/controller_edit_widget.cpp | 2 +- .../src/widgets/ros_controllers_widget.cpp | 30 +++++++++---------- .../src/widgets/ros_controllers_widget.h | 8 ++--- 6 files changed, 39 insertions(+), 39 deletions(-) diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h index 3bd215bcc4..358126be74 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h @@ -76,9 +76,9 @@ struct GroupMetaData }; /** - * ROS Controllers settings which may be set in the config files + * Controllers settings which may be set in the config files */ -struct ROSControlConfig +struct ControllerConfig { std::string name_; // controller name std::string type_; // controller type @@ -453,13 +453,13 @@ class MoveItConfigData * \param new_controller a new ROS Controller to add * \return true if inserted correctly */ - bool addROSController(const ROSControlConfig& new_controller); + bool addROSController(const ControllerConfig& new_controller); /** * \brief Gets ros_controllers_config_ vector * \return pointer to ros_controllers_config_ */ - std::vector& getROSControllers(); + std::vector& getROSControllers(); /** * Find the associated ROS controller by name @@ -467,7 +467,7 @@ class MoveItConfigData * @param controller_name - name of ROS controller to find in datastructure * @return pointer to data in datastructure */ - ROSControlConfig* findROSControllerByName(const std::string& controller_name); + ControllerConfig* findROSControllerByName(const std::string& controller_name); /** * Delete ROS controller by name @@ -523,8 +523,8 @@ class MoveItConfigData /// Shared kinematic model moveit::core::RobotModelPtr robot_model_; - /// ROS Controllers config data - std::vector ros_controllers_config_; + /// Controllers config data + std::vector ros_controllers_config_; /// Shared planning scene planning_scene::PlanningScenePtr planning_scene_; diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 44b56f1717..4603f8edbc 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -374,7 +374,7 @@ bool MoveItConfigData::outputKinematicsYAML(const std::string& file_path) // ****************************************************************************************** std::string MoveItConfigData::getJointHardwareInterface(const std::string& joint_name) { - for (ROSControlConfig& ros_control_config : ros_controllers_config_) + for (ControllerConfig& ros_control_config : ros_controllers_config_) { std::vector::iterator joint_it = std::find(ros_control_config.joints_.begin(), ros_control_config.joints_.end(), joint_name); @@ -1404,7 +1404,7 @@ bool MoveItConfigData::inputPlanningContextLaunch(const std::string& file_path) bool MoveItConfigData::parseROSController(const YAML::Node& controller) { // Used in parsing ROS controllers - ROSControlConfig control_setting; + ControllerConfig control_setting; if (const YAML::Node& trajectory_controllers = controller) { @@ -1450,7 +1450,7 @@ bool MoveItConfigData::parseROSController(const YAML::Node& controller) bool MoveItConfigData::processROSControllers(std::ifstream& input_stream) { // Used in parsing ROS controllers - ROSControlConfig control_setting; + ControllerConfig control_setting; YAML::Node controllers = YAML::Load(input_stream); // Loop through all controllers @@ -1537,7 +1537,7 @@ bool MoveItConfigData::addDefaultControllers() // Loop through groups for (const srdf::Model::Group& group_it : srdf_->srdf_model_->getGroups()) { - ROSControlConfig group_controller; + ControllerConfig group_controller; // Get list of associated joints const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group_it.name_); const std::vector& joint_models = joint_model_group->getActiveJointModels(); @@ -1883,12 +1883,12 @@ srdf::Model::Group* MoveItConfigData::findGroupByName(const std::string& name) // ****************************************************************************************** // Find ROS controller by name // ****************************************************************************************** -ROSControlConfig* MoveItConfigData::findROSControllerByName(const std::string& controller_name) +ControllerConfig* MoveItConfigData::findROSControllerByName(const std::string& controller_name) { // Find the ROSController we are editing based on the ROSController name string - ROSControlConfig* searched_ros_controller = nullptr; // used for holding our search results + ControllerConfig* searched_ros_controller = nullptr; // used for holding our search results - for (ROSControlConfig& ros_control_config : ros_controllers_config_) + for (ControllerConfig& ros_control_config : ros_controllers_config_) { if (ros_control_config.name_ == controller_name) // string match { @@ -1905,7 +1905,7 @@ ROSControlConfig* MoveItConfigData::findROSControllerByName(const std::string& c // ****************************************************************************************** bool MoveItConfigData::deleteROSController(const std::string& controller_name) { - for (std::vector::iterator controller_it = ros_controllers_config_.begin(); + for (std::vector::iterator controller_it = ros_controllers_config_.begin(); controller_it != ros_controllers_config_.end(); ++controller_it) { if (controller_it->name_ == controller_name) // string match @@ -1921,10 +1921,10 @@ bool MoveItConfigData::deleteROSController(const std::string& controller_name) // ****************************************************************************************** // Adds a ROS controller to ros_controllers_config_ vector // ****************************************************************************************** -bool MoveItConfigData::addROSController(const ROSControlConfig& new_controller) +bool MoveItConfigData::addROSController(const ControllerConfig& new_controller) { // Used for holding our search results - ROSControlConfig* searched_ros_controller = nullptr; + ControllerConfig* searched_ros_controller = nullptr; // Find if there is an existing controller with the same name searched_ros_controller = findROSControllerByName(new_controller.name_); @@ -1939,7 +1939,7 @@ bool MoveItConfigData::addROSController(const ROSControlConfig& new_controller) // ****************************************************************************************** // Gets ros_controllers_config_ vector // ****************************************************************************************** -std::vector& MoveItConfigData::getROSControllers() +std::vector& MoveItConfigData::getROSControllers() { return ros_controllers_config_; } diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 11665d4986..3d721cbcca 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -1157,7 +1157,7 @@ void ConfigurationFilesWidget::loadTemplateStrings() else { std::stringstream controllers; - for (ROSControlConfig& controller : config_data_->getROSControllers()) + for (ControllerConfig& controller : config_data_->getROSControllers()) { // Check if the controller belongs to controller_list namespace if (controller.type_ != "FollowJointTrajectory") diff --git a/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp b/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp index 38be579c2f..1e3ae6b1b1 100644 --- a/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp +++ b/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp @@ -161,7 +161,7 @@ ControllerEditWidget::ControllerEditWidget(QWidget* parent, const MoveItConfigDa void ControllerEditWidget::setSelected(const std::string& controller_name) { controller_name_field_->setText(QString(controller_name.c_str())); - moveit_setup_assistant::ROSControlConfig* searched_controller = + moveit_setup_assistant::ControllerConfig* searched_controller = config_data_->findROSControllerByName(controller_name); if (searched_controller != nullptr) { diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp index f29e71817a..488dc2ccaa 100644 --- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp +++ b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp @@ -213,7 +213,7 @@ void ROSControllersWidget::loadControllersTree() controllers_tree_->clear(); // reset the tree // Display all controllers by looping through them - for (ROSControlConfig& controller : config_data_->getROSControllers()) + for (ControllerConfig& controller : config_data_->getROSControllers()) { loadToControllersTree(controller); } @@ -228,7 +228,7 @@ void ROSControllersWidget::loadControllersTree() // ****************************************************************************************** // Displays data in the controller_config_ data structure into a QtTableWidget // ****************************************************************************************** -void ROSControllersWidget::loadToControllersTree(const moveit_setup_assistant::ROSControlConfig& controller_it) +void ROSControllersWidget::loadToControllersTree(const moveit_setup_assistant::ControllerConfig& controller_it) { // Fonts for tree const QFont top_level_font(QFont().defaultFamily(), 11, QFont::Bold); @@ -285,7 +285,7 @@ void ROSControllersWidget::focusGiven() // ****************************************************************************************** // Load the popup screen with correct data for joints // ****************************************************************************************** -void ROSControllersWidget::loadJointsScreen(moveit_setup_assistant::ROSControlConfig* this_controller) +void ROSControllersWidget::loadJointsScreen(moveit_setup_assistant::ControllerConfig* this_controller) { // Retrieve pointer to the shared kinematic model const moveit::core::RobotModelConstPtr& model = config_data_->getRobotModel(); @@ -316,7 +316,7 @@ void ROSControllersWidget::loadJointsScreen(moveit_setup_assistant::ROSControlCo // ****************************************************************************************** // Load the popup screen with correct data for gcroups // ****************************************************************************************** -void ROSControllersWidget::loadGroupsScreen(moveit_setup_assistant::ROSControlConfig* this_controller) +void ROSControllersWidget::loadGroupsScreen(moveit_setup_assistant::ControllerConfig* this_controller) { // Load all groups into the subgroup screen std::vector groups; @@ -415,7 +415,7 @@ void ROSControllersWidget::addDefaultControllers() // ****************************************************************************************** // Load the popup screen with correct data for controllers // ****************************************************************************************** -void ROSControllersWidget::loadControllerScreen(moveit_setup_assistant::ROSControlConfig* this_controller) +void ROSControllersWidget::loadControllerScreen(moveit_setup_assistant::ControllerConfig* this_controller) { // Load the avail controllers. This function only runs once controller_edit_widget_->loadControllersTypesComboBox(); @@ -448,7 +448,7 @@ void ROSControllersWidget::cancelEditing() { if (!current_edit_controller_.empty() && adding_new_controller_) { - moveit_setup_assistant::ROSControlConfig* editing = config_data_->findROSControllerByName(current_edit_controller_); + moveit_setup_assistant::ControllerConfig* editing = config_data_->findROSControllerByName(current_edit_controller_); if (editing && editing->joints_.empty()) { config_data_->deleteROSController(current_edit_controller_); @@ -543,7 +543,7 @@ void ROSControllersWidget::saveControllerScreenJoints() return; // Find the controller we are editing based on the controller name string - moveit_setup_assistant::ROSControlConfig* editing_controller = + moveit_setup_assistant::ControllerConfig* editing_controller = config_data_->findROSControllerByName(current_edit_controller_); loadJointsScreen(editing_controller); @@ -562,7 +562,7 @@ void ROSControllersWidget::saveControllerScreenGroups() return; // Find the controller we are editing based on the controller name string - moveit_setup_assistant::ROSControlConfig* editing_controller = + moveit_setup_assistant::ControllerConfig* editing_controller = config_data_->findROSControllerByName(current_edit_controller_); loadGroupsScreen(editing_controller); @@ -577,7 +577,7 @@ void ROSControllersWidget::saveControllerScreenGroups() void ROSControllersWidget::saveJointsScreen() { // Find the controller we are editing based on the controller name string - moveit_setup_assistant::ROSControlConfig* searched_controller = + moveit_setup_assistant::ControllerConfig* searched_controller = config_data_->findROSControllerByName(current_edit_controller_); // Clear the old data @@ -602,7 +602,7 @@ void ROSControllersWidget::saveJointsScreen() void ROSControllersWidget::saveJointsGroupsScreen() { // Find the controller we are editing based on the controller name string - moveit_setup_assistant::ROSControlConfig* searched_controller = + moveit_setup_assistant::ControllerConfig* searched_controller = config_data_->findROSControllerByName(current_edit_controller_); // Clear the old data @@ -655,7 +655,7 @@ bool ROSControllersWidget::saveControllerScreen() const std::string& controller_type = controller_edit_widget_->getControllerType(); // Used for editing existing controllers - moveit_setup_assistant::ROSControlConfig* searched_controller = nullptr; + moveit_setup_assistant::ControllerConfig* searched_controller = nullptr; std::smatch invalid_name_match; std::regex invalid_reg_ex("[^a-z|^1-9|^_]"); @@ -693,7 +693,7 @@ bool ROSControllersWidget::saveControllerScreen() // Save the new controller name or create the new controller if (searched_controller == nullptr) // create new { - moveit_setup_assistant::ROSControlConfig new_controller; + moveit_setup_assistant::ControllerConfig new_controller; new_controller.name_ = controller_name; new_controller.type_ = controller_type; config_data_->addROSController(new_controller); @@ -741,7 +741,7 @@ void ROSControllersWidget::editSelected() // The controller this joint belong to controller_item = item->parent()->parent(); current_edit_controller_ = controller_item->text(0).toUtf8().constData(); - moveit_setup_assistant::ROSControlConfig* this_controller = + moveit_setup_assistant::ControllerConfig* this_controller = config_data_->findROSControllerByName(current_edit_controller_); // Load the data @@ -754,7 +754,7 @@ void ROSControllersWidget::editSelected() { controller_item = item->parent(); current_edit_controller_ = controller_item->text(0).toUtf8().constData(); - moveit_setup_assistant::ROSControlConfig* this_controller = + moveit_setup_assistant::ControllerConfig* this_controller = config_data_->findROSControllerByName(current_edit_controller_); // Load the data @@ -767,7 +767,7 @@ void ROSControllersWidget::editSelected() { // Load the data current_edit_controller_ = item->text(0).toUtf8().constData(); - moveit_setup_assistant::ROSControlConfig* this_controller = + moveit_setup_assistant::ControllerConfig* this_controller = config_data_->findROSControllerByName(current_edit_controller_); loadControllerScreen(this_controller); diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.h b/moveit_setup_assistant/src/widgets/ros_controllers_widget.h index f38408d1c7..bfd0bbbf37 100644 --- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.h +++ b/moveit_setup_assistant/src/widgets/ros_controllers_widget.h @@ -148,11 +148,11 @@ private Q_SLOTS: QWidget* createContentsWidget(); void loadControllersTree(); - void loadToControllersTree(const moveit_setup_assistant::ROSControlConfig& controller_it); + void loadToControllersTree(const moveit_setup_assistant::ControllerConfig& controller_it); void showMainScreen(); - void loadJointsScreen(moveit_setup_assistant::ROSControlConfig* this_controller); - void loadGroupsScreen(moveit_setup_assistant::ROSControlConfig* this_controller); - void loadControllerScreen(moveit_setup_assistant::ROSControlConfig* this_controller); + void loadJointsScreen(moveit_setup_assistant::ControllerConfig* this_controller); + void loadGroupsScreen(moveit_setup_assistant::ControllerConfig* this_controller); + void loadControllerScreen(moveit_setup_assistant::ControllerConfig* this_controller); }; } // namespace moveit_setup_assistant From f7ab083dbb3e9ee7ebc78a97bc45fc9f36978cbf Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 13:12:05 +0100 Subject: [PATCH 065/229] Rename functions *ROSController* -> *Controller* --- .../tools/moveit_config_data.h | 20 ++++++------- .../src/tools/moveit_config_data.cpp | 18 +++++------ .../widgets/configuration_files_widget.cpp | 4 +-- .../src/widgets/controller_edit_widget.cpp | 3 +- .../src/widgets/ros_controllers_widget.cpp | 30 +++++++++---------- .../src/widgets/start_screen_widget.cpp | 2 +- .../test/moveit_config_data_test.cpp | 8 ++--- 7 files changed, 42 insertions(+), 43 deletions(-) diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h index 358126be74..71b640495c 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h @@ -449,33 +449,33 @@ class MoveItConfigData std::string appendPaths(const std::string& path1, const std::string& path2); /** - * \brief Adds a ROS controller to ros_controllers_config_ vector - * \param new_controller a new ROS Controller to add + * \brief Adds a controller to ros_controllers_config_ vector + * \param new_controller a new Controller to add * \return true if inserted correctly */ - bool addROSController(const ControllerConfig& new_controller); + bool addController(const ControllerConfig& new_controller); /** * \brief Gets ros_controllers_config_ vector * \return pointer to ros_controllers_config_ */ - std::vector& getROSControllers(); + std::vector& getControllers(); /** - * Find the associated ROS controller by name + * Find the associated controller by name * - * @param controller_name - name of ROS controller to find in datastructure + * @param controller_name - name of controller to find in datastructure * @return pointer to data in datastructure */ - ControllerConfig* findROSControllerByName(const std::string& controller_name); + ControllerConfig* findControllerByName(const std::string& controller_name); /** - * Delete ROS controller by name + * Delete controller by name * - * @param controller_name - name of ROS controller to delete + * @param controller_name - name of controller to delete * @return true if deleted, false if not found */ - bool deleteROSController(const std::string& controller_name); + bool deleteController(const std::string& controller_name); /** * \brief Used for adding a sensor plugin configuation prameter to the sensor plugin configuration parameter list diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 4603f8edbc..2852dc68f9 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -1553,7 +1553,7 @@ bool MoveItConfigData::addDefaultControllers() { group_controller.name_ = group_it.name_ + "_controller"; group_controller.type_ = "FollowJointTrajectory"; - addROSController(group_controller); + addController(group_controller); } } return true; @@ -1881,9 +1881,9 @@ srdf::Model::Group* MoveItConfigData::findGroupByName(const std::string& name) } // ****************************************************************************************** -// Find ROS controller by name +// Find a controller by name // ****************************************************************************************** -ControllerConfig* MoveItConfigData::findROSControllerByName(const std::string& controller_name) +ControllerConfig* MoveItConfigData::findControllerByName(const std::string& controller_name) { // Find the ROSController we are editing based on the ROSController name string ControllerConfig* searched_ros_controller = nullptr; // used for holding our search results @@ -1901,9 +1901,9 @@ ControllerConfig* MoveItConfigData::findROSControllerByName(const std::string& c } // ****************************************************************************************** -// Deletes a ROS controller by name +// Deletes a controller by name // ****************************************************************************************** -bool MoveItConfigData::deleteROSController(const std::string& controller_name) +bool MoveItConfigData::deleteController(const std::string& controller_name) { for (std::vector::iterator controller_it = ros_controllers_config_.begin(); controller_it != ros_controllers_config_.end(); ++controller_it) @@ -1919,15 +1919,15 @@ bool MoveItConfigData::deleteROSController(const std::string& controller_name) } // ****************************************************************************************** -// Adds a ROS controller to ros_controllers_config_ vector +// Adds a controller to ros_controllers_config_ vector // ****************************************************************************************** -bool MoveItConfigData::addROSController(const ControllerConfig& new_controller) +bool MoveItConfigData::addController(const ControllerConfig& new_controller) { // Used for holding our search results ControllerConfig* searched_ros_controller = nullptr; // Find if there is an existing controller with the same name - searched_ros_controller = findROSControllerByName(new_controller.name_); + searched_ros_controller = findControllerByName(new_controller.name_); if (searched_ros_controller && searched_ros_controller->type_ == new_controller.type_) return false; @@ -1939,7 +1939,7 @@ bool MoveItConfigData::addROSController(const ControllerConfig& new_controller) // ****************************************************************************************** // Gets ros_controllers_config_ vector // ****************************************************************************************** -std::vector& MoveItConfigData::getROSControllers() +std::vector& MoveItConfigData::getControllers() { return ros_controllers_config_; } diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 3d721cbcca..4b1b71f175 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -1150,14 +1150,14 @@ void ConfigurationFilesWidget::loadTemplateStrings() } // Pair 9 - Add ROS Controllers to ros_controllers.launch file - if (config_data_->getROSControllers().empty()) + if (config_data_->getControllers().empty()) { addTemplateString("[ROS_CONTROLLERS]", ""); } else { std::stringstream controllers; - for (ControllerConfig& controller : config_data_->getROSControllers()) + for (ControllerConfig& controller : config_data_->getControllers()) { // Check if the controller belongs to controller_list namespace if (controller.type_ != "FollowJointTrajectory") diff --git a/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp b/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp index 1e3ae6b1b1..078af7a562 100644 --- a/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp +++ b/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp @@ -161,8 +161,7 @@ ControllerEditWidget::ControllerEditWidget(QWidget* parent, const MoveItConfigDa void ControllerEditWidget::setSelected(const std::string& controller_name) { controller_name_field_->setText(QString(controller_name.c_str())); - moveit_setup_assistant::ControllerConfig* searched_controller = - config_data_->findROSControllerByName(controller_name); + moveit_setup_assistant::ControllerConfig* searched_controller = config_data_->findControllerByName(controller_name); if (searched_controller != nullptr) { const std::string controller_type = searched_controller->type_; diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp index 488dc2ccaa..a550e7a01c 100644 --- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp +++ b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp @@ -213,7 +213,7 @@ void ROSControllersWidget::loadControllersTree() controllers_tree_->clear(); // reset the tree // Display all controllers by looping through them - for (ControllerConfig& controller : config_data_->getROSControllers()) + for (ControllerConfig& controller : config_data_->getControllers()) { loadToControllersTree(controller); } @@ -371,7 +371,7 @@ void ROSControllersWidget::deleteController() return; } // Delete actual controller - if (config_data_->deleteROSController(controller_name)) + if (config_data_->deleteController(controller_name)) { ROS_INFO_STREAM_NAMED("Setup Assistant", "Controller " << controller_name << " deleted succefully"); } @@ -448,10 +448,10 @@ void ROSControllersWidget::cancelEditing() { if (!current_edit_controller_.empty() && adding_new_controller_) { - moveit_setup_assistant::ControllerConfig* editing = config_data_->findROSControllerByName(current_edit_controller_); + moveit_setup_assistant::ControllerConfig* editing = config_data_->findControllerByName(current_edit_controller_); if (editing && editing->joints_.empty()) { - config_data_->deleteROSController(current_edit_controller_); + config_data_->deleteController(current_edit_controller_); current_edit_controller_.clear(); // Load the data to the tree @@ -544,7 +544,7 @@ void ROSControllersWidget::saveControllerScreenJoints() // Find the controller we are editing based on the controller name string moveit_setup_assistant::ControllerConfig* editing_controller = - config_data_->findROSControllerByName(current_edit_controller_); + config_data_->findControllerByName(current_edit_controller_); loadJointsScreen(editing_controller); @@ -563,7 +563,7 @@ void ROSControllersWidget::saveControllerScreenGroups() // Find the controller we are editing based on the controller name string moveit_setup_assistant::ControllerConfig* editing_controller = - config_data_->findROSControllerByName(current_edit_controller_); + config_data_->findControllerByName(current_edit_controller_); loadGroupsScreen(editing_controller); @@ -578,7 +578,7 @@ void ROSControllersWidget::saveJointsScreen() { // Find the controller we are editing based on the controller name string moveit_setup_assistant::ControllerConfig* searched_controller = - config_data_->findROSControllerByName(current_edit_controller_); + config_data_->findControllerByName(current_edit_controller_); // Clear the old data searched_controller->joints_.clear(); @@ -603,7 +603,7 @@ void ROSControllersWidget::saveJointsGroupsScreen() { // Find the controller we are editing based on the controller name string moveit_setup_assistant::ControllerConfig* searched_controller = - config_data_->findROSControllerByName(current_edit_controller_); + config_data_->findControllerByName(current_edit_controller_); // Clear the old data searched_controller->joints_.clear(); @@ -671,11 +671,11 @@ bool ROSControllersWidget::saveControllerScreen() if (!current_edit_controller_.empty()) { // Find the controller we are editing based on the goup name string - searched_controller = config_data_->findROSControllerByName(current_edit_controller_); + searched_controller = config_data_->findControllerByName(current_edit_controller_); } // Check that the controller name is unique - for (const auto& controller : config_data_->getROSControllers()) + for (const auto& controller : config_data_->getControllers()) { if (controller.name_.compare(controller_name) == 0) // the names are the same { @@ -696,7 +696,7 @@ bool ROSControllersWidget::saveControllerScreen() moveit_setup_assistant::ControllerConfig new_controller; new_controller.name_ = controller_name; new_controller.type_ = controller_type; - config_data_->addROSController(new_controller); + config_data_->addController(new_controller); adding_new_controller_ = true; // remember this controller is new } @@ -742,7 +742,7 @@ void ROSControllersWidget::editSelected() controller_item = item->parent()->parent(); current_edit_controller_ = controller_item->text(0).toUtf8().constData(); moveit_setup_assistant::ControllerConfig* this_controller = - config_data_->findROSControllerByName(current_edit_controller_); + config_data_->findControllerByName(current_edit_controller_); // Load the data loadJointsScreen(this_controller); @@ -755,7 +755,7 @@ void ROSControllersWidget::editSelected() controller_item = item->parent(); current_edit_controller_ = controller_item->text(0).toUtf8().constData(); moveit_setup_assistant::ControllerConfig* this_controller = - config_data_->findROSControllerByName(current_edit_controller_); + config_data_->findControllerByName(current_edit_controller_); // Load the data loadJointsScreen(this_controller); @@ -768,7 +768,7 @@ void ROSControllersWidget::editSelected() // Load the data current_edit_controller_ = item->text(0).toUtf8().constData(); moveit_setup_assistant::ControllerConfig* this_controller = - config_data_->findROSControllerByName(current_edit_controller_); + config_data_->findControllerByName(current_edit_controller_); loadControllerScreen(this_controller); // Switch to screen @@ -793,7 +793,7 @@ void ROSControllersWidget::editController() adding_new_controller_ = false; - loadControllerScreen(config_data_->findROSControllerByName(current_edit_controller_)); + loadControllerScreen(config_data_->findControllerByName(current_edit_controller_)); // Switch to screen changeScreen(2); // 1 is index of controller edit diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp index 941cf320ec..267d216a58 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp @@ -428,7 +428,7 @@ bool StartScreenWidget::loadExistingFiles() // Load 3d_sensors config file load3DSensorsFile(); - // Load ros controllers yaml file if available----------------------------------------------- + // Load ros_controllers.yaml file if available----------------------------------------------- fs::path ros_controllers_yaml_path = config_data_->config_pkg_path_; ros_controllers_yaml_path /= "config/ros_controllers.yaml"; config_data_->inputROSControllersYAML(ros_controllers_yaml_path.make_preferred().string()); diff --git a/moveit_setup_assistant/test/moveit_config_data_test.cpp b/moveit_setup_assistant/test/moveit_config_data_test.cpp index f89316060f..278b40b061 100644 --- a/moveit_setup_assistant/test/moveit_config_data_test.cpp +++ b/moveit_setup_assistant/test/moveit_config_data_test.cpp @@ -74,7 +74,7 @@ TEST_F(MoveItConfigData, ReadingControllers) config_data->setRobotModel(robot_model_); // Initially no controllers - EXPECT_EQ(config_data->getROSControllers().size(), 0u); + EXPECT_EQ(config_data->getControllers().size(), 0u); // Adding default controllers, a controller for each planning group config_data->addDefaultControllers(); @@ -83,7 +83,7 @@ TEST_F(MoveItConfigData, ReadingControllers) size_t group_count = config_data->srdf_->srdf_model_->getGroups().size(); // Test that addDefaultControllers() did accually add a controller for the new_group - EXPECT_EQ(config_data->getROSControllers().size(), group_count); + EXPECT_EQ(config_data->getControllers().size(), group_count); // Temporary file used during the test and is deleted when the test is finished char test_file[] = "/tmp/msa_unittest_ros_controller.yaml"; @@ -95,13 +95,13 @@ TEST_F(MoveItConfigData, ReadingControllers) config_data = std::make_shared(); // Initially no controllers - EXPECT_EQ(config_data->getROSControllers().size(), 0u); + EXPECT_EQ(config_data->getControllers().size(), 0u); // ros_controllers.yaml read correctly EXPECT_EQ(config_data->inputROSControllersYAML(test_file), true); // ros_controllers.yaml parsed correctly - EXPECT_EQ(config_data->getROSControllers().size(), group_count); + EXPECT_EQ(config_data->getControllers().size(), group_count); // Remove ros_controllers.yaml temp file which was used in testing boost::filesystem::remove(test_file); From 2c243c6e845308c5be4374f8efa17436fd1a7b21 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 13:14:15 +0100 Subject: [PATCH 066/229] Rename ros_controllers_config_ -> controller_configs_ --- .../tools/moveit_config_data.h | 15 ++++++---- .../src/tools/moveit_config_data.cpp | 30 +++++++------------ .../src/widgets/ros_controllers_widget.cpp | 2 +- 3 files changed, 21 insertions(+), 26 deletions(-) diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h index 71b640495c..46876d8e88 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h @@ -387,7 +387,7 @@ class MoveItConfigData /** * \brief Add a Follow Joint Trajectory action Controller for each Planning Group - * \return true if controllers were added to the ros_controllers_config_ data structure + * \return true if controllers were added to the controller_configs_ data structure */ bool addDefaultControllers(); @@ -449,17 +449,20 @@ class MoveItConfigData std::string appendPaths(const std::string& path1, const std::string& path2); /** - * \brief Adds a controller to ros_controllers_config_ vector + * \brief Adds a controller to controller_configs_ vector * \param new_controller a new Controller to add * \return true if inserted correctly */ bool addController(const ControllerConfig& new_controller); /** - * \brief Gets ros_controllers_config_ vector - * \return pointer to ros_controllers_config_ + * \brief Gets controller_configs_ vector + * \return pointer to controller_configs_ */ - std::vector& getControllers(); + std::vector& getControllers() + { + return controller_configs_; + } /** * Find the associated controller by name @@ -524,7 +527,7 @@ class MoveItConfigData moveit::core::RobotModelPtr robot_model_; /// Controllers config data - std::vector ros_controllers_config_; + std::vector controller_configs_; /// Shared planning scene planning_scene::PlanningScenePtr planning_scene_; diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 2852dc68f9..a76a587e68 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -374,7 +374,7 @@ bool MoveItConfigData::outputKinematicsYAML(const std::string& file_path) // ****************************************************************************************** std::string MoveItConfigData::getJointHardwareInterface(const std::string& joint_name) { - for (ControllerConfig& ros_control_config : ros_controllers_config_) + for (ControllerConfig& ros_control_config : controller_configs_) { std::vector::iterator joint_it = std::find(ros_control_config.joints_.begin(), ros_control_config.joints_.end(), joint_name); @@ -829,7 +829,7 @@ bool MoveItConfigData::outputSimpleControllersYAML(const std::string& file_path) emitter << YAML::BeginMap; emitter << YAML::Key << "controller_list"; emitter << YAML::Value << YAML::BeginSeq; - for (const auto& controller : ros_controllers_config_) + for (const auto& controller : controller_configs_) { // Only process FollowJointTrajectory types if (controller.type_ == "FollowJointTrajectory" || controller.type_ == "GripperCommand") @@ -980,7 +980,7 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) emitter << YAML::EndMap; } #endif - for (const auto& controller : ros_controllers_config_) + for (const auto& controller : controller_configs_) { if (controller.type_ == "FollowJointTrajectory" || controller.type_ == "GripperCommand") continue; // these are handled by outputSimpleControllersYAML() @@ -1431,7 +1431,7 @@ bool MoveItConfigData::parseROSController(const YAML::Node& controller) return false; } // All required fields were parsed correctly - ros_controllers_config_.push_back(control_setting); + controller_configs_.push_back(control_setting); } else { @@ -1491,7 +1491,7 @@ bool MoveItConfigData::processROSControllers(std::ifstream& input_stream) { control_setting.type_ = controller_it->second["type"].as(); control_setting.name_ = controller_name; - ros_controllers_config_.push_back(control_setting); + controller_configs_.push_back(control_setting); control_setting.joints_.clear(); } } @@ -1888,7 +1888,7 @@ ControllerConfig* MoveItConfigData::findControllerByName(const std::string& cont // Find the ROSController we are editing based on the ROSController name string ControllerConfig* searched_ros_controller = nullptr; // used for holding our search results - for (ControllerConfig& ros_control_config : ros_controllers_config_) + for (ControllerConfig& ros_control_config : controller_configs_) { if (ros_control_config.name_ == controller_name) // string match { @@ -1905,12 +1905,12 @@ ControllerConfig* MoveItConfigData::findControllerByName(const std::string& cont // ****************************************************************************************** bool MoveItConfigData::deleteController(const std::string& controller_name) { - for (std::vector::iterator controller_it = ros_controllers_config_.begin(); - controller_it != ros_controllers_config_.end(); ++controller_it) + for (std::vector::iterator controller_it = controller_configs_.begin(); + controller_it != controller_configs_.end(); ++controller_it) { if (controller_it->name_ == controller_name) // string match { - ros_controllers_config_.erase(controller_it); + controller_configs_.erase(controller_it); // we are done searching return true; } @@ -1919,7 +1919,7 @@ bool MoveItConfigData::deleteController(const std::string& controller_name) } // ****************************************************************************************** -// Adds a controller to ros_controllers_config_ vector +// Adds a controller to controller_configs_ vector // ****************************************************************************************** bool MoveItConfigData::addController(const ControllerConfig& new_controller) { @@ -1932,18 +1932,10 @@ bool MoveItConfigData::addController(const ControllerConfig& new_controller) if (searched_ros_controller && searched_ros_controller->type_ == new_controller.type_) return false; - ros_controllers_config_.push_back(new_controller); + controller_configs_.push_back(new_controller); return true; } -// ****************************************************************************************** -// Gets ros_controllers_config_ vector -// ****************************************************************************************** -std::vector& MoveItConfigData::getControllers() -{ - return ros_controllers_config_; -} - // ****************************************************************************************** // Used to add a sensor plugin configuation parameter to the sensor plugin configuration parameter list // ****************************************************************************************** diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp index a550e7a01c..9e52f8353e 100644 --- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp +++ b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp @@ -203,7 +203,7 @@ QWidget* ROSControllersWidget::createContentsWidget() } // ****************************************************************************************** -// Displays data in the ros_controllers_config_ data structure into a QtTableWidget +// Displays data in the controller_configs_ data structure into a QtTableWidget // ****************************************************************************************** void ROSControllersWidget::loadControllersTree() { From 8fbd8a09ce91b6cf785d60afbdc3ea0fbfb3aef9 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 13:16:36 +0100 Subject: [PATCH 067/229] Rename files ros_controllers_widget.* -> controllers_widget.* --- moveit_setup_assistant/CMakeLists.txt | 4 ++-- .../{ros_controllers_widget.cpp => controllers_widget.cpp} | 2 +- .../{ros_controllers_widget.h => controllers_widget.h} | 0 moveit_setup_assistant/src/widgets/setup_assistant_widget.h | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) rename moveit_setup_assistant/src/widgets/{ros_controllers_widget.cpp => controllers_widget.cpp} (99%) rename moveit_setup_assistant/src/widgets/{ros_controllers_widget.h => controllers_widget.h} (100%) diff --git a/moveit_setup_assistant/CMakeLists.txt b/moveit_setup_assistant/CMakeLists.txt index 58909804de..229e53c7c1 100644 --- a/moveit_setup_assistant/CMakeLists.txt +++ b/moveit_setup_assistant/CMakeLists.txt @@ -69,7 +69,7 @@ set(HEADERS src/widgets/virtual_joints_widget.h src/widgets/passive_joints_widget.h src/widgets/perception_widget.h - src/widgets/ros_controllers_widget.h + src/widgets/controllers_widget.h src/widgets/controller_edit_widget.h src/widgets/configuration_files_widget.h src/widgets/setup_screen_widget.h @@ -107,7 +107,7 @@ add_library(${PROJECT_NAME}_widgets src/widgets/passive_joints_widget.cpp src/widgets/perception_widget.cpp src/widgets/configuration_files_widget.cpp - src/widgets/ros_controllers_widget.cpp + src/widgets/controllers_widget.cpp src/widgets/controller_edit_widget.cpp src/widgets/navigation_widget.cpp src/widgets/header_widget.cpp diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp b/moveit_setup_assistant/src/widgets/controllers_widget.cpp similarity index 99% rename from moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp rename to moveit_setup_assistant/src/widgets/controllers_widget.cpp index 9e52f8353e..3c65cb127e 100644 --- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp +++ b/moveit_setup_assistant/src/widgets/controllers_widget.cpp @@ -34,7 +34,7 @@ /* Author: Mohamad Ayman */ // SA -#include "ros_controllers_widget.h" +#include "controllers_widget.h" #include "double_list_widget.h" #include "controller_edit_widget.h" #include "header_widget.h" diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.h b/moveit_setup_assistant/src/widgets/controllers_widget.h similarity index 100% rename from moveit_setup_assistant/src/widgets/ros_controllers_widget.h rename to moveit_setup_assistant/src/widgets/controllers_widget.h diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.h b/moveit_setup_assistant/src/widgets/setup_assistant_widget.h index 154f2dc0b4..71edf27bba 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.h +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.h @@ -52,7 +52,7 @@ class QSplitter; #include "simulation_widget.h" #include "configuration_files_widget.h" #include "perception_widget.h" -#include "ros_controllers_widget.h" +#include "controllers_widget.h" #ifndef Q_MOC_RUN #include From 89c6ba70732191d53b3533771cffc34267e3cc04 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 13:30:25 +0100 Subject: [PATCH 068/229] Rename ROSControllersWidget -> ControllersWidget --- .../src/widgets/controllers_widget.cpp | 54 +++++++++---------- .../src/widgets/controllers_widget.h | 4 +- .../src/widgets/setup_assistant_widget.cpp | 2 +- .../src/widgets/setup_assistant_widget.h | 2 +- 4 files changed, 31 insertions(+), 31 deletions(-) diff --git a/moveit_setup_assistant/src/widgets/controllers_widget.cpp b/moveit_setup_assistant/src/widgets/controllers_widget.cpp index 3c65cb127e..6560c17ca3 100644 --- a/moveit_setup_assistant/src/widgets/controllers_widget.cpp +++ b/moveit_setup_assistant/src/widgets/controllers_widget.cpp @@ -65,7 +65,7 @@ namespace moveit_setup_assistant // ****************************************************************************************** // Outer User Interface for MoveIt Configuration Assistant // ****************************************************************************************** -ROSControllersWidget::ROSControllersWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) +ControllersWidget::ControllersWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) : SetupScreenWidget(parent), config_data_(config_data) { // Basic widget container @@ -122,7 +122,7 @@ ROSControllersWidget::ROSControllersWidget(QWidget* parent, const MoveItConfigDa // ****************************************************************************************** // Create the main tree view widget // ****************************************************************************************** -QWidget* ROSControllersWidget::createContentsWidget() +QWidget* ControllersWidget::createContentsWidget() { // Main widget QWidget* content_widget = new QWidget(this); @@ -205,7 +205,7 @@ QWidget* ROSControllersWidget::createContentsWidget() // ****************************************************************************************** // Displays data in the controller_configs_ data structure into a QtTableWidget // ****************************************************************************************** -void ROSControllersWidget::loadControllersTree() +void ControllersWidget::loadControllersTree() { // Disable Tree controllers_tree_->setUpdatesEnabled(false); // prevent table from updating until we are completely done @@ -228,7 +228,7 @@ void ROSControllersWidget::loadControllersTree() // ****************************************************************************************** // Displays data in the controller_config_ data structure into a QtTableWidget // ****************************************************************************************** -void ROSControllersWidget::loadToControllersTree(const moveit_setup_assistant::ControllerConfig& controller_it) +void ControllersWidget::loadToControllersTree(const moveit_setup_assistant::ControllerConfig& controller_it) { // Fonts for tree const QFont top_level_font(QFont().defaultFamily(), 11, QFont::Bold); @@ -274,7 +274,7 @@ void ROSControllersWidget::loadToControllersTree(const moveit_setup_assistant::C // ****************************************************************************************** // Called when setup assistant navigation switches to this screen // ****************************************************************************************** -void ROSControllersWidget::focusGiven() +void ControllersWidget::focusGiven() { // load controllers tree btn_edit_->setEnabled(false); @@ -285,7 +285,7 @@ void ROSControllersWidget::focusGiven() // ****************************************************************************************** // Load the popup screen with correct data for joints // ****************************************************************************************** -void ROSControllersWidget::loadJointsScreen(moveit_setup_assistant::ControllerConfig* this_controller) +void ControllersWidget::loadJointsScreen(moveit_setup_assistant::ControllerConfig* this_controller) { // Retrieve pointer to the shared kinematic model const moveit::core::RobotModelConstPtr& model = config_data_->getRobotModel(); @@ -316,7 +316,7 @@ void ROSControllersWidget::loadJointsScreen(moveit_setup_assistant::ControllerCo // ****************************************************************************************** // Load the popup screen with correct data for gcroups // ****************************************************************************************** -void ROSControllersWidget::loadGroupsScreen(moveit_setup_assistant::ControllerConfig* this_controller) +void ControllersWidget::loadGroupsScreen(moveit_setup_assistant::ControllerConfig* this_controller) { // Load all groups into the subgroup screen std::vector groups; @@ -345,7 +345,7 @@ void ROSControllersWidget::loadGroupsScreen(moveit_setup_assistant::ControllerCo // ****************************************************************************************** // Delete a controller // ****************************************************************************************** -void ROSControllersWidget::deleteController() +void ControllersWidget::deleteController() { std::string controller_name = current_edit_controller_; @@ -392,7 +392,7 @@ void ROSControllersWidget::deleteController() // ****************************************************************************************** // Create a new controller // ****************************************************************************************** -void ROSControllersWidget::addController() +void ControllersWidget::addController() { // New Controller adding_new_controller_ = true; @@ -405,7 +405,7 @@ void ROSControllersWidget::addController() // ****************************************************************************************** // Add a Follow Joint Trajectory action Controller for each Planning Group // ****************************************************************************************** -void ROSControllersWidget::addDefaultControllers() +void ControllersWidget::addDefaultControllers() { if (!config_data_->addDefaultControllers()) QMessageBox::warning(this, "Error adding contollers", "No Planning Groups configured!"); @@ -415,7 +415,7 @@ void ROSControllersWidget::addDefaultControllers() // ****************************************************************************************** // Load the popup screen with correct data for controllers // ****************************************************************************************** -void ROSControllersWidget::loadControllerScreen(moveit_setup_assistant::ControllerConfig* this_controller) +void ControllersWidget::loadControllerScreen(moveit_setup_assistant::ControllerConfig* this_controller) { // Load the avail controllers. This function only runs once controller_edit_widget_->loadControllersTypesComboBox(); @@ -444,7 +444,7 @@ void ROSControllersWidget::loadControllerScreen(moveit_setup_assistant::Controll // ****************************************************************************************** // Call when edit screen is canceled // ****************************************************************************************** -void ROSControllersWidget::cancelEditing() +void ControllersWidget::cancelEditing() { if (!current_edit_controller_.empty() && adding_new_controller_) { @@ -470,7 +470,7 @@ void ROSControllersWidget::cancelEditing() // ****************************************************************************************** // Called from Double List widget to highlight joints // ****************************************************************************************** -void ROSControllersWidget::previewSelectedJoints(const std::vector& joints) +void ControllersWidget::previewSelectedJoints(const std::vector& joints) { // Unhighlight all links Q_EMIT unhighlightAll(); @@ -501,7 +501,7 @@ void ROSControllersWidget::previewSelectedJoints(const std::vector& // ****************************************************************************************** // Called from Double List widget to highlight a group // ****************************************************************************************** -void ROSControllersWidget::previewSelectedGroup(const std::vector& groups) +void ControllersWidget::previewSelectedGroup(const std::vector& groups) { // Unhighlight all links Q_EMIT unhighlightAll(); @@ -516,7 +516,7 @@ void ROSControllersWidget::previewSelectedGroup(const std::vector& // ****************************************************************************************** // Called when an item is seleceted from the controllers tree // ****************************************************************************************** -void ROSControllersWidget::previewSelected(QTreeWidgetItem* selected_item, int /*column*/) +void ControllersWidget::previewSelected(QTreeWidgetItem* selected_item, int /*column*/) { // Get the user custom properties of the currently selected row int type = selected_item->data(0, Qt::UserRole).value(); @@ -536,7 +536,7 @@ void ROSControllersWidget::previewSelected(QTreeWidgetItem* selected_item, int / // ****************************************************************************************** // Call when a new controller is created and ready to progress to next screen // ****************************************************************************************** -void ROSControllersWidget::saveControllerScreenJoints() +void ControllersWidget::saveControllerScreenJoints() { // Save the controller if (!saveControllerScreen()) @@ -555,7 +555,7 @@ void ROSControllersWidget::saveControllerScreenJoints() // ****************************************************************************************** // Call when a new controller is created and ready to progress to next screen // ****************************************************************************************** -void ROSControllersWidget::saveControllerScreenGroups() +void ControllersWidget::saveControllerScreenGroups() { // Save the controller if (!saveControllerScreen()) @@ -574,7 +574,7 @@ void ROSControllersWidget::saveControllerScreenGroups() // ****************************************************************************************** // Call when joints edit sceen is done and needs to be saved // ****************************************************************************************** -void ROSControllersWidget::saveJointsScreen() +void ControllersWidget::saveJointsScreen() { // Find the controller we are editing based on the controller name string moveit_setup_assistant::ControllerConfig* searched_controller = @@ -599,7 +599,7 @@ void ROSControllersWidget::saveJointsScreen() // ****************************************************************************************** // Call when joints edit sceen is done and needs to be saved // ****************************************************************************************** -void ROSControllersWidget::saveJointsGroupsScreen() +void ControllersWidget::saveJointsGroupsScreen() { // Find the controller we are editing based on the controller name string moveit_setup_assistant::ControllerConfig* searched_controller = @@ -635,7 +635,7 @@ void ROSControllersWidget::saveJointsGroupsScreen() // ****************************************************************************************** // Call when joints edit sceen is done and needs to be saved // ****************************************************************************************** -void ROSControllersWidget::saveControllerScreenEdit() +void ControllersWidget::saveControllerScreenEdit() { // Save the controller if (!saveControllerScreen()) @@ -648,7 +648,7 @@ void ROSControllersWidget::saveControllerScreenEdit() // ****************************************************************************************** // Call when controller edit sceen is done and needs to be saved // ****************************************************************************************** -bool ROSControllersWidget::saveControllerScreen() +bool ControllersWidget::saveControllerScreen() { // Get a reference to the supplied strings const std::string& controller_name = controller_edit_widget_->getControllerName(); @@ -722,7 +722,7 @@ bool ROSControllersWidget::saveControllerScreen() // ****************************************************************************************** // Edit whenever element is selected in the controllers tree view // ****************************************************************************************** -void ROSControllersWidget::editSelected() +void ControllersWidget::editSelected() { QTreeWidgetItem* item = controllers_tree_->currentItem(); QTreeWidgetItem* controller_item; @@ -783,7 +783,7 @@ void ROSControllersWidget::editSelected() // ****************************************************************************************** // Edit a controller // ****************************************************************************************** -void ROSControllersWidget::editController() +void ControllersWidget::editController() { QTreeWidgetItem* item = controllers_tree_->currentItem(); @@ -802,7 +802,7 @@ void ROSControllersWidget::editController() // ****************************************************************************************** // Switch to current controllers view // ****************************************************************************************** -void ROSControllersWidget::showMainScreen() +void ControllersWidget::showMainScreen() { stacked_widget_->setCurrentIndex(0); @@ -813,7 +813,7 @@ void ROSControllersWidget::showMainScreen() // ****************************************************************************************** // Switch which screen is being shown // ****************************************************************************************** -void ROSControllersWidget::changeScreen(int index) +void ControllersWidget::changeScreen(int index) { stacked_widget_->setCurrentIndex(index); @@ -824,7 +824,7 @@ void ROSControllersWidget::changeScreen(int index) // ****************************************************************************************** // Expand/Collapse Tree // ****************************************************************************************** -void ROSControllersWidget::alterTree(const QString& link) +void ControllersWidget::alterTree(const QString& link) { if (link.contains("expand")) controllers_tree_->expandAll(); @@ -832,7 +832,7 @@ void ROSControllersWidget::alterTree(const QString& link) controllers_tree_->collapseAll(); } -void ROSControllersWidget::itemSelectionChanged() +void ControllersWidget::itemSelectionChanged() { QList selected_items = controllers_tree_->selectedItems(); if (selected_items.empty()) diff --git a/moveit_setup_assistant/src/widgets/controllers_widget.h b/moveit_setup_assistant/src/widgets/controllers_widget.h index bfd0bbbf37..5ab5c4d2aa 100644 --- a/moveit_setup_assistant/src/widgets/controllers_widget.h +++ b/moveit_setup_assistant/src/widgets/controllers_widget.h @@ -54,7 +54,7 @@ namespace moveit_setup_assistant class DoubleListWidget; class ControllerEditWidget; -class ROSControllersWidget : public SetupScreenWidget +class ControllersWidget : public SetupScreenWidget { Q_OBJECT @@ -63,7 +63,7 @@ class ROSControllersWidget : public SetupScreenWidget // Public Functions // ****************************************************************************************** - ROSControllersWidget(QWidget* parent, const MoveItConfigDataPtr& config_data); + ControllersWidget(QWidget* parent, const MoveItConfigDataPtr& config_data); void changeScreen(int index); diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp index 4604629940..1d2c25a56e 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp @@ -297,7 +297,7 @@ void SetupAssistantWidget::progressPastStartScreen() connect(passive_joints_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll())); // ROS Controllers - controllers_widget_ = new ROSControllersWidget(this, config_data_); + controllers_widget_ = new ControllersWidget(this, config_data_); main_content_->addWidget(controllers_widget_); connect(controllers_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool))); connect(controllers_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this, diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.h b/moveit_setup_assistant/src/widgets/setup_assistant_widget.h index 71edf27bba..ffae4a7924 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.h +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.h @@ -215,7 +215,7 @@ private Q_SLOTS: ConfigurationFilesWidget* configuration_files_widget_; SimulationWidget* simulation_widget_; PerceptionWidget* perception_widget_; - ROSControllersWidget* controllers_widget_; + ControllersWidget* controllers_widget_; /// Contains all the configuration data for the setup assistant MoveItConfigDataPtr config_data_; From 42fafe59e367d2d350058a0d8a2d5a8dbf6b4644 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 13:33:27 +0100 Subject: [PATCH 069/229] Update widget texts to speak about generic controllers --- moveit_setup_assistant/src/widgets/controllers_widget.cpp | 7 ++++--- .../src/widgets/setup_assistant_widget.cpp | 4 ++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/moveit_setup_assistant/src/widgets/controllers_widget.cpp b/moveit_setup_assistant/src/widgets/controllers_widget.cpp index 6560c17ca3..f2d1bad8a3 100644 --- a/moveit_setup_assistant/src/widgets/controllers_widget.cpp +++ b/moveit_setup_assistant/src/widgets/controllers_widget.cpp @@ -74,11 +74,12 @@ ControllersWidget::ControllersWidget(QWidget* parent, const MoveItConfigDataPtr& layout->setAlignment(Qt::AlignTop); // Title - this->setWindowTitle("ROS Control Controllers"); // title of window + this->setWindowTitle("Controller Configuration"); // title of window // Top Header Area ------------------------------------------------ moveit_setup_assistant::HeaderWidget* header = new moveit_setup_assistant::HeaderWidget( - "Setup ROS Controllers", "Configure MoveIt to work with ROS Control to control the robot's physical hardware", + "Setup Controllers", + "Configure controllers to be used by MoveIt's controller manager(s) to operate the robot's physical hardware", this); layout->addWidget(header); @@ -663,7 +664,7 @@ bool ControllersWidget::saveControllerScreen() // Check that a valid controller name has been given if (controller_name.empty() || std::regex_search(controller_name, invalid_name_match, invalid_reg_ex)) { - QMessageBox::warning(this, "Error Saving", "Invalid ROS controller name"); + QMessageBox::warning(this, "Error Saving", "Invalid controller name"); return false; } diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp index 1d2c25a56e..98bd321f37 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp @@ -123,7 +123,7 @@ SetupAssistantWidget::SetupAssistantWidget(QWidget* parent, const boost::program nav_name_list_ << "Robot Poses"; nav_name_list_ << "End Effectors"; nav_name_list_ << "Passive Joints"; - nav_name_list_ << "ROS Control"; + nav_name_list_ << "Controllers"; nav_name_list_ << "Simulation"; nav_name_list_ << "3D Perception"; nav_name_list_ << "Author Information"; @@ -296,7 +296,7 @@ void SetupAssistantWidget::progressPastStartScreen() SLOT(highlightGroup(const std::string&))); connect(passive_joints_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll())); - // ROS Controllers + // Controllers controllers_widget_ = new ControllersWidget(this, config_data_); main_content_->addWidget(controllers_widget_); connect(controllers_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool))); From 5e21f026ab29ff7d88df78fee27aed394320d196 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 13:26:38 +0100 Subject: [PATCH 070/229] Simplify code --- .../src/tools/moveit_config_data.cpp | 22 ++++++------------- 1 file changed, 7 insertions(+), 15 deletions(-) diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index a76a587e68..6422b97eb4 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -1885,19 +1885,14 @@ srdf::Model::Group* MoveItConfigData::findGroupByName(const std::string& name) // ****************************************************************************************** ControllerConfig* MoveItConfigData::findControllerByName(const std::string& controller_name) { - // Find the ROSController we are editing based on the ROSController name string - ControllerConfig* searched_ros_controller = nullptr; // used for holding our search results - - for (ControllerConfig& ros_control_config : controller_configs_) + // Find the controller we are editing based on its name + for (ControllerConfig& controller : controller_configs_) { - if (ros_control_config.name_ == controller_name) // string match - { - searched_ros_controller = &ros_control_config; // convert to pointer from iterator - break; // we are done searching - } + if (controller.name_ == controller_name) // string match + return &controller; // convert to pointer from iterator } - return searched_ros_controller; + return nullptr; // not found } // ****************************************************************************************** @@ -1923,13 +1918,10 @@ bool MoveItConfigData::deleteController(const std::string& controller_name) // ****************************************************************************************** bool MoveItConfigData::addController(const ControllerConfig& new_controller) { - // Used for holding our search results - ControllerConfig* searched_ros_controller = nullptr; - // Find if there is an existing controller with the same name - searched_ros_controller = findControllerByName(new_controller.name_); + ControllerConfig* controller = findControllerByName(new_controller.name_); - if (searched_ros_controller && searched_ros_controller->type_ == new_controller.type_) + if (controller && controller->type_ == new_controller.type_) return false; controller_configs_.push_back(new_controller); From c56d86a4dbf867f09e6392be01b3804ea76343f0 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 22:24:29 +0100 Subject: [PATCH 071/229] Fix controller choice - Provide all types of JointTrajectoryController as well as FollowJointTrajectory and GripperCommand (use by simple manager) - Use effort_controllers/JointTrajectoryController as default - Create FollowJointTrajectory entries for any JointTrajectoryController --- .../setup_assistant/tools/moveit_config_data.h | 2 +- .../src/tools/moveit_config_data.cpp | 15 +++++++++------ .../src/widgets/configuration_files_widget.cpp | 2 +- .../src/widgets/controller_edit_widget.cpp | 14 ++++---------- 4 files changed, 15 insertions(+), 18 deletions(-) diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h index 46876d8e88..c85d2095ff 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h @@ -389,7 +389,7 @@ class MoveItConfigData * \brief Add a Follow Joint Trajectory action Controller for each Planning Group * \return true if controllers were added to the controller_configs_ data structure */ - bool addDefaultControllers(); + bool addDefaultControllers(const std::string& controller_type = "effort_controllers/JointTrajectoryController"); /** * Set package path; try to resolve path from package name if directory does not exist diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 6422b97eb4..82ada1aaff 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -41,6 +41,7 @@ #include // for creating folders/files #include // is_regular_file, is_directory, etc. #include +#include // ROS #include @@ -832,16 +833,18 @@ bool MoveItConfigData::outputSimpleControllersYAML(const std::string& file_path) for (const auto& controller : controller_configs_) { // Only process FollowJointTrajectory types - if (controller.type_ == "FollowJointTrajectory" || controller.type_ == "GripperCommand") + std::string type = controller.type_; + if (boost::ends_with(type, "/JointTrajectoryController")) + type = "FollowJointTrajectory"; + if (type == "FollowJointTrajectory" || type == "GripperCommand") { emitter << YAML::BeginMap; emitter << YAML::Key << "name"; emitter << YAML::Value << controller.name_; emitter << YAML::Key << "action_ns"; - emitter << YAML::Value - << (controller.type_ == "FollowJointTrajectory" ? "follow_joint_trajectory" : "gripper_action"); + emitter << YAML::Value << (type == "FollowJointTrajectory" ? "follow_joint_trajectory" : "gripper_action"); emitter << YAML::Key << "type"; - emitter << YAML::Value << controller.type_; + emitter << YAML::Value << type; emitter << YAML::Key << "default"; emitter << YAML::Value << "True"; @@ -1530,7 +1533,7 @@ bool MoveItConfigData::inputROSControllersYAML(const std::string& file_path) // ****************************************************************************************** // Add a Follow Joint Trajectory action Controller for each Planning Group // ****************************************************************************************** -bool MoveItConfigData::addDefaultControllers() +bool MoveItConfigData::addDefaultControllers(const std::string& controller_type) { if (srdf_->srdf_model_->getGroups().empty()) return false; @@ -1552,7 +1555,7 @@ bool MoveItConfigData::addDefaultControllers() if (!group_controller.joints_.empty()) { group_controller.name_ = group_it.name_ + "_controller"; - group_controller.type_ = "FollowJointTrajectory"; + group_controller.type_ = controller_type; addController(group_controller); } } diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 4b1b71f175..64e66573e9 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -1149,7 +1149,7 @@ void ConfigurationFilesWidget::loadTemplateStrings() addTemplateString("[OTHER_DEPENDENCIES]", deps.str()); // not relative to a ROS package } - // Pair 9 - Add ROS Controllers to ros_controllers.launch file + // Pair 9 - List of ROS Controllers to load in ros_controllers.launch file if (config_data_->getControllers().empty()) { addTemplateString("[ROS_CONTROLLERS]", ""); diff --git a/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp b/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp index 078af7a562..fe7894064f 100644 --- a/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp +++ b/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp @@ -194,20 +194,14 @@ void ControllerEditWidget::loadControllersTypesComboBox() return; has_loaded_ = true; - const std::array default_types = { - "effort_controllers/JointTrajectoryController", "effort_controllers/JointPositionController", - "effort_controllers/JointVelocityController", "effort_controllers/JointEffortController", - "position_controllers/JointPositionController", "position_controllers/JointTrajectoryController", - "velocity_controllers/JointTrajectoryController", "velocity_controllers/JointVelocityController", - "pos_vel_controllers/JointTrajectoryController", "pos_vel_acc_controllers/JointTrajectoryController" - }; + const std::vector default_types = { "effort_controllers/JointTrajectoryController", + "velocity_controllers/JointTrajectoryController", + "position_controllers/JointTrajectoryController", + "FollowJointTrajectory", "GripperCommand" }; // Remove all old items controller_type_field_->clear(); - // Add FollowJointTrajectory option, the default - controller_type_field_->addItem("FollowJointTrajectory"); - // Loop through all controller default_types and add to combo box for (const std::string& type : default_types) controller_type_field_->addItem(type.c_str()); From 42c48ac61d202e7c24f68b0f4cf67a2f36dd5234 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 09:51:52 +0100 Subject: [PATCH 072/229] Formatting --- .../src/tools/moveit_config_data.cpp | 2 +- .../launch/chomp_planning_pipeline.launch.xml | 15 +++++++-------- .../moveit_config_pkg_template/launch/demo.launch | 2 +- .../launch/demo_gazebo.launch | 2 +- .../launch/move_group.launch | 3 ++- .../launch/ompl_planning_pipeline.launch.xml | 15 +++++++-------- ...al_motion_planner_planning_pipeline.launch.xml | 7 +++---- .../launch/trajopt_planning_pipeline.launch.xml | 14 +++++++------- .../package.xml.template | 2 +- 9 files changed, 30 insertions(+), 32 deletions(-) diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index e90d555edb..905d22bd57 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -535,7 +535,7 @@ bool MoveItConfigData::outputFakeControllersYAML(const std::string& file_path) emitter << YAML::EndSeq; // Add an initial pose for each group - emitter << YAML::Key << "initial" << YAML::Comment("Define initial robot poses."); + emitter << YAML::Key << "initial" << YAML::Comment("Define initial robot poses per group"); bool poses_found = false; std::string default_group_name; diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml index 80ff7e8b31..e46e78270f 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml @@ -4,16 +4,15 @@ - + + default_planner_request_adapters/ResolveConstraintFrames + default_planner_request_adapters/FixWorkspaceBounds + default_planner_request_adapters/FixStartStateBounds + default_planner_request_adapters/FixStartStateCollision + default_planner_request_adapters/FixStartStatePathConstraints" + /> diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch index 5b71919cc5..e0491b4239 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch @@ -57,7 +57,7 @@ - + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch index bfbd69772d..9cd5e3bcbe 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch @@ -35,7 +35,7 @@ - [VIRTUAL_JOINT_BROADCASTER] +[VIRTUAL_JOINT_BROADCASTER] 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 4336184fcf..bee3b0c767 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 @@ -62,7 +62,8 @@ - + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml index 2e2561f8bd..b7c36376e7 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml @@ -3,16 +3,15 @@ - + + default_planner_request_adapters/ResolveConstraintFrames + default_planner_request_adapters/FixWorkspaceBounds + default_planner_request_adapters/FixStartStateBounds + default_planner_request_adapters/FixStartStateCollision + default_planner_request_adapters/FixStartStatePathConstraints" + /> diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml index b6bf529185..75aabcccea 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -3,14 +3,13 @@ - + - + + pilz_industrial_motion_planner/MoveGroupSequenceService" /> diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml index f86030852c..b7dc6fdccd 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml @@ -9,13 +9,13 @@ - - + + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template b/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template index 7db6b8c009..4ecf18505a 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template @@ -35,6 +35,6 @@ - [OTHER_DEPENDENCIES] +[OTHER_DEPENDENCIES] From b95ba80346dd9d87151dd1052af0ef1385302152 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 09:52:17 +0100 Subject: [PATCH 073/229] demo.launch: start joint + robot-state publishers in fake mode only This will facilitate re-use of demo.launch. --- .../launch/demo.launch | 33 +++++++++---------- 1 file changed, 16 insertions(+), 17 deletions(-) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch index e0491b4239..0f2fc8ab38 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch @@ -19,30 +19,29 @@ - + [VIRTUAL_JOINT_BROADCASTER] - - - [move_group/fake_controller_joint_states] - - - [move_group/fake_controller_joint_states] - - - + + + + [move_group/fake_controller_joint_states] + + + + [move_group/fake_controller_joint_states] + + + + + From ce8e8249795a5028a78787c86002d1d2ae9281a9 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 10:02:16 +0100 Subject: [PATCH 074/229] Modularize demo_gazebo.launch: draw on demo.launch --- .../launch/demo_gazebo.launch | 61 ++----------------- 1 file changed, 6 insertions(+), 55 deletions(-) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch index 9cd5e3bcbe..c7833b3a11 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch @@ -1,25 +1,7 @@ - - - - - - - - - - - - - + + @@ -34,40 +16,9 @@ - -[VIRTUAL_JOINT_BROADCASTER] - - - - [move_group/fake_controller_joint_states] - [/joint_states] - - - [move_group/fake_controller_joint_states] - [/joint_states] - - - - - - - - - - - - - - - - - - + + + + - - - - - - From 0461e6f514475895b995d5abe129d25064bbd41d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 10:02:52 +0100 Subject: [PATCH 075/229] gazebo.launch: Load URDF via xacro if neccessary --- .../moveit_config_pkg_template/launch/demo_gazebo.launch | 3 --- .../templates/moveit_config_pkg_template/launch/gazebo.launch | 3 +-- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch index c7833b3a11..a9f320c596 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch @@ -6,14 +6,11 @@ - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gazebo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gazebo.launch index 6213ad3c58..05c8806f9c 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gazebo.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gazebo.launch @@ -2,7 +2,6 @@ - @@ -12,7 +11,7 @@ - + Date: Thu, 4 Nov 2021 23:24:27 +0100 Subject: [PATCH 076/229] gazebo.yaml: Allow initial_joint_positions --- .../tools/moveit_config_data.h | 3 ++- .../src/tools/moveit_config_data.cpp | 24 ++++++++++++++++++- .../widgets/configuration_files_widget.cpp | 7 ++++++ .../launch/gazebo.launch | 4 +++- 4 files changed, 35 insertions(+), 3 deletions(-) diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h index 4601e24d65..3a792a4b94 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h @@ -305,7 +305,8 @@ class MoveItConfigData // ****************************************************************************************** // Public Functions for outputting configuration and setting files // ****************************************************************************************** - std::vector getOMPLPlanners(); + std::vector getOMPLPlanners() const; + std::map getInitialJoints() const; bool outputSetupAssistantFile(const std::string& file_path); bool outputOMPLPlanningYAML(const std::string& file_path); bool outputCHOMPPlanningYAML(const std::string& file_path); diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 905d22bd57..2a41c0da52 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -593,7 +593,29 @@ bool MoveItConfigData::outputFakeControllersYAML(const std::string& file_path) return true; // file created successfully } -std::vector MoveItConfigData::getOMPLPlanners() +std::map MoveItConfigData::getInitialJoints() const +{ + std::map joints; + for (const srdf::Model::Group& group : srdf_->groups_) + { + // use first pose of each group as initial pose + for (const srdf::Model::GroupState& group_state : srdf_->group_states_) + { + if (group.name_ != group_state.group_) + continue; + for (const auto& pair : group_state.joint_values_) + { + if (pair.second.size() != 1) + continue; // only handle simple joints here + joints[pair.first] = pair.second.front(); + } + break; + } + } + return joints; +} + +std::vector MoveItConfigData::getOMPLPlanners() const { std::vector planner_des; diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 43e8e08b7a..500987d83e 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -1176,6 +1176,13 @@ void ConfigurationFilesWidget::loadTemplateStrings() addTemplateString("[AUTHOR_NAME]", config_data_->author_name_); addTemplateString("[AUTHOR_EMAIL]", config_data_->author_email_); + + { + std::stringstream joints; + for (const auto& pair : config_data_->getInitialJoints()) + joints << " -J " << pair.first << " " << pair.second; + addTemplateString("[GAZEBO_INITIAL_JOINTS]", joints.str()); + } } // ****************************************************************************************** diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gazebo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gazebo.launch index 05c8806f9c..749b590d06 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gazebo.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gazebo.launch @@ -2,6 +2,8 @@ + @@ -14,7 +16,7 @@ - From 9dc3e3564a5a1fdf91ef05c39364a04642b24b02 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 5 Nov 2021 00:27:44 +0100 Subject: [PATCH 077/229] gazebo.launch: delayed unpause Only unpause simulation when robot model was loaded. This ensures that the initial pose is actually held. --- .../moveit_config_pkg_template/launch/gazebo.launch | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gazebo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gazebo.launch index 749b590d06..a71045c007 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gazebo.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gazebo.launch @@ -8,15 +8,18 @@ - + + + - + From 592c86410f751666d1fa22ec0aa4ca887ec1fa2d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 10:58:45 +0100 Subject: [PATCH 078/229] Pilz unittests: use test_environment.launch --- .../test/integrationtest_sequence_action.test | 14 +------------- ...nce_action_capability_frankaemika_panda.test | 15 +-------------- ...sequence_action_capability_with_gripper.test | 14 ++------------ ...egrationtest_sequence_action_preemption.test | 17 +++-------------- 4 files changed, 7 insertions(+), 53 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.test index df51f007b1..369da0e53b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.test +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.test @@ -33,19 +33,7 @@ POSSIBILITY OF SUCH DAMAGE. --> - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_frankaemika_panda.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_frankaemika_panda.test index bc6afb0a8e..4037981141 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_frankaemika_panda.test +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_frankaemika_panda.test @@ -34,20 +34,7 @@ POSSIBILITY OF SUCH DAMAGE. - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_with_gripper.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_with_gripper.test index c45f964dbc..ce9044196e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_with_gripper.test +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_with_gripper.test @@ -34,18 +34,8 @@ POSSIBILITY OF SUCH DAMAGE. - - - [/move_group/fake_controller_joint_states] - - - - - - - - - + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.test index 614b5ffe1d..73b3ed6a05 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.test +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.test @@ -33,21 +33,10 @@ POSSIBILITY OF SUCH DAMAGE. --> - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - + + - + From cc71b612831b497b0928394a346293f5c670cba8 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 11:00:39 +0100 Subject: [PATCH 079/229] Rename launch argument execution_type -> fake_execution_type ... to clarify that this parameter is only used for fake controllers --- .../test/integrationtest_sequence_action.test | 2 +- ...tiontest_sequence_action_capability_frankaemika_panda.test | 2 +- ...tegrationtest_sequence_action_capability_with_gripper.test | 2 +- .../test/integrationtest_sequence_action_preemption.test | 2 +- moveit_setup_assistant/src/tools/moveit_config_data.cpp | 2 +- .../templates/moveit_config_pkg_template/launch/demo.launch | 4 ++-- .../launch/fake_moveit_controller_manager.launch.xml | 2 +- .../moveit_config_pkg_template/launch/move_group.launch | 4 ++-- .../launch/trajectory_execution.launch.xml | 4 ++-- 9 files changed, 12 insertions(+), 12 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.test index 369da0e53b..5fb2934c69 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.test +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.test @@ -35,7 +35,7 @@ POSSIBILITY OF SUCH DAMAGE. - + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_frankaemika_panda.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_frankaemika_panda.test index 4037981141..75c4afc29f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_frankaemika_panda.test +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_frankaemika_panda.test @@ -35,7 +35,7 @@ POSSIBILITY OF SUCH DAMAGE. - + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_with_gripper.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_with_gripper.test index ce9044196e..2c36fb699a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_with_gripper.test +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_with_gripper.test @@ -37,7 +37,7 @@ POSSIBILITY OF SUCH DAMAGE. - + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.test index 73b3ed6a05..11f7c5572a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.test +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.test @@ -36,7 +36,7 @@ POSSIBILITY OF SUCH DAMAGE. - + diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 2a41c0da52..2ff8824213 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -517,7 +517,7 @@ bool MoveItConfigData::outputFakeControllersYAML(const std::string& file_path) emitter << YAML::Key << "name"; emitter << YAML::Value << "fake_" + group.name_ + "_controller"; emitter << YAML::Key << "type"; - emitter << YAML::Value << "$(arg execution_type)"; + emitter << YAML::Value << "$(arg fake_execution_type)"; emitter << YAML::Key << "joints"; emitter << YAML::Value << YAML::BeginSeq; diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch index 0f2fc8ab38..837ffb93f7 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch @@ -17,7 +17,7 @@ - + @@ -47,7 +47,7 @@ - + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/fake_moveit_controller_manager.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/fake_moveit_controller_manager.launch.xml index f668745bb0..b0a803df1f 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/fake_moveit_controller_manager.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/fake_moveit_controller_manager.launch.xml @@ -1,7 +1,7 @@ - + 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 bee3b0c767..c161e13cbe 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 @@ -15,7 +15,7 @@ - + @@ -72,7 +72,7 @@ - + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml index e6d23c178b..20c3dfc459 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml @@ -3,7 +3,7 @@ - + @@ -16,7 +16,7 @@ - From 251b1a29060511ff8e886fe20a524be3e511c4cd Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 11:25:23 +0100 Subject: [PATCH 080/229] moveit.rviz template: remove link names --- .../launch/moveit.rviz | 564 +----------------- 1 file changed, 2 insertions(+), 562 deletions(-) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit.rviz b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit.rviz index ed81b2a664..2ee8b3b564 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit.rviz +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit.rviz @@ -44,287 +44,7 @@ Visualization Manager: MoveIt_Warehouse_Port: 33829 Name: MotionPlanning Planned Path: - Links: - base_bellow_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - double_stereo_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_ir_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_rgb_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_prosilica_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_plate_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_tilt_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - laser_tilt_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true + Links: ~ Loop Animation: true Robot Alpha: 0.5 Show Robot Collision: false @@ -361,287 +81,7 @@ Visualization Manager: Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 - Links: - base_bellow_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - double_stereo_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_ir_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_rgb_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_prosilica_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_plate_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_tilt_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - laser_tilt_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true + Links: ~ Robot Alpha: 0.5 Show Scene Robot: true Value: true From 12bf90be994cf59b8100348f6389c52390510417 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 19:46:52 +0100 Subject: [PATCH 081/229] moveit.rviz: Use Orbit view controller --- .../launch/moveit.rviz | 52 ++++++++++--------- 1 file changed, 27 insertions(+), 25 deletions(-) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit.rviz b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit.rviz index 2ee8b3b564..aab5d6e012 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit.rviz +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit.rviz @@ -5,8 +5,8 @@ Panels: Property Tree Widget: Expanded: - /MotionPlanning1 - Splitter Ratio: 0.74256 - Tree Height: 664 + Splitter Ratio: 0.5 + Tree Height: 226 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -37,11 +37,6 @@ Visualization Manager: Value: true - Class: moveit_rviz_plugin/MotionPlanning Enabled: true - MoveIt_Goal_Tolerance: 0 - MoveIt_Planning_Time: 5 - MoveIt_Use_Constraint_Aware_IK: true - MoveIt_Warehouse_Host: 127.0.0.1 - MoveIt_Warehouse_Port: 33829 Name: MotionPlanning Planned Path: Links: ~ @@ -64,7 +59,6 @@ Visualization Manager: Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: left_arm Query Goal State: true Query Start State: false Show Workspace: false @@ -74,8 +68,6 @@ Visualization Manager: Robot Description: robot_description Scene Geometry: Scene Alpha: 1 - Scene Color: 50; 230; 50 - Scene Display Time: 0.2 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels @@ -98,32 +90,42 @@ Visualization Manager: Value: true Views: Current: - Class: rviz/XYOrbit - Distance: 2.9965 + Class: rviz/Orbit + Distance: 2.0 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.75 Focal Point: - X: 0.113567 - Y: 0.10592 - Z: 2.23518e-07 + X: -0.1 + Y: 0.25 + Z: 0.30 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05 + Invert Z Axis: false Name: Current View Near Clip Distance: 0.01 - Pitch: 0.509797 - Target Frame: /[ROBOT_ROOT_LINK] - Value: XYOrbit (rviz) - Yaw: 5.65995 + Pitch: 0.5 + Target Frame: [PLANNING_FRAME] + Yaw: -0.6232355833053589 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1337 + Height: 848 Help: collapsed: false Hide Left Dock: false Hide Right Dock: false - Motion Planning: + MotionPlanning: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000173000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1828 - X: 459 - Y: -243 + Width: 1291 + X: 454 + Y: 25 From a9307c667b441a07051eaf5d87d8c0b39f552060 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 14:46:52 +0100 Subject: [PATCH 082/229] Improve instructions --- .../src/widgets/configuration_files_widget.cpp | 2 +- .../src/widgets/planning_groups_widget.cpp | 2 +- .../src/widgets/robot_poses_widget.cpp | 4 ++-- .../src/widgets/virtual_joints_widget.cpp | 9 +++++---- 4 files changed, 9 insertions(+), 8 deletions(-) diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 500987d83e..288234aec9 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -98,7 +98,7 @@ ConfigurationFilesWidget::ConfigurationFilesWidget(QWidget* parent, const MoveIt stack_path_->setPath(config_data_->config_pkg_path_); // Generated Files List ------------------------------------------- - QLabel* generated_list = new QLabel("Files to be generated: (checked)", this); + QLabel* generated_list = new QLabel("Check files you want to be generated:", this); layout->addWidget(generated_list); QSplitter* splitter = new QSplitter(Qt::Horizontal, this); diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp index 9c91095b42..e050176a79 100644 --- a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp +++ b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp @@ -107,7 +107,7 @@ PlanningGroupsWidget::PlanningGroupsWidget(QWidget* parent, const MoveItConfigDa "Create and edit 'joint model' groups for your robot based on joint collections, " "link collections, kinematic chains or subgroups. " "A planning group defines the set of (joint, link) pairs considered for planning " - "and collision checking. Define individual groups for each subset of the robot you want to plan for." + "and collision checking. Define individual groups for each subset of the robot you want to plan for.\n" "Note: when adding a link to the group, its parent joint is added too and vice versa.", this); layout->addWidget(header); diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp index 61a59d1602..0e9d000a78 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp @@ -75,8 +75,8 @@ RobotPosesWidget::RobotPosesWidget(QWidget* parent, const MoveItConfigDataPtr& c HeaderWidget* header = new HeaderWidget("Define Robot Poses", "Create poses for the robot. Poses are defined as sets of joint values for " - "particular planning groups. This is useful for things like home position." - "The first pose for each robot will be its initial pose in simulation.", + "particular planning groups. This is useful for things like home position. " + "The first listed pose will be the robot's initial pose in simulation.", this); layout->addWidget(header); diff --git a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp index b00d098865..6138a7d775 100644 --- a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp +++ b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp @@ -65,10 +65,11 @@ VirtualJointsWidget::VirtualJointsWidget(QWidget* parent, const MoveItConfigData // Top Header Area ------------------------------------------------ - HeaderWidget* header = new HeaderWidget("Define Virtual Joints", - "Create a virtual joint between a robot link and an external frame of " - "reference (considered fixed with respect to the robot).", - this); + HeaderWidget* header = + new HeaderWidget("Define Virtual Joints", + "Create a virtual joint between the base robot link and an external frame of reference. " + "This allows to place the robot in the world or on a mobile platform.", + this); layout->addWidget(header); // Create contents screens --------------------------------------- From c976011d47b96df75f35e1b840bdbf4e50e72cc6 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 15:41:35 +0100 Subject: [PATCH 083/229] Allow checking/unchecking multiple files for generation --- .../src/widgets/configuration_files_widget.cpp | 16 ++++++++++++++++ .../src/widgets/configuration_files_widget.h | 3 +++ 2 files changed, 19 insertions(+) diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 288234aec9..4277413b20 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -35,6 +35,7 @@ /* Author: Dave Coleman */ // Qt +#include #include #include #include @@ -107,7 +108,16 @@ ConfigurationFilesWidget::ConfigurationFilesWidget(QWidget* parent, const MoveIt // List Box action_list_ = new QListWidget(this); action_list_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + action_list_->setSelectionMode(QAbstractItemView::ExtendedSelection); connect(action_list_, SIGNAL(currentRowChanged(int)), this, SLOT(changeActionDesc(int))); + // Allow checking / unchecking of multiple items + action_list_->setContextMenuPolicy(Qt::ActionsContextMenu); + QAction* action = new QAction("Check all selected files", this); + connect(action, &QAction::triggered, [this]() { setCheckSelected(true); }); + action_list_->addAction(action); + action = new QAction("Uncheck all selected files", this); + connect(action, &QAction::triggered, [this]() { setCheckSelected(false); }); + action_list_->addAction(action); // Description action_label_ = new QLabel(this); @@ -174,6 +184,12 @@ ConfigurationFilesWidget::ConfigurationFilesWidget(QWidget* parent, const MoveIt this->setLayout(layout); } +void ConfigurationFilesWidget::setCheckSelected(bool checked) +{ + for (const QModelIndex& row : action_list_->selectionModel()->selectedRows()) + action_list_->model()->setData(row, checked ? Qt::Checked : Qt::Unchecked, Qt::CheckStateRole); +} + // ****************************************************************************************** // Populate the 'Files to be generated' list // ****************************************************************************************** diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.h b/moveit_setup_assistant/src/widgets/configuration_files_widget.h index edc7cf8271..f235eb2538 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.h +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.h @@ -118,6 +118,9 @@ private Q_SLOTS: /// Disable or enable item in gen_files_ array void changeCheckedState(QListWidgetItem* item); + /// Set checked state of all selected items + void setCheckSelected(bool checked); + private: // ****************************************************************************************** // Variables From e2c3c3d51d47e9b92a26e32152db02dc58ea0967 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 17:26:35 +0100 Subject: [PATCH 084/229] Simplify definition of `planning_plugin` parameter There is no means to declare the planning_plugin as an arg first. --- .../launch/chomp_planning_pipeline.launch.xml | 5 +---- .../launch/ompl_planning_pipeline.launch.xml | 5 +---- ...lz_industrial_motion_planner_planning_pipeline.launch.xml | 5 +---- .../launch/trajopt_planning_pipeline.launch.xml | 5 +---- 4 files changed, 4 insertions(+), 16 deletions(-) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml index e46e78270f..3b47012050 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml @@ -1,7 +1,4 @@ - - - @@ -14,7 +11,7 @@ default_planner_request_adapters/FixStartStatePathConstraints" /> - + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml index b7c36376e7..afbdf88d25 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml @@ -1,8 +1,5 @@ - - - - + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml index 75aabcccea..629781a7b6 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -1,12 +1,9 @@ - - - - + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml index b7dc6fdccd..5c1b3f6718 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml @@ -1,8 +1,5 @@ - - - @@ -20,7 +17,7 @@ - + From 170e6d2091a9e3f4b65b57eaa64a470d18b62cea Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 Nov 2021 17:29:06 +0100 Subject: [PATCH 085/229] Pilz: Define default planner --- ...pilz_industrial_motion_planner_planning_pipeline.launch.xml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml index 629781a7b6..c7c4cf5005 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -6,6 +6,9 @@ + + + From 776ff5366a5f6adec021d8d3e480b7118502e689 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 5 Nov 2021 16:56:57 +0100 Subject: [PATCH 086/229] Enable compiler warnings --- .github/workflows/ci.yaml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 9909cc1b62..08d756148d 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -27,6 +27,8 @@ jobs: - IMAGE: master-ci # ROS Melodic with all dependencies required for master CATKIN_LINT: true env: + CXXFLAGS: >- + -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file DOCKER_IMAGE: moveit/moveit:${{ matrix.env.IMAGE }} UPSTREAM_WORKSPACE: .github/workflows/upstream.rosinstall @@ -37,7 +39,8 @@ jobs: AFTER_SETUP_DOWNSTREAM_WORKSPACE: vcs pull $BASEDIR/downstream_ws/src TARGET_CMAKE_ARGS: > -DCMAKE_BUILD_TYPE=${{ matrix.env.CCOV && 'Debug' || 'Release'}} - -DCMAKE_CXX_FLAGS="${{ matrix.env.CLANG_TIDY != 'pedantic' && '-Werror ' || '' }}${{ matrix.env.CCOV && ' --coverage -O2 -fno-omit-frame-pointer'}}" + -DCMAKE_CXX_FLAGS="${{ matrix.env.CLANG_TIDY != 'pedantic' && '-Werror ' || '' }}$CXXFLAGS${{ matrix.env.CCOV && ' --coverage -O2 -fno-omit-frame-pointer'}}" + UPSTREAM_CMAKE_ARGS: "-DCMAKE_CXX_FLAGS=''" DOWNSTREAM_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-Wall -Wextra" CCACHE_DIR: ${{ github.workspace }}/.ccache BASEDIR: ${{ github.workspace }}/.work From 5795c6ce52338b963fe8fcf5505f01cf1a7ecfde Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 5 Nov 2021 17:02:11 +0100 Subject: [PATCH 087/229] Use catkin_tools_devel builder from industrial_ci https://github.com/ros-industrial/industrial_ci/pull/707 --- .github/workflows/catkin_tools_devel.sh | 15 --------------- .github/workflows/ci.yaml | 2 +- 2 files changed, 1 insertion(+), 16 deletions(-) delete mode 100644 .github/workflows/catkin_tools_devel.sh diff --git a/.github/workflows/catkin_tools_devel.sh b/.github/workflows/catkin_tools_devel.sh deleted file mode 100644 index fa1c4f597e..0000000000 --- a/.github/workflows/catkin_tools_devel.sh +++ /dev/null @@ -1,15 +0,0 @@ -#!/bin/bash - -# Custome BUILDER to build in devel-space only - -BUILDER=catkin_tools ici_source_builder - -function ici_extend_space { - echo "$1/devel" -} - -function _catkin_config { - local extend=$1; shift - local ws=$1; shift - ici_exec_in_workspace "$extend" "$ws" catkin config --init -} diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 08d756148d..4d80800a55 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -19,7 +19,7 @@ jobs: env: - IMAGE: noetic-ci CCOV: true - BUILDER: .github/workflows/catkin_tools_devel.sh + BUILDER: catkin_tools_devel - IMAGE: noetic-ci-shadow-fixed IKFAST_TEST: true CATKIN_LINT: true From 1e18c60e4680808276a4103138c6f9a2a471b175 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 5 Nov 2021 17:18:25 +0100 Subject: [PATCH 088/229] Add comments --- .github/workflows/ci.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 4d80800a55..3841c3d2fe 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -37,6 +37,7 @@ jobs: # Pull any updates to the upstream workspace (after restoring it from cache) AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src AFTER_SETUP_DOWNSTREAM_WORKSPACE: vcs pull $BASEDIR/downstream_ws/src + # Compile CCOV with Debug. Enable -Werror (except CLANG_TIDY=pedantic, which makes the clang-tidy step fail on warnings) TARGET_CMAKE_ARGS: > -DCMAKE_BUILD_TYPE=${{ matrix.env.CCOV && 'Debug' || 'Release'}} -DCMAKE_CXX_FLAGS="${{ matrix.env.CLANG_TIDY != 'pedantic' && '-Werror ' || '' }}$CXXFLAGS${{ matrix.env.CCOV && ' --coverage -O2 -fno-omit-frame-pointer'}}" From 0785f1ea46f3fa3fdda1273ac62bf718ce6cc8fe Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 5 Nov 2021 17:49:50 +0100 Subject: [PATCH 089/229] Use test_environment.launch in unittests --- .../test/rrbot_move_group.launch | 5 ++- .../integrationtest_command_planning.test | 14 +------- ...st_command_planning_frankaemika_panda.test | 14 +------- ...iontest_command_planning_with_gripper.test | 12 +------ ...ationtest_sequence_service_capability.test | 14 +------- ..._service_capability_frankaemika_panda.test | 14 +------- ...uence_service_capability_with_gripper.test | 12 +------ .../launch/moveit_planning_execution.launch | 33 ++----------------- .../test/move_group_interface_cpp_test.test | 24 +------------- .../test/move_group_pick_place_test.test | 24 +------------- .../test/subframes_test.test | 24 +------------- 11 files changed, 13 insertions(+), 177 deletions(-) diff --git a/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch b/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch index ab54cf53fc..56100a65ff 100644 --- a/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch +++ b/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch @@ -27,9 +27,8 @@ - - - + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.test index ba73e8b56b..6d26187e4d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.test +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.test @@ -34,19 +34,7 @@ POSSIBILITY OF SUCH DAMAGE. - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_frankaemika_panda.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_frankaemika_panda.test index d360fbcafa..44f7d54b3d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_frankaemika_panda.test +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_frankaemika_panda.test @@ -34,19 +34,7 @@ POSSIBILITY OF SUCH DAMAGE. - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_with_gripper.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_with_gripper.test index 64fb904bde..27212a2b92 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_with_gripper.test +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_with_gripper.test @@ -34,17 +34,7 @@ POSSIBILITY OF SUCH DAMAGE. - - - [/move_group/fake_controller_joint_states] - - - - - - - - + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.test index 9eec6f308b..049cb5a253 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.test +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.test @@ -33,19 +33,7 @@ POSSIBILITY OF SUCH DAMAGE. --> - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_frankaemika_panda.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_frankaemika_panda.test index dbec926599..604d6ace07 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_frankaemika_panda.test +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_frankaemika_panda.test @@ -34,19 +34,7 @@ POSSIBILITY OF SUCH DAMAGE. - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_with_gripper.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_with_gripper.test index f214d4baf7..0c516453ac 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_with_gripper.test +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_with_gripper.test @@ -33,17 +33,7 @@ POSSIBILITY OF SUCH DAMAGE. --> - - - [/move_group/fake_controller_joint_states] - - - - - - - - + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch index f8ccaa62a3..134a32ba3e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch @@ -37,38 +37,9 @@ POSSIBILITY OF SUCH DAMAGE. - - - - - - - - - - - - - - - - - - - - - - - [/move_group/fake_controller_joint_states] - - - - - - - - + + diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test index 9975f257d2..02d1f7201c 100644 --- a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test +++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test @@ -1,27 +1,5 @@ - - - - - - - - - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - - - + - - - - - - - - - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - - - + - - - - - - - - - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - - - + Date: Fri, 5 Nov 2021 18:07:30 +0100 Subject: [PATCH 090/229] Remove unused moveit_planning_execution.launch --- .../acceptance_tests/acceptance_test_lin.md | 2 +- .../acceptance_tests/acceptance_test_ptp.md | 2 +- .../launch/moveit_planning_execution.launch | 48 ------------------- 3 files changed, 2 insertions(+), 50 deletions(-) delete mode 100644 moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md index 4f2abec293..867c7f6587 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md +++ b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md @@ -41,7 +41,7 @@ to be moved out of singularities. ## Test Sequence: 1. Bringup can: `sudo ip link set can0 up type can bitrate 1000000` - 2. Run `roslaunch moveit_resources_prbt_moveit_config moveit_planning_execution.launch sim:=False pipeline:=pilz_industrial_motion_planner` + 2. Run `roslaunch moveit_resources_prbt_moveit_config demo.launch pipeline:=pilz_industrial_motion_planner` 3. In the motion planing widget (lower left part of moveit) choose PTP in the dropdown below "Trapezoidal Command Planner" (see image) ![moveit_1](img/acceptance_test_lin_img1.png) 4. Switch to the tab "Planning" in the moveit planning plugin. Move the ball handle to select a singularity free position. Click on "plan and execute". diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_ptp.md b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_ptp.md index 732ec8ec4f..be2dce5489 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_ptp.md +++ b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_ptp.md @@ -40,7 +40,7 @@ This test checks that the real robot system is able to perform a PTP Motion to a ## Test Sequence: 1. Bringup can: `sudo ip link set can0 up type can bitrate 1000000` - 2. Run `roslaunch moveit_resources_prbt_moveit_config moveit_planning_execution.launch sim:=False pipeline:=pilz_industrial_motion_planner` + 2. Run `roslaunch moveit_resources_prbt_moveit_config demo.launch pipeline:=pilz_industrial_motion_planner` 3. The motion planning widget (lower left part of moveit) choose PTP in the dropdown below "Trapezoidal Command Planner" (see image) ![moveit_1](img/acceptance_test_ptp_img1.png) 4. Switch to the tab "Planning" in the moveit planning plugin. Move the ball handle the select goal pose. Click on "plan and execute". diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch deleted file mode 100644 index 134a32ba3e..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch +++ /dev/null @@ -1,48 +0,0 @@ - - - - - - - - - - - - - - - - From e30c6fa853dbc699c0cc84e420626000eefe3212 Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Sat, 6 Nov 2021 00:22:51 +0100 Subject: [PATCH 091/229] Fix orientation of subframe offset in Pilz planners (#2890) Fix #2879 by reorienting the subframe offset applied to a goal pose in the PTP planner, so that it is applied in the correct direction. Also add the same offset computation to LIN and CIRC generators, thus allowing them to work with subframes. Co-authored-by: Robert Haschke --- .../trajectory_functions.h | 17 +++++++++++ .../src/trajectory_functions.cpp | 23 +++++++++++++++ .../src/trajectory_generator_circ.cpp | 29 +++++++++++-------- .../src/trajectory_generator_lin.cpp | 7 +---- .../src/trajectory_generator_ptp.cpp | 17 ++--------- 5 files changed, 61 insertions(+), 32 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h index fc78cf994b..75be0fa56b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h @@ -220,3 +220,20 @@ bool isStateColliding(const bool test_for_self_collision, const planning_scene:: } // namespace pilz_industrial_motion_planner void normalizeQuaternion(geometry_msgs::Quaternion& quat); + +/** + * @brief Adapt goal pose, defined by position+orientation, to consider offset + * @param constraint to apply offset to + * @param offset to apply to the constraint + * @param orientation to apply to the offset + * @return final goal pose + */ +Eigen::Isometry3d getConstraintPose(const geometry_msgs::Point& position, const geometry_msgs::Quaternion& orientation, + const geometry_msgs::Vector3& offset); + +/** + * @brief Conviencency method, passing args from a goal constraint + * @param goal goal constraint + * @return final goal pose + */ +Eigen::Isometry3d getConstraintPose(const moveit_msgs::Constraints& goal); diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index 3810fca709..93ba27e133 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -585,3 +585,26 @@ void normalizeQuaternion(geometry_msgs::Quaternion& quat) tf2::fromMsg(quat, q); quat = tf2::toMsg(q.normalize()); } + +Eigen::Isometry3d getConstraintPose(const geometry_msgs::Point& position, const geometry_msgs::Quaternion& orientation, + const geometry_msgs::Vector3& offset) +{ + Eigen::Quaterniond quat; + tf2::fromMsg(orientation, quat); + quat.normalize(); + Eigen::Vector3d v; + tf2::fromMsg(position, v); + + Eigen::Isometry3d pose = Eigen::Translation3d(v) * quat; + + tf2::fromMsg(offset, v); + pose.translation() -= quat * v; + return pose; +} + +Eigen::Isometry3d getConstraintPose(const moveit_msgs::Constraints& goal) +{ + return getConstraintPose(goal.position_constraints.front().constraint_region.primitive_poses.front().position, + goal.orientation_constraints.front().orientation, + goal.position_constraints.front().target_point_offset); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index 44115f6e2e..c949793b54 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -142,12 +142,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni { frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; } - geometry_msgs::Pose goal_pose_msg; - goal_pose_msg.position = - req.goal_constraints.front().position_constraints.front().constraint_region.primitive_poses.front().position; - goal_pose_msg.orientation = req.goal_constraints.front().orientation_constraints.front().orientation; - normalizeQuaternion(goal_pose_msg.orientation); - tf2::fromMsg(goal_pose_msg, info.goal_pose); + info.goal_pose = getConstraintPose(req.goal_constraints.front()); } assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size()); @@ -179,13 +174,23 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni // LCOV_EXCL_STOP // not able to trigger here since lots of checks before // are in place } - - Eigen::Vector3d circ_path_point; - tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position, - circ_path_point); - info.circ_path_point.first = req.path_constraints.name; - info.circ_path_point.second = circ_path_point; + if (!req.goal_constraints.front().position_constraints.empty()) + { + const moveit_msgs::Constraints& goal = req.goal_constraints.front(); + info.circ_path_point.second = + getConstraintPose( + req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position, + goal.orientation_constraints.front().orientation, goal.position_constraints.front().target_point_offset) + .translation(); + } + else + { + Eigen::Vector3d circ_path_point; + tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position, + circ_path_point); + info.circ_path_point.second = circ_path_point; + } } void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& scene, diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index dede65781f..b2052ed8af 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -116,12 +116,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin { frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; } - geometry_msgs::Pose goal_pose_msg; - goal_pose_msg.position = - req.goal_constraints.front().position_constraints.front().constraint_region.primitive_poses.front().position; - goal_pose_msg.orientation = req.goal_constraints.front().orientation_constraints.front().orientation; - normalizeQuaternion(goal_pose_msg.orientation); - tf2::fromMsg(goal_pose_msg, info.goal_pose); + info.goal_pose = getConstraintPose(req.goal_constraints.front()); } assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size()); 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 d1dc58f1c2..ebb35c4b4f 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 @@ -221,23 +221,12 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin info.goal_joint_position[joint_constraint.joint_name] = joint_constraint.position; } } - // slove the ik + // solve the ik else { - geometry_msgs::Point p = - req.goal_constraints.at(0).position_constraints.at(0).constraint_region.primitive_poses.at(0).position; - p.x -= req.goal_constraints.at(0).position_constraints.at(0).target_point_offset.x; - p.y -= req.goal_constraints.at(0).position_constraints.at(0).target_point_offset.y; - p.z -= req.goal_constraints.at(0).position_constraints.at(0).target_point_offset.z; - - geometry_msgs::Pose pose; - pose.position = p; - pose.orientation = req.goal_constraints.at(0).orientation_constraints.at(0).orientation; - Eigen::Isometry3d pose_eigen; - normalizeQuaternion(pose.orientation); - tf2::fromMsg(pose, pose_eigen); + Eigen::Isometry3d goal_pose = getConstraintPose(req.goal_constraints.front()); if (!computePoseIK(scene, req.group_name, req.goal_constraints.at(0).position_constraints.at(0).link_name, - pose_eigen, robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position)) + goal_pose, robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position)) { throw PtpNoIkSolutionForGoalPose("No IK solution for goal pose"); } From 304ae6acdf9bb3994ad7f7315c23048ca6c72f7e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 6 Nov 2021 06:15:18 +0100 Subject: [PATCH 092/229] CI: rename cross_platform_ci -> robostack --- .github/{ci_cross_platform_env.yml => robostack_env.yaml} | 0 .../workflows/{cross_platform_ci.yml => robostack.yaml} | 7 +++---- 2 files changed, 3 insertions(+), 4 deletions(-) rename .github/{ci_cross_platform_env.yml => robostack_env.yaml} (100%) rename .github/workflows/{cross_platform_ci.yml => robostack.yaml} (95%) diff --git a/.github/ci_cross_platform_env.yml b/.github/robostack_env.yaml similarity index 100% rename from .github/ci_cross_platform_env.yml rename to .github/robostack_env.yaml diff --git a/.github/workflows/cross_platform_ci.yml b/.github/workflows/robostack.yaml similarity index 95% rename from .github/workflows/cross_platform_ci.yml rename to .github/workflows/robostack.yaml index c43ed62bd7..1840976190 100644 --- a/.github/workflows/cross_platform_ci.yml +++ b/.github/workflows/robostack.yaml @@ -1,4 +1,4 @@ -name: MoveIt core cross-platform RoboStack build +name: RoboStack on: workflow_dispatch: @@ -21,9 +21,9 @@ jobs: - uses: actions/checkout@v2 - name: Set up Build Dependencies - uses: mamba-org/provision-with-micromamba@v10 + uses: mamba-org/provision-with-micromamba@v11 with: - environment-file: .github/ci_cross_platform_env.yml + environment-file: .github/robostack_env.yaml micromamba-version: 0.17.0 - name: Set up MoveIt Core Dependencies on Unix @@ -70,7 +70,6 @@ jobs: if: runner.os == 'Windows' shell: cmd run: | - echo "Remove unnecessary / colliding things from PATH" set "PATH=%PATH:C:\ProgramData\Chocolatey\bin;=%" set "PATH=%PATH:C:\Program Files (x86)\sbt\bin;=%" From 331aff5de359b50a21794fcaf781a0e238a2ee68 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 6 Nov 2021 06:57:45 +0100 Subject: [PATCH 093/229] CI: prerelease.yaml: only allow manual triggering --- .github/workflows/prerelease.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/prerelease.yaml b/.github/workflows/prerelease.yaml index 5c8b6b8f94..06a7214db8 100644 --- a/.github/workflows/prerelease.yaml +++ b/.github/workflows/prerelease.yaml @@ -5,7 +5,6 @@ name: pre-release on: workflow_dispatch: - push: jobs: default: From 568154498ac04ce1911007e95d3790d080403557 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 6 Nov 2021 07:54:00 +0100 Subject: [PATCH 094/229] README.md: Update badges --- README.md | 97 ++++++++++++--------------- moveit/scripts/create_readme_table.py | 5 +- 2 files changed, 47 insertions(+), 55 deletions(-) diff --git a/README.md b/README.md index 073957755b..65e99faa75 100644 --- a/README.md +++ b/README.md @@ -11,8 +11,8 @@ ## Branches Policy -- We develop latest features on ``master``. -- The ``*-devel`` branches correspond to released and stable versions of MoveIt for specific distributions of ROS. +- We develop latest features on `master`. +- The `*-devel` branches correspond to released and stable versions of MoveIt for specific distributions of ROS. `noetic-devel` is synced to `master` currently. - Bug fixes occasionally get backported to these released versions of MoveIt. - For MoveIt 2 development, see [moveit2](https://github.com/ros-planning/moveit2). @@ -20,65 +20,56 @@ ### Continuous Integration -service | Kinetic | Melodic | Master ----------- | ------- | ------- | ------ -GitHub | [![Format](https://github.com/ros-planning/moveit/actions/workflows/format.yaml/badge.svg?branch=kinetic-devel)](https://github.https://github.com/ros-planning/moveit/actions/workflows/format.yaml?query=branch%3Akinetic-devel) [![CI](https://github.com/ros-planning/moveit/actions/workflows/ci.yaml/badge.svg?branch=kinetic-devel)](https://github.https://github.com/ros-planning/moveit/actions/workflows/ci.yaml?query=branch%3Akinetic-devel) | [![Format](https://github.com/ros-planning/moveit/actions/workflows/format.yaml/badge.svg?branch=melodic-devel)](https://github.https://github.com/ros-planning/moveit/actions/workflows/format.yaml?query=branch%3Amelodic-devel) [![CI](https://github.com/ros-planning/moveit/actions/workflows/ci.yaml/badge.svg?branch=melodic-devel)](https://github.https://github.com/ros-planning/moveit/actions/workflows/ci.yaml?query=branch%3Amelodic-devel) | [![Format](https://github.com/ros-planning/moveit/actions/workflows/format.yaml/badge.svg?branch=master)](https://github.https://github.com/ros-planning/moveit/actions/workflows/format.yaml?query=branch%3Amaster) [![CI](https://github.com/ros-planning/moveit/actions/workflows/ci.yaml/badge.svg?branch=master)](https://github.https://github.com/ros-planning/moveit/actions/workflows/ci.yaml?query=branch%3Amaster) | -CodeCov | [![codecov](https://codecov.io/gh/ros-planning/moveit/branch/kinetic-devel/graph/badge.svg?token=W7uHKcY0ly)](https://codecov.io/gh/ros-planning/moveit) | [![codecov](https://codecov.io/gh/ros-planning/moveit/branch/melodic-devel/graph/badge.svg?token=W7uHKcY0ly)](https://codecov.io/gh/ros-planning/moveit) | [![codecov](https://codecov.io/gh/ros-planning/moveit/branch/master/graph/badge.svg?token=W7uHKcY0ly)](https://codecov.io/gh/ros-planning/moveit) | -build farm | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kdev__moveit__ubuntu_xenial_amd64)](http://build.ros.org/job/Kdev__moveit__ubuntu_xenial_amd64) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mdev__moveit__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdev__moveit__ubuntu_bionic_amd64) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ndev__moveit__ubuntu_focal_amd64)](https://build.ros.org/job/Ndev__moveit__ubuntu_focal_amd64/) | +service | Melodic | Master +---------- | ------- | ------ +GitHub | [![Format](https://github.com/ros-planning/moveit/actions/workflows/format.yaml/badge.svg?branch=melodic-devel)](https://github.com/ros-planning/moveit/actions/workflows/format.yaml?query=branch%3Amelodic-devel) [![CI](https://github.com/ros-planning/moveit/actions/workflows/ci.yaml/badge.svg?branch=melodic-devel)](https://github.com/ros-planning/moveit/actions/workflows/ci.yaml?query=branch%3Amelodic-devel) | [![Format](https://github.com/ros-planning/moveit/actions/workflows/format.yaml/badge.svg?branch=master)](https://github.com/ros-planning/moveit/actions/workflows/format.yaml?query=branch%3Amaster) [![CI](https://github.com/ros-planning/moveit/actions/workflows/ci.yaml/badge.svg?branch=master)](https://github.com/ros-planning/moveit/actions/workflows/ci.yaml?query=branch%3Amaster) | +CodeCov | [![codecov](https://codecov.io/gh/ros-planning/moveit/branch/melodic-devel/graph/badge.svg?token=W7uHKcY0ly)](https://codecov.io/gh/ros-planning/moveit) | [![codecov](https://codecov.io/gh/ros-planning/moveit/branch/master/graph/badge.svg?token=W7uHKcY0ly)](https://codecov.io/gh/ros-planning/moveit) | +build farm | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mdev__moveit__ubuntu_bionic_amd64)](https://build.ros.org/job/Mdev__moveit__ubuntu_bionic_amd64) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Ndev__moveit__ubuntu_focal_amd64)](https://build.ros.org/job/Ndev__moveit__ubuntu_focal_amd64/) | +| [![Docker](https://img.shields.io/docker/stars/moveit/moveit.svg)](https://registry.hub.docker.com/moveit/moveit)
[![Pulls](https://img.shields.io/docker/pulls/moveit/moveit.svg?maxAge=2592000)](https://hub.docker.com/r/moveit/moveit) | [![automated build](https://img.shields.io/docker/automated/moveit/moveit.svg?maxAge=2592000)](https://hub.docker.com/r/moveit/moveit) | [![docker](https://github.com/ros-planning/moveit/actions/workflows/docker.yaml/badge.svg?branch=master)](https://github.com/ros-planning/moveit/actions/workflows/docker.yaml?query=branch%3Amaster) | ### Licenses [![FOSSA Status](https://app.fossa.com/api/projects/git%2Bgithub.com%2Fros-planning%2Fmoveit.svg?type=shield)](https://app.fossa.com/projects/git%2Bgithub.com%2Fros-planning%2Fmoveit?ref=badge_shield) -### Docker Containers - -[![Docker Build](https://img.shields.io/docker/build/moveit/moveit.svg)](https://hub.docker.com/r/moveit/moveit/builds) -[![Docker Automated build](https://img.shields.io/docker/automated/moveit/moveit.svg?maxAge=2592000)](https://hub.docker.com/r/moveit/moveit/) [![Docker Pulls](https://img.shields.io/docker/pulls/moveit/moveit.svg?maxAge=2592000)](https://hub.docker.com/r/moveit/moveit/) [![Docker Stars](https://img.shields.io/docker/stars/moveit/moveit.svg)](https://registry.hub.docker.com/moveit/moveit/) ### ROS Buildfarm -MoveIt Package | Kinetic Source | Kinetic Debian | Melodic Source | Melodic Debian | Noetic Source | Noetic Debian --------------- | -------------- | -------------- | -------------- | -------------- | ------------- | ------------- -moveit | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit__ubuntu_focal_amd64__binary) -moveit_chomp_optimizer_adapter | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_chomp_optimizer_adapter__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_chomp_optimizer_adapter__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_chomp_optimizer_adapter__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_chomp_optimizer_adapter__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_chomp_optimizer_adapter__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_chomp_optimizer_adapter__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_chomp_optimizer_adapter__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_chomp_optimizer_adapter__ubuntu_focal_amd64__binary) -moveit_commander | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_commander__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_commander__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_commander__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_commander__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_commander__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_commander__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_commander__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_commander__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_commander__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_commander__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_commander__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_commander__ubuntu_focal_amd64__binary) -moveit_core | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_core__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_core__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_core__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_core__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_core__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_core__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_core__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_core__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_core__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_core__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_core__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_core__ubuntu_focal_amd64__binary) -moveit_fake_controller_manager | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_fake_controller_manager__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_fake_controller_manager__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_fake_controller_manager__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_fake_controller_manager__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_fake_controller_manager__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_fake_controller_manager__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_fake_controller_manager__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_fake_controller_manager__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_fake_controller_manager__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_fake_controller_manager__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_fake_controller_manager__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_fake_controller_manager__ubuntu_focal_amd64__binary) -moveit_grasps | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_grasps__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_grasps__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_grasps__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_grasps__ubuntu_focal_amd64__binary) -moveit_jog_arm | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_jog_arm__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_jog_arm__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_jog_arm__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_jog_arm__ubuntu_focal_amd64__binary) -moveit_kinematics | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_kinematics__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_kinematics__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_kinematics__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_kinematics__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_kinematics__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_kinematics__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_kinematics__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_kinematics__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_kinematics__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_kinematics__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_kinematics__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_kinematics__ubuntu_focal_amd64__binary) -moveit_msgs | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_msgs__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_msgs__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_msgs__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_msgs__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_msgs__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_msgs__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_msgs__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_msgs__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_msgs__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_msgs__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_msgs__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_msgs__ubuntu_focal_amd64__binary) -moveit_planners | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_planners__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_planners__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_planners__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_planners__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_planners__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_planners__ubuntu_focal_amd64__binary) -moveit_planners_chomp | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_planners_chomp__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners_chomp__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_planners_chomp__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners_chomp__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners_chomp__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners_chomp__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners_chomp__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners_chomp__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_planners_chomp__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_planners_chomp__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_planners_chomp__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_planners_chomp__ubuntu_focal_amd64__binary) -moveit_planners_ompl | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_planners_ompl__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners_ompl__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_planners_ompl__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners_ompl__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners_ompl__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners_ompl__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners_ompl__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners_ompl__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_planners_ompl__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_planners_ompl__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_planners_ompl__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_planners_ompl__ubuntu_focal_amd64__binary) -moveit_plugins | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_plugins__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_plugins__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_plugins__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_plugins__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_plugins__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_plugins__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_plugins__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_plugins__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_plugins__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_plugins__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_plugins__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_plugins__ubuntu_focal_amd64__binary) -moveit_resources | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_resources__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_resources__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_resources__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_resources__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_resources__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_resources__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_resources__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_resources__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_resources__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_resources__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_resources__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_resources__ubuntu_focal_amd64__binary) -moveit_ros | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros__ubuntu_focal_amd64__binary) -moveit_ros_benchmarks | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_benchmarks__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_benchmarks__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_benchmarks__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_benchmarks__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_benchmarks__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_benchmarks__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_benchmarks__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_benchmarks__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_benchmarks__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_benchmarks__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_benchmarks__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_benchmarks__ubuntu_focal_amd64__binary) -moveit_ros_control_interface | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_control_interface__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_control_interface__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_control_interface__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_control_interface__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_control_interface__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_control_interface__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_control_interface__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_control_interface__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_control_interface__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_control_interface__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_control_interface__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_control_interface__ubuntu_focal_amd64__binary) -moveit_ros_manipulation | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_manipulation__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_manipulation__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_manipulation__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_manipulation__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_manipulation__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_manipulation__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_manipulation__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_manipulation__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_manipulation__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_manipulation__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_manipulation__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_manipulation__ubuntu_focal_amd64__binary) -moveit_ros_move_group | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_move_group__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_move_group__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_move_group__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_move_group__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_move_group__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_move_group__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_move_group__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_move_group__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_move_group__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_move_group__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_move_group__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_move_group__ubuntu_focal_amd64__binary) -moveit_ros_occupancy_map_monitor | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_occupancy_map_monitor__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_occupancy_map_monitor__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_occupancy_map_monitor__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_occupancy_map_monitor__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_occupancy_map_monitor__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_occupancy_map_monitor__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_occupancy_map_monitor__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_occupancy_map_monitor__ubuntu_focal_amd64__binary) -moveit_ros_perception | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_perception__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_perception__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_perception__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_perception__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_perception__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_perception__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_perception__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_perception__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_perception__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_perception__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_perception__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_perception__ubuntu_focal_amd64__binary) -moveit_ros_planning | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_planning__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_planning__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_planning__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_planning__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_planning__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_planning__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_planning__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_planning__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_planning__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_planning__ubuntu_focal_amd64__binary) -moveit_ros_planning_interface | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_planning_interface__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_planning_interface__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_planning_interface__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_planning_interface__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_planning_interface__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning_interface__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_planning_interface__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning_interface__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_planning_interface__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_planning_interface__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_planning_interface__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_planning_interface__ubuntu_focal_amd64__binary) -moveit_ros_robot_interaction | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_robot_interaction__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_robot_interaction__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_robot_interaction__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_robot_interaction__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_robot_interaction__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_robot_interaction__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_robot_interaction__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_robot_interaction__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_robot_interaction__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_robot_interaction__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_robot_interaction__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_robot_interaction__ubuntu_focal_amd64__binary) -moveit_ros_visualization | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_visualization__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_visualization__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_visualization__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_visualization__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_visualization__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_visualization__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_visualization__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_visualization__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_visualization__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_visualization__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_visualization__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_visualization__ubuntu_focal_amd64__binary) -moveit_ros_warehouse | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_warehouse__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_warehouse__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_warehouse__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_warehouse__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_warehouse__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_warehouse__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_warehouse__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_warehouse__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_warehouse__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_warehouse__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_warehouse__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_warehouse__ubuntu_focal_amd64__binary) -moveit_runtime | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_runtime__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_runtime__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_runtime__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_runtime__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_runtime__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_runtime__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_runtime__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_runtime__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_runtime__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_runtime__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_runtime__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_runtime__ubuntu_focal_amd64__binary) -moveit_setup_assistant | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_setup_assistant__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_setup_assistant__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_setup_assistant__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_setup_assistant__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_setup_assistant__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_setup_assistant__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_setup_assistant__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_setup_assistant__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_setup_assistant__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_setup_assistant__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_setup_assistant__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_setup_assistant__ubuntu_focal_amd64__binary) -moveit_simple_controller_manager | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_simple_controller_manager__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_simple_controller_manager__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_simple_controller_manager__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_simple_controller_manager__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_simple_controller_manager__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_simple_controller_manager__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_simple_controller_manager__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_simple_controller_manager__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_simple_controller_manager__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_simple_controller_manager__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_simple_controller_manager__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_simple_controller_manager__ubuntu_focal_amd64__binary) -moveit_task_constructor_capabilities | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_task_constructor_capabilities__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_task_constructor_capabilities__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_task_constructor_capabilities__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_task_constructor_capabilities__ubuntu_focal_amd64__binary) -moveit_task_constructor_core | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_task_constructor_core__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_task_constructor_core__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_task_constructor_core__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_task_constructor_core__ubuntu_focal_amd64__binary) -moveit_task_constructor_demo | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_task_constructor_demo__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_task_constructor_demo__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_task_constructor_demo__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_task_constructor_demo__ubuntu_focal_amd64__binary) -moveit_task_constructor_msgs | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_task_constructor_msgs__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_task_constructor_msgs__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_task_constructor_msgs__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_task_constructor_msgs__ubuntu_focal_amd64__binary) -moveit_task_constructor_visualization | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_task_constructor_visualization__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_task_constructor_visualization__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_task_constructor_visualization__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_task_constructor_visualization__ubuntu_focal_amd64__binary) -moveit_visual_tools | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_visual_tools__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_visual_tools__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_visual_tools__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_visual_tools__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_visual_tools__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_visual_tools__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_visual_tools__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_visual_tools__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_visual_tools__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_visual_tools__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_visual_tools__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_visual_tools__ubuntu_focal_amd64__binary) -chomp_motion_planner | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__chomp_motion_planner__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__chomp_motion_planner__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__chomp_motion_planner__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__chomp_motion_planner__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__chomp_motion_planner__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__chomp_motion_planner__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__chomp_motion_planner__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__chomp_motion_planner__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__chomp_motion_planner__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__chomp_motion_planner__ubuntu_focal_amd64__binary) -geometric_shapes | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__geometric_shapes__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__geometric_shapes__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__geometric_shapes__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__geometric_shapes__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__geometric_shapes__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__geometric_shapes__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__geometric_shapes__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__geometric_shapes__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary) -rviz_marker_tools | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__rviz_marker_tools__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__rviz_marker_tools__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__rviz_marker_tools__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__rviz_marker_tools__ubuntu_focal_amd64__binary) -rviz_visual_tools | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__rviz_visual_tools__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__rviz_visual_tools__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__rviz_visual_tools__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__rviz_visual_tools__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__rviz_visual_tools__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__rviz_visual_tools__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__rviz_visual_tools__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__rviz_visual_tools__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__rviz_visual_tools__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__rviz_visual_tools__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__rviz_visual_tools__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__rviz_visual_tools__ubuntu_focal_amd64__binary) -srdfdom | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__srdfdom__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__srdfdom__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__srdfdom__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__srdfdom__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__srdfdom__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__srdfdom__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__srdfdom__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__srdfdom__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__srdfdom__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__srdfdom__ubuntu_focal_amd64__binary) +MoveIt Package | Melodic Source | Melodic Debian | Noetic Source | Noetic Debian +-------------- | -------------- | -------------- | ------------- | ------------- +**moveit** | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit__ubuntu_focal_amd64__binary) +moveit_core | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_core__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_core__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_core__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_core__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_core__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_core__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_core__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_core__ubuntu_focal_amd64__binary) +moveit_kinematics | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_kinematics__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_kinematics__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_kinematics__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_kinematics__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_kinematics__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_kinematics__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_kinematics__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_kinematics__ubuntu_focal_amd64__binary) +**moveit_planners** | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_planners__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_planners__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_planners__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_planners__ubuntu_focal_amd64__binary) +moveit_planners_ompl | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners_ompl__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners_ompl__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners_ompl__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners_ompl__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_planners_ompl__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_planners_ompl__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_planners_ompl__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_planners_ompl__ubuntu_focal_amd64__binary) +moveit_planners_chomp | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners_chomp__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners_chomp__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners_chomp__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners_chomp__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_planners_chomp__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_planners_chomp__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_planners_chomp__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_planners_chomp__ubuntu_focal_amd64__binary) +chomp_motion_planner | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__chomp_motion_planner__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__chomp_motion_planner__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__chomp_motion_planner__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__chomp_motion_planner__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__chomp_motion_planner__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__chomp_motion_planner__ubuntu_focal_amd64__binary) +moveit_chomp_optimizer_adapter | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_chomp_optimizer_adapter__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_chomp_optimizer_adapter__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_chomp_optimizer_adapter__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_chomp_optimizer_adapter__ubuntu_focal_amd64__binary) +pilz_industrial_motion_planner | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__pilz_industrial_motion_planner__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__pilz_industrial_motion_planner__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__pilz_industrial_motion_planner__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__pilz_industrial_motion_planner__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__pilz_industrial_motion_planner__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__pilz_industrial_motion_planner__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__pilz_industrial_motion_planner__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__pilz_industrial_motion_planner__ubuntu_focal_amd64__binary) +pilz_industrial_motion_planner_testutils | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__pilz_industrial_motion_planner_testutils__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__pilz_industrial_motion_planner_testutils__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__pilz_industrial_motion_planner_testutils__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__pilz_industrial_motion_planner_testutils__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__pilz_industrial_motion_planner_testutils__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__pilz_industrial_motion_planner_testutils__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__pilz_industrial_motion_planner_testutils__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__pilz_industrial_motion_planner_testutils__ubuntu_focal_amd64__binary) +**moveit_plugins** | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_plugins__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_plugins__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_plugins__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_plugins__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_plugins__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_plugins__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_plugins__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_plugins__ubuntu_focal_amd64__binary) +moveit_fake_controller_manager | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_fake_controller_manager__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_fake_controller_manager__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_fake_controller_manager__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_fake_controller_manager__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_fake_controller_manager__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_fake_controller_manager__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_fake_controller_manager__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_fake_controller_manager__ubuntu_focal_amd64__binary) +moveit_simple_controller_manager | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_simple_controller_manager__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_simple_controller_manager__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_simple_controller_manager__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_simple_controller_manager__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_simple_controller_manager__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_simple_controller_manager__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_simple_controller_manager__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_simple_controller_manager__ubuntu_focal_amd64__binary) +moveit_ros_control_interface | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_control_interface__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_control_interface__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_control_interface__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_control_interface__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_control_interface__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_control_interface__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_control_interface__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_control_interface__ubuntu_focal_amd64__binary) +**moveit_ros** | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros__ubuntu_focal_amd64__binary) +moveit_ros_planning | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_planning__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_planning__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_planning__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_planning__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_planning__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_planning__ubuntu_focal_amd64__binary) +moveit_ros_move_group | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_move_group__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_move_group__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_move_group__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_move_group__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_move_group__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_move_group__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_move_group__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_move_group__ubuntu_focal_amd64__binary) +moveit_ros_planning_interface | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_planning_interface__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning_interface__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_planning_interface__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning_interface__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_planning_interface__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_planning_interface__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_planning_interface__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_planning_interface__ubuntu_focal_amd64__binary) +moveit_ros_benchmarks | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_benchmarks__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_benchmarks__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_benchmarks__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_benchmarks__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_benchmarks__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_benchmarks__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_benchmarks__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_benchmarks__ubuntu_focal_amd64__binary) +moveit_ros_perception | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_perception__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_perception__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_perception__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_perception__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_perception__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_perception__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_perception__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_perception__ubuntu_focal_amd64__binary) +moveit_ros_occupancy_map_monitor | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_occupancy_map_monitor__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_occupancy_map_monitor__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_occupancy_map_monitor__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_occupancy_map_monitor__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_occupancy_map_monitor__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_occupancy_map_monitor__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_occupancy_map_monitor__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_occupancy_map_monitor__ubuntu_focal_amd64__binary) +moveit_ros_manipulation | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_manipulation__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_manipulation__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_manipulation__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_manipulation__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_manipulation__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_manipulation__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_manipulation__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_manipulation__ubuntu_focal_amd64__binary) +moveit_ros_robot_interaction | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_robot_interaction__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_robot_interaction__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_robot_interaction__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_robot_interaction__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_robot_interaction__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_robot_interaction__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_robot_interaction__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_robot_interaction__ubuntu_focal_amd64__binary) +moveit_ros_visualization | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_visualization__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_visualization__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_visualization__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_visualization__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_visualization__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_visualization__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_visualization__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_visualization__ubuntu_focal_amd64__binary) +moveit_ros_warehouse | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_warehouse__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_warehouse__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_warehouse__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_warehouse__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_warehouse__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_warehouse__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_warehouse__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_warehouse__ubuntu_focal_amd64__binary) +moveit_servo | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_servo__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_servo__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_servo__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_servo__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_servo__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_servo__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_servo__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_servo__ubuntu_focal_amd64__binary) +**moveit_runtime** | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_runtime__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_runtime__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_runtime__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_runtime__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_runtime__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_runtime__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_runtime__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_runtime__ubuntu_focal_amd64__binary) +moveit_commander | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_commander__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_commander__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_commander__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_commander__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_commander__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_commander__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_commander__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_commander__ubuntu_focal_amd64__binary) +moveit_setup_assistant | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_setup_assistant__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_setup_assistant__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_setup_assistant__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_setup_assistant__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_setup_assistant__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_setup_assistant__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_setup_assistant__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_setup_assistant__ubuntu_focal_amd64__binary) +**moveit_msgs** | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_msgs__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_msgs__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_msgs__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_msgs__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_msgs__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_msgs__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_msgs__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_msgs__ubuntu_focal_amd64__binary) +geometric_shapes | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__geometric_shapes__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__geometric_shapes__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__geometric_shapes__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__geometric_shapes__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary) +srdfdom | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__srdfdom__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__srdfdom__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__srdfdom__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__srdfdom__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__srdfdom__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__srdfdom__ubuntu_focal_amd64__binary) +moveit_visual_tools | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_visual_tools__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_visual_tools__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_visual_tools__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_visual_tools__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_visual_tools__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_visual_tools__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_visual_tools__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_visual_tools__ubuntu_focal_amd64__binary) +moveit_tutorials | | | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_tutorials__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_tutorials__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_tutorials__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_tutorials__ubuntu_focal_amd64__binary) ## Stargazers over time diff --git a/moveit/scripts/create_readme_table.py b/moveit/scripts/create_readme_table.py index a7906fc02f..ab661c85c1 100644 --- a/moveit/scripts/create_readme_table.py +++ b/moveit/scripts/create_readme_table.py @@ -1,5 +1,7 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- +# Usage: python moveit/moveit/scripts/create_readme_table.py > /tmp/table.md +# First update supported_distro_ubuntu_dict below! from __future__ import print_function import os @@ -46,7 +48,7 @@ def create_line(package, ros_ubuntu_dict): U=ubuntu[0].upper(), ubuntu=ubuntu.lower(), package=package, - base_url="http://build.ros.org", + base_url="https://build.ros.org", ) for target in ["src", "bin"]: define_urls(target, params) @@ -74,7 +76,6 @@ def create_moveit_buildfarm_table(): # remove {"indigo":"trusty"} and add {"noetic":"fbuntu"} with "fbuntu" # being whatever the 20.04 distro is named supported_distro_ubuntu_dict = { - "kinetic": "xenial", "melodic": "bionic", "noetic": "focal", } From 679d285595f89f4c0be4ea3fe1ab78e9425cf226 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 6 Nov 2021 15:11:39 +0100 Subject: [PATCH 095/229] 1.1.6 --- moveit/CHANGELOG.rst | 3 + moveit_commander/CHANGELOG.rst | 7 ++ moveit_core/CHANGELOG.rst | 59 +++++++++++++ moveit_kinematics/CHANGELOG.rst | 6 ++ .../chomp/chomp_interface/CHANGELOG.rst | 8 ++ .../chomp/chomp_motion_planner/CHANGELOG.rst | 7 ++ .../chomp_optimizer_adapter/CHANGELOG.rst | 6 ++ moveit_planners/moveit_planners/CHANGELOG.rst | 3 + moveit_planners/ompl/CHANGELOG.rst | 9 ++ .../CHANGELOG.rst | 18 ++++ .../CHANGELOG.rst | 5 ++ .../CHANGELOG.rst | 6 ++ moveit_plugins/moveit_plugins/CHANGELOG.rst | 3 + .../CHANGELOG.rst | 6 ++ .../CHANGELOG.rst | 7 ++ moveit_ros/benchmarks/CHANGELOG.rst | 7 ++ moveit_ros/manipulation/CHANGELOG.rst | 8 ++ moveit_ros/move_group/CHANGELOG.rst | 7 ++ moveit_ros/moveit_ros/CHANGELOG.rst | 3 + moveit_ros/moveit_servo/CHANGELOG.rst | 10 +++ .../occupancy_map_monitor/CHANGELOG.rst | 8 ++ moveit_ros/perception/CHANGELOG.rst | 8 ++ moveit_ros/planning/CHANGELOG.rst | 17 ++++ moveit_ros/planning_interface/CHANGELOG.rst | 12 +++ moveit_ros/robot_interaction/CHANGELOG.rst | 6 ++ moveit_ros/visualization/CHANGELOG.rst | 20 +++++ moveit_ros/warehouse/CHANGELOG.rst | 6 ++ moveit_runtime/CHANGELOG.rst | 3 + moveit_setup_assistant/CHANGELOG.rst | 88 +++++++++++++++++++ 29 files changed, 356 insertions(+) diff --git a/moveit/CHANGELOG.rst b/moveit/CHANGELOG.rst index e13afc099d..ba31fd2f92 100644 --- a/moveit/CHANGELOG.rst +++ b/moveit/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_commander/CHANGELOG.rst b/moveit_commander/CHANGELOG.rst index 4d142984f6..05021c05db 100644 --- a/moveit_commander/CHANGELOG.rst +++ b/moveit_commander/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit_commander ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Use relative imports (`#2912 `_) +* Fix trajectory constraints (`#2429 `_) +* Fix ``get_planning_pipeline_id`` in Python MGI (`#2753 `_) +* Contributors: Felix von Drigalski, Kevin Chang, Michael Görner, Robert Haschke + 1.1.5 (2021-05-23) ------------------ * Allow selecting planning pipeline in MotionSequenceAction (`#2657 `_) diff --git a/moveit_core/CHANGELOG.rst b/moveit_core/CHANGELOG.rst index fb575e9f96..f381b2f6ee 100644 --- a/moveit_core/CHANGELOG.rst +++ b/moveit_core/CHANGELOG.rst @@ -2,6 +2,65 @@ Changelog for package moveit_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Silent warning about invalid ``virtual_joint`` in Gazebo setups +* Add ``RobotState::getRigidlyConnectedParentLinkModel`` `#2918 `_ (add RobotState::getRigidlyAttachedParentLink) +* Normalize incoming transforms (`#2920 `_) +* Reworked compiler flags and fixed various warnings (`#2915 `_) + + * Remove unused arguments from global_adjustment_factor() + * Simplify API: Remove obviously unused arguments + * Introduced cmake macro ``moveit_build_options()`` in ``moveit_core`` to centrally define + common build options like ``CMAKE_CXX_STANDARD``, ``CMAKE_BUILD_TYPE``, and compiler warning flags +* Fix uninitialized orientation in default shape pose (`#2896 `_) +* Drop the minimum velocity/acceleration limits for TOTG (`#2937 `_) +* Readability and consistency improvements in TOTG (`#2882 `_) +* Bullet collision: Consider ACM defaults using ``getAllowedCollision()`` (`#2871 `_) +* ``PlanningScene::getPlanningSceneDiffMsg()``: Do not list an object as destroyed when it got attached (`#2864 `_) + + The information in the diff is redundant because attaching implies the removal from the PlanningScene. + In the unlikely case, you relied on the ``REMOVE`` entry in the diff message, + use the newly attached collision object to indicate the same instead. +* Fix Bullet collision: Register ``notify`` function to receive world updates (`#2830 `_) +* Split CollisionPluginLoader (`#2834 `_) + + To avoid circular dependencies, but enable reuse of the ``CollisionPluginLoader``, the non-ROS part was moved into ``moveit_core/moveit_collision_detection.so`` + and the ROS part (reading the plugin name from the parameter server) into ``moveit_ros_planning/moveit_collision_plugin_loader.so`` (as before). +* Use default copy constructor to clone attached objects (`#2855 `_) +* Fix pose-not-set-bug (`#2852 `_) +* Add API for passing a ``RNG`` to ``setToRandomPositionsNearBy()`` (`#2799 `_) +* Fix backwards compatibility for specifying poses for a single collision shape (`#2816 `_) +* Fix Bullet collision returning wrong contact type (`#2829 `_) +* Add ``RobotState::setToDefaultValues(string group)`` (`#2828 `_) +* Fix confusion of tolerance limits in JointConstraint (`#2815 `_) +* Fix RobotState constructor segfault (`#2790 `_) +* Preserve metadata (color, type) when detaching objects (`#2814 `_) +* Introduce a reference frame for collision objects (`#2037 `_) + + ``CollisionObject`` messages are now defined with a ``Pose``. Shapes and subframes are defined relative to that pose. + This makes it easier to place objects with subframes and multiple shapes in the scene. + This causes several changes: + + - ``getFrameTransform()`` now returns this pose instead of the first shape's pose. + - The Rviz plugin's manipulation tab now uses the object's pose instead of the shape pose to evaluate if object's are in the region of interest. + - PlanningScene geometry text files (``.scene``) have changed format. + + Add a line ``0 0 0 0 0 0 1`` under each line with an asterisk to upgrade old files if required. +* Fix bullet plugin library path name (`#2783 `_) +* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 `_) +* ``RobotTrajectory``: convenience constructor + chain setter support (`#2751 `_) +* Fix Windows build (`#2604 `_, `#2776 `_) +* Allow axis-angle representation for orientation constraints (`#2402 `_) +* Optimization: ``reserve()`` vector in advance (`#2732 `_) +* Use same padding/scale for attached collision objects as for parent link (`#2721 `_) +* Optimize ``FCL distanceCallback()``: use thread_local vars, avoid copying (`#2698 `_) +* Remove octomap from catkin_packages ``LIBRARIES`` entry (`#2700 `_) +* Remove deprecated header ``deprecation.h`` (`#2693 `_) +* ``collision_detection_fcl``: Report link_names in correct order (`#2682 `_) +* Move ``OccMapTree`` to ``moveit_core/collision_detection`` (`#2684 `_) +* Contributors: 0Nel, AndyZe, Captain Yoshi, Felix von Drigalski, Jafar Abdi, Jeroen, Jochen Sprickerhof, John Stechschulte, Jonathan Grebe, Max Schwarz, Michael Görner, Michael Wiznitzer, Peter Mitrano, Robert Haschke, Silvio Traversaro, Simon Schmeisser, Tobias Fischer, Tyler Weaver, Wolf Vollprecht, Yuri Rocha, pvanlaar, toru-kuga, v4hn, werner291 + 1.1.5 (2021-05-23) ------------------ * Revert "Lock the octomap/octree while collision checking (`#2683 `_) diff --git a/moveit_kinematics/CHANGELOG.rst b/moveit_kinematics/CHANGELOG.rst index a4665cf89c..042f1dc1bb 100644 --- a/moveit_kinematics/CHANGELOG.rst +++ b/moveit_kinematics/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_kinematics ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` +* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 `_) +* Contributors: Robert Haschke, pvanlaar + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst index ca1d44da19..7ce1926fe2 100644 --- a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package chomp_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Use test_environment.launch in unittests (`#2949 `_) +* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` +* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 `_) +* CHOMP: Rename param 'clearence' to 'clearance' (`#2702 `_, `#2707 `_) +* Contributors: Martin Günther, Michael Görner, Robert Haschke, pvanlaar + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst index 33c95b2fd5..a0e95d0ff1 100644 --- a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package chomp_motion_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` +* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 `_) +* CHOMP: Rename param 'clearence' to 'clearance' (`#2702 `_, `#2707 `_) +* Contributors: Martin Günther, Michael Görner, Robert Haschke, pvanlaar + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst index 2ce53076e6..f157e261ab 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_chomp_optimizer_adapter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` +* CHOMP: Rename param 'clearence' to 'clearance' (`#2702 `_, `#2707 `_) +* Contributors: Martin Günther, Michael Görner, Robert Haschke + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_planners/moveit_planners/CHANGELOG.rst b/moveit_planners/moveit_planners/CHANGELOG.rst index 2a6b1726df..88bc0f818b 100644 --- a/moveit_planners/moveit_planners/CHANGELOG.rst +++ b/moveit_planners/moveit_planners/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_planners ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_planners/ompl/CHANGELOG.rst b/moveit_planners/ompl/CHANGELOG.rst index 4f92d41837..74380e604a 100644 --- a/moveit_planners/ompl/CHANGELOG.rst +++ b/moveit_planners/ompl/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package moveit_planners_ompl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` +* Fix ConstrainedGoalSampler (`#2811 `_): actually call ``sample()`` (`#2872 `_) +* Provide override for missing isValid method (`#2802 `_) +* Add missing dependencies to generated dynamic_reconfigure headers (`#2772 `_) +* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 `_) +* Contributors: Mathias Lüdtke, Michael Görner, Robert Haschke, pvanlaar, v4hn, werner291 + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst index e3325a5963..6db7860dbc 100644 --- a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst +++ b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package pilz_industrial_motion_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Fix calculation of subframe offset (`#2890 `_) +* Remove unused moveit_planning_execution.launch +* Use test_environment.launch in unittests (`#2949 `_) +* Rename launch argument execution_type -> fake_execution_type +* Improve error messages (`#2940 `_) + * Remove deprecated xacro --inorder + * Don't complain about missing limits for irrelevant JMGs + * Avoid duplicate error messages + * Downgrade ERROR to WARN when checking joint limits, report affected joint name + * Quote (possibly empty) planner id +* Consider attached bodies for planning (`#2773 `_, `#2824 `_, `#2878 `_) +* Fix collision detection: consider current PlanningScene (`#2803 `_) +* Add planning_pipeline_id to MotionSequence action + service (`#2755 `_) +* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 `_) +* Contributors: Felix von Drigalski, Leroy Rügemer, Michael Görner, Robert Haschke, Tom Noble, aa-tom, cambel, pvanlaar + 1.1.5 (2021-05-23) ------------------ * Allow selecting planning pipeline in MotionSequenceAction (`#2657 `_) diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst index c2d02e5f29..3a52f57b77 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pilz_industrial_motion_planner_testutils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 `_) +* Contributors: Robert Haschke, pvanlaar + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst index a73125936b..bd397a6b35 100644 --- a/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst +++ b/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_fake_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` +* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 `_) +* Contributors: Robert Haschke, pvanlaar + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_plugins/moveit_plugins/CHANGELOG.rst b/moveit_plugins/moveit_plugins/CHANGELOG.rst index f278b749f5..1f4b2c84ee 100644 --- a/moveit_plugins/moveit_plugins/CHANGELOG.rst +++ b/moveit_plugins/moveit_plugins/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst index ede525eb96..8fa38fd676 100644 --- a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst +++ b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_ros_control_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` +* Fix reversed check in switchControllers (`#2726 `_) +* Contributors: Nathan Brooks, Robert Haschke + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst index fa85618a51..f6dff94a2f 100644 --- a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst +++ b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit_simple_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` +* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 `_) +* Facilitate spotting of empty controller names, using quotes (`#2761 `_) +* Contributors: G.A. vd. Hoorn, Robert Haschke + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_ros/benchmarks/CHANGELOG.rst b/moveit_ros/benchmarks/CHANGELOG.rst index 84b78ee71f..60ef11f3cf 100644 --- a/moveit_ros/benchmarks/CHANGELOG.rst +++ b/moveit_ros/benchmarks/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit_ros_benchmarks ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` +* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 `_) +* Fix predefined poses benchmark example (`#2718 `_) +* Contributors: Captain Yoshi, Robert Haschke + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_ros/manipulation/CHANGELOG.rst b/moveit_ros/manipulation/CHANGELOG.rst index ed2222aee6..412b0702c4 100644 --- a/moveit_ros/manipulation/CHANGELOG.rst +++ b/moveit_ros/manipulation/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package moveit_ros_manipulation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` +* Introduce a reference frame for collision objects (`#2037 `_) +* Add missing dependencies to generated dynamic_reconfigure headers (`#2772 `_) +* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 `_) +* Contributors: Felix von Drigalski, Mathias Lüdtke, Robert Haschke, pvanlaar + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_ros/move_group/CHANGELOG.rst b/moveit_ros/move_group/CHANGELOG.rst index f9dfde4142..2fd523bd35 100644 --- a/moveit_ros/move_group/CHANGELOG.rst +++ b/moveit_ros/move_group/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit_ros_move_group ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` +* Introduce a reference frame for collision objects (`#2037 `_) +* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 `_) +* Contributors: Felix von Drigalski, Robert Haschke, pvanlaar + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_ros/moveit_ros/CHANGELOG.rst b/moveit_ros/moveit_ros/CHANGELOG.rst index b3a04d400b..0fc302e0a1 100644 --- a/moveit_ros/moveit_ros/CHANGELOG.rst +++ b/moveit_ros/moveit_ros/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_ros/moveit_servo/CHANGELOG.rst b/moveit_ros/moveit_servo/CHANGELOG.rst index 12986c8f2e..48af78cadf 100644 --- a/moveit_ros/moveit_servo/CHANGELOG.rst +++ b/moveit_ros/moveit_servo/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package moveit_servo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Backport position limit enforcement from MoveIt2 (`#2898 `_) +* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` +* Minor fixups (`#2759 `_) +* Remove gtest include from non-testing source (`#2747 `_) +* Fix an off-by-one error in servo_calcs.cpp (`#2740 `_) +* Refactor ``moveit_servo::LowPassFilter`` to be assignable (`#2722 `_) +* Contributors: Griswald Brooks, Michael Görner, Michael Wiznitzer, Robert Haschke, luisrayas3, toru-kuga + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst index bf39697d1c..8783014b85 100644 --- a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst +++ b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package moveit_ros_occupancy_map_monitor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` +* Prefer ``std::make_shared`` over ``new`` operator (`#2756 `_) +* Add missing ``OCTOMAP_INCLUDE_DIRS`` (`#2671 `_) +* Move ``OccMapTree`` to ``moveit_core/collision_detection`` (`#2684 `_) +* Contributors: 0Nel, Michael Görner, Robert Haschke, Simon Schmeisser, Tyler Weaver + 1.1.5 (2021-05-23) ------------------ * Revert "Lock the octomap/octree while collision checking (`#2683 `_) diff --git a/moveit_ros/perception/CHANGELOG.rst b/moveit_ros/perception/CHANGELOG.rst index 66cae07257..2a07ed4e95 100644 --- a/moveit_ros/perception/CHANGELOG.rst +++ b/moveit_ros/perception/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package moveit_ros_perception ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` +* Fix clipping of points: only considered points up to ``max_range`` (`#2848 `_) +* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 `_) +* Move ``OccMapTree`` to ``moveit_core/collision_detection`` (`#2684 `_) +* Contributors: Michael Görner, Robert Haschke, Simon Schmeisser, Tyler Weaver, pvanlaar + 1.1.5 (2021-05-23) ------------------ * Revert "Lock the octomap/octree while collision checking (`#2683 `_) diff --git a/moveit_ros/planning/CHANGELOG.rst b/moveit_ros/planning/CHANGELOG.rst index 4bf14edb28..5d62ec94d7 100644 --- a/moveit_ros/planning/CHANGELOG.rst +++ b/moveit_ros/planning/CHANGELOG.rst @@ -2,6 +2,23 @@ Changelog for package moveit_ros_planning ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` +* Split ``CollisionPluginLoader`` (`#2834 `_) + + To avoid circular dependencies, but enable reuse of the ``CollisionPluginLoader``, the non-ROS part was moved into ``moveit_core/moveit_collision_detection.so`` + and the ROS part (reading the plugin name from the parameter server) into ``moveit_ros_planning/moveit_collision_plugin_loader.so`` (as before). +* Introduce a reference frame for collision objects (`#2037 `_) +* Fix RDFLoader: uninitialized member (`#2806 `_) +* Add missing dependencies to generated dynamic_reconfigure headers (`#2772 `_) +* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 `_) +* PSM: Read padding parameters from correct namespace (`#2706 `_) +* Load ``max_safe_path_cost`` into namespace ``sense_for_plan`` (`#2703 `_) +* Fix order of member initialization (`#2687 `_) +* Move ``OccMapTree`` to ``moveit_core/collision_detection`` (`#2684 `_) +* Contributors: Felix von Drigalski, Martin Günther, Mathias Lüdtke, Michael Görner, Robert Haschke, Simon Schmeisser, Tyler Weaver, pvanlaar, werner291 + 1.1.5 (2021-05-23) ------------------ * Revert "Lock the octomap/octree while collision checking (`#2683 `_) diff --git a/moveit_ros/planning_interface/CHANGELOG.rst b/moveit_ros/planning_interface/CHANGELOG.rst index 04aabc8fe8..18f34c1b38 100644 --- a/moveit_ros/planning_interface/CHANGELOG.rst +++ b/moveit_ros/planning_interface/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package moveit_ros_planning_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Use test_environment.launch in unittests (`#2949 `_) +* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` +* Add missing replan/look options to interface (`#2892 `_) +* PSI: get object.pose from new msg field (`#2877 `_) +* Introduce a reference frame for collision objects (`#2037 `_) +* Fix trajectory constraints for ``moveit_commander`` (`#2429 `_) +* ``MGI::setStartState``: Only fetch current state when new state is diff (`#2775 `_) +* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 `_) +* Contributors: Felix von Drigalski, Gauthier Hentz, Kevin Chang, Michael Görner, Robert Haschke, pvanlaar + 1.1.5 (2021-05-23) ------------------ * Allow selecting planning pipeline in MotionSequenceAction (`#2657 `_) diff --git a/moveit_ros/robot_interaction/CHANGELOG.rst b/moveit_ros/robot_interaction/CHANGELOG.rst index 5d68f588b4..5e4d171fdf 100644 --- a/moveit_ros/robot_interaction/CHANGELOG.rst +++ b/moveit_ros/robot_interaction/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_ros_robot_interaction ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` +* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 `_) +* Contributors: Robert Haschke, pvanlaar + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_ros/visualization/CHANGELOG.rst b/moveit_ros/visualization/CHANGELOG.rst index 84c7880dff..4d1935f035 100644 --- a/moveit_ros/visualization/CHANGELOG.rst +++ b/moveit_ros/visualization/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package moveit_ros_visualization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Re-initialize params, subscribers, and topics when the ``MoveGroupNS`` has changed (`#2922 `_) +* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` +* Do not save/restore warehouse parameters (`#2865 `_) but use the ROS parameters only +* PSD: Correctly update robot's base pose (`#2876 `_) +* Fix Python2: convert keys() into list (`#2862 `_) +* MP panel: fix order of input widgets for shape size (`#2847 `_) +* Use relative topic name in trajectory visualization to allow namespacing (`#2835 `_) +* MotionPlanningFrame: Gracefully handle undefined parent widget, e.g. for use via ``librviz.so`` (`#2833 `_) +* Introduce a reference frame for collision objects (`#2037 `_) +* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 `_) +* Support arbitrary real-time factors in trajectory visualization (`#2745 `_) + + Replaced special value ``REALTIME`` to accept arbitrary real-time factors in the format ``x``, e.g. ``3x``. +* Joints tab: Fix handling of mimic + passive joints (`#2744 `_) +* Fix ``TrajectoryPanel``: Keep "Pause/Play" button in correct state (`#2737 `_) +* Fixed error: ``moveit_joy: RuntimeError: dictionary changed size during iteration`` (`#2625 `_, `#2628 `_) +* Contributors: Felix von Drigalski, Michael Görner, Rick Staa, Robert Haschke, Yuri Rocha, lorepieri8, pvanlaar + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_ros/warehouse/CHANGELOG.rst b/moveit_ros/warehouse/CHANGELOG.rst index 153234b518..03a4958608 100644 --- a/moveit_ros/warehouse/CHANGELOG.rst +++ b/moveit_ros/warehouse/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_ros_warehouse ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` +* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 `_) +* Contributors: Robert Haschke, pvanlaar + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_runtime/CHANGELOG.rst b/moveit_runtime/CHANGELOG.rst index 58a8e9bd94..87365e3bc5 100644 --- a/moveit_runtime/CHANGELOG.rst +++ b/moveit_runtime/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_runtime ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ + 1.1.5 (2021-05-23) ------------------ diff --git a/moveit_setup_assistant/CHANGELOG.rst b/moveit_setup_assistant/CHANGELOG.rst index dd33bc3d32..9486988fc9 100644 --- a/moveit_setup_assistant/CHANGELOG.rst +++ b/moveit_setup_assistant/CHANGELOG.rst @@ -2,6 +2,94 @@ Changelog for package moveit_setup_assistant ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.6 (2021-11-06) +------------------ +* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` +* Correctly state not-found package name +* Finish setup_assistant.launch when MSA finished (`#2749 `_) + +* **Enabling the following new features requires recreation of your robot's MoveIt config with the new MSA!** +* Various improvements (`#2932 `_) + + * Define default planner for Pilz pipeline + * Allow checking/unchecking multiple files for generation + * ``moveit.rviz``: Use Orbit view controller + * Rename launch argument ``execution_type`` -> ``fake_execution_type`` to clarify that this parameter is only used for fake controllers + * ``demo.launch``: Start joint + robot-state publishers in fake mode only + * Modularize ``demo_gazebo.launch`` reusing ``demo.launch`` + * ``gazebo.launch`` + + * Delay unpause to ensure that the robot's initial pose is actually held + * Allow initial_joint_positions + * Load URDF via xacro if neccessary + + * Rework ``moveit_controller_manager`` handling + + So far, ``move_group.launch`` distinguished between fake and real-robot operation only. + The boolean launch-file argument ``fake_execution`` was translated to ``moveit_controller_manager = [fake|robot]`` + in ``move_group.launch`` and then further translated to the actual plugin name. + + However, MoveIt provides 3 basic controller manager plugins: + + - ``fake`` = ``moveit_fake_controller_manager::MoveItFakeControllerManager`` (default in ``demo.launch``) + + Doesn't really control the robot. Provides these interpolation types (``fake_execution_type``): + + - ``via points``: jumps to the via points + - ``interpolate``: linearly interpolates between via points (default) + - ``last point``: jumps to the final trajectory point (used for fast execution testing) + - ``ros_control`` = ``moveit_ros_control_interface::MoveItControllerManager`` + + Interfaces to ``ros_control`` controllers. + - ``simple`` = ``moveit_simple_controller_manager/MoveItSimpleControllerManager`` + Interfaces to action servers for ``FollowJointTrajectory`` and/or ``GripperCommand`` + that in turn interface to the low-level robot controllers (typically based on ros_control). + + Now, the argument ``moveit_controller_manager`` allows for switching between these 3 variants using the given names. + Adding more ``*_moveit_controller_manager.launch`` files allows for further extension of this scheme. + +* Rework Controller Handling (`#2945 `_) + + * Write separate controller config files for different MoveIt controller managers: + + - ``fake_controllers.yaml`` for use with ``MoveItFakeControllerManager`` + - ``simple_moveit_controllers.yaml`` handles everything relevant for ``MoveItSimpleControllerManager`` + - ``ros_controllers.yaml`` defines ``ros_control`` controllers + - ``gazebo_controllers.yaml`` lists controllers required for Gazebo + + * Rework controller config generation + + - Provide all types of ``JointTrajectoryController`` (position, velocity, and effort based) + as well as ``FollowJointTrajectory`` and ``GripperCommand`` (use by simple controller manager) + - Use ``effort_controllers/JointTrajectoryController`` as default + - Create ``FollowJointTrajectory`` entries for any ``JointTrajectoryController`` + - Fix controller list generation: always write joint names as a list + + * Code refactoring to clarify that controller widget handles all controllers, not only ``ros_control`` controllers + + * Update widget texts to speak about generic controllers + * Rename ``ROSControllersWidget`` -> ``ControllersWidget`` + * Rename files ``ros_controllers_widget.*`` -> ``controllers_widget.*`` + * Rename ``ros_controllers_config_`` -> ``controller_configs_`` + * Rename functions ``*ROSController*`` -> ``*Controller*`` + * Rename ``ROSControlConfig`` -> ``ControllerConfig`` + +* Fix sensor config handling (`#2708 `_, `#2946 `_) + +* Load planning pipelines into their own namespace (`#2888 `_) +* Add ``jiggle_fraction`` arg to trajopt template (`#2858 `_) +* Only define ``default`` values for input argumens in ``*_planning_pipeline.launch`` templates (`#2849 `_) +* Mention (optional) Gazebo deps in package.xml templates (`#2839 `_) +* Create ``static_transform_publisher`` for each virtual joint type (`#2769 `_) +* Use $(dirname) in launch files (`#2748 `_) +* CHOMP: Read parameters from proper namespace (`#2707 `_) + + * Pilz pipeline: remove unused arg ``start_state_max_bounds_error`` + * Set ``jiggle_fraction`` per pipeline + * Rename param ``clearence`` to ``clearance`` +* Load ``max_safe_path_cost`` into namespace ``sense_for_plan`` (`#2703 `_) +* Contributors: David V. Lu!!, Martin Günther, Max Puig, Michael Görner, Rick Staa, Robert Haschke, pvanlaar, v4hn + 1.1.5 (2021-05-23) ------------------ From ccbfdac484534a2c6343f2a6666ee35bf556921f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 7 Nov 2021 18:27:49 +0100 Subject: [PATCH 096/229] GHA: auto-sync noetic-devel to master (#2952) --- .github/workflows/ci.yaml | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 3841c3d2fe..dd9ffa6661 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -9,7 +9,6 @@ on: push: branches: - master - - "[kmn]*-devel" jobs: default: @@ -150,3 +149,13 @@ jobs: sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete sudo rm -rf ${{ env.BASEDIR }}/target_ws/src du -sh ${{ env.BASEDIR }}/target_ws + + # Sync noetic-devel branch with master on successful build + sync-noetic-devel: + runs-on: ubuntu-latest + needs: default # only trigger on success of default build job + if: github.event_name == 'push' && github.repository_owner == 'ros-planning' && github.ref == 'refs/heads/master' + steps: + - uses: actions/checkout@v2 + - name: Fast-forward noetic-devel to sync with master + run: git push origin HEAD:noetic-devel From 15c8a6e3a0c51ec0b27b13f6b84e54f520c75172 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 7 Nov 2021 23:32:41 +0100 Subject: [PATCH 097/229] Fixup auto-sync noetic-devel to master --- .github/workflows/ci.yaml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index dd9ffa6661..daaa7be1b4 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -158,4 +158,8 @@ jobs: steps: - uses: actions/checkout@v2 - name: Fast-forward noetic-devel to sync with master - run: git push origin HEAD:noetic-devel + run: | + # Extend shallow copy from actions/checkout to connect noetic-devel..master + git fetch --depth=1 origin noetic-devel + git fetch --depth=200 origin master + git push origin master:noetic-devel From fb9fc79a4a6faf6d6aa76e61a0d1c4dfcbc7800f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 8 Nov 2021 12:26:11 +0100 Subject: [PATCH 098/229] Upload controller_list for simple controller manager (#2954) --- .../launch/simple_moveit_controller_manager.launch.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/simple_moveit_controller_manager.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/simple_moveit_controller_manager.launch.xml index dec8cff842..f05ca303f4 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/simple_moveit_controller_manager.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/simple_moveit_controller_manager.launch.xml @@ -3,5 +3,6 @@ + From 4db626d9c3b5d6296b012188a4a0bfe4bddf6bce Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 8 Nov 2021 13:43:52 +0100 Subject: [PATCH 099/229] Fixup auto-sync noetic-devel to master Fix push permissions. --- .github/workflows/ci.yaml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index daaa7be1b4..23274bd83e 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -159,6 +159,10 @@ jobs: - uses: actions/checkout@v2 - name: Fast-forward noetic-devel to sync with master run: | + # Configure push user+url + git config --global user.name 'GHA sync bot' + git config --global user.email 'rhaschke@users.noreply.github.com' + git remote set-url origin https://x-access-token:${{ secrets.GH_PAGES_TOKEN }}@github.com/${{ github.repository }} # Extend shallow copy from actions/checkout to connect noetic-devel..master git fetch --depth=1 origin noetic-devel git fetch --depth=200 origin master From 8a6c6ba003687566e75d0553633e412b130b7f3e Mon Sep 17 00:00:00 2001 From: Tim Redick Date: Tue, 16 Nov 2021 09:43:48 +0100 Subject: [PATCH 100/229] Add ns for depth image & pointcloud octomap updaters (#2916) --- .../depth_image_octomap_updater.h | 1 + .../src/depth_image_octomap_updater.cpp | 14 ++++++++++---- .../pointcloud_octomap_updater.h | 1 + .../src/pointcloud_octomap_updater.cpp | 9 ++++++++- 4 files changed, 20 insertions(+), 5 deletions(-) diff --git a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h index 6779854111..7683d00a4c 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h +++ b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h @@ -80,6 +80,7 @@ class DepthImageOctomapUpdater : public OccupancyMapUpdater ros::Time last_update_time_; std::string filtered_cloud_topic_; + std::string ns_; std::string sensor_type_; std::string image_topic_; std::size_t queue_size_; diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp index ef865194f2..c391989f90 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp +++ b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp @@ -105,6 +105,8 @@ bool DepthImageOctomapUpdater::setParams(XmlRpc::XmlRpcValue& params) readXmlParam(params, "skip_horizontal_pixels", &skip_horizontal_pixels_); if (params.hasMember("filtered_cloud_topic")) filtered_cloud_topic_ = static_cast(params["filtered_cloud_topic"]); + if (params.hasMember("ns")) + ns_ = static_cast(params["ns"]); } catch (XmlRpc::XmlRpcException& ex) { @@ -135,14 +137,18 @@ bool DepthImageOctomapUpdater::initialize() void DepthImageOctomapUpdater::start() { image_transport::TransportHints hints("raw", ros::TransportHints(), nh_); - pub_model_depth_image_ = model_depth_transport_.advertiseCamera("model_depth", 1); + std::string prefix = ""; + if (!ns_.empty()) + prefix = ns_ + "/"; + + pub_model_depth_image_ = model_depth_transport_.advertiseCamera(prefix + "model_depth", 1); if (!filtered_cloud_topic_.empty()) - pub_filtered_depth_image_ = filtered_depth_transport_.advertiseCamera(filtered_cloud_topic_, 1); + pub_filtered_depth_image_ = filtered_depth_transport_.advertiseCamera(prefix + filtered_cloud_topic_, 1); else - pub_filtered_depth_image_ = filtered_depth_transport_.advertiseCamera("filtered_depth", 1); + pub_filtered_depth_image_ = filtered_depth_transport_.advertiseCamera(prefix + "filtered_depth", 1); - pub_filtered_label_image_ = filtered_label_transport_.advertiseCamera("filtered_label", 1); + pub_filtered_label_image_ = filtered_label_transport_.advertiseCamera(prefix + "filtered_label", 1); sub_depth_image_ = input_depth_transport_.subscribeCamera(image_topic_, queue_size_, &DepthImageOctomapUpdater::depthImageCallback, this, hints); diff --git a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h index 69cd73f047..44df18d637 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h +++ b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h @@ -87,6 +87,7 @@ class PointCloudOctomapUpdater : public OccupancyMapUpdater unsigned int point_subsample_; double max_update_rate_; std::string filtered_cloud_topic_; + std::string ns_; ros::Publisher filtered_cloud_publisher_; message_filters::Subscriber* point_cloud_subscriber_; diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp index 859f13ec24..fb37bbf1a0 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp +++ b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp @@ -82,6 +82,8 @@ bool PointCloudOctomapUpdater::setParams(XmlRpc::XmlRpcValue& params) readXmlParam(params, "max_update_rate", &max_update_rate_); if (params.hasMember("filtered_cloud_topic")) filtered_cloud_topic_ = static_cast(params["filtered_cloud_topic"]); + if (params.hasMember("ns")) + ns_ = static_cast(params["ns"]); } catch (XmlRpc::XmlRpcException& ex) { @@ -98,8 +100,13 @@ bool PointCloudOctomapUpdater::initialize() tf_listener_ = std::make_shared(*tf_buffer_, root_nh_); shape_mask_ = std::make_unique(); shape_mask_->setTransformCallback(boost::bind(&PointCloudOctomapUpdater::getShapeTransform, this, _1, _2)); + + std::string prefix = ""; + if (!ns_.empty()) + prefix = ns_ + "/"; if (!filtered_cloud_topic_.empty()) - filtered_cloud_publisher_ = private_nh_.advertise(filtered_cloud_topic_, 10, false); + filtered_cloud_publisher_ = + private_nh_.advertise(prefix + filtered_cloud_topic_, 10, false); return true; } From 82ece8e6e239811cbaf4b6f4fc6b0dbffd0b1dff Mon Sep 17 00:00:00 2001 From: Rick Staa Date: Tue, 16 Nov 2021 17:13:25 +0100 Subject: [PATCH 101/229] MSA: Notice file updates (#2964) This commit fixes a MSA bug causing files in a loaded MoveIt config to be incorrectly classified as externally modified after being written by the "Generate Package" button. As this status is solely based on the file timestamp relative to the timestamp stored in the .setupassistant file, we need to update this timestamp when we wrote the files. --- moveit_setup_assistant/src/tools/moveit_config_data.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index d1e0e8a99f..4535622c36 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -182,7 +182,8 @@ bool MoveItConfigData::outputSetupAssistantFile(const std::string& file_path) emitter << YAML::Value << YAML::BeginMap; emitter << YAML::Key << "author_name" << YAML::Value << author_name_; emitter << YAML::Key << "author_email" << YAML::Value << author_email_; - emitter << YAML::Key << "generated_timestamp" << YAML::Value << std::time(nullptr); // TODO: is this cross-platform? + auto cur_time = std::time(nullptr); + emitter << YAML::Key << "generated_timestamp" << YAML::Value << cur_time; // TODO: is this cross-platform? emitter << YAML::EndMap; emitter << YAML::EndMap; @@ -197,6 +198,10 @@ bool MoveItConfigData::outputSetupAssistantFile(const std::string& file_path) output_stream << emitter.c_str(); output_stream.close(); + /// Update the parsed setup_assistant timestamp + // NOTE: Needed for when people run the MSA generator multiple times in a row. + config_pkg_generated_timestamp_ = cur_time; + return true; // file created successfully } From c4815761ffa350371a0c4ad19d8593758cdd2178 Mon Sep 17 00:00:00 2001 From: Rick Staa Date: Tue, 16 Nov 2021 19:32:30 +0100 Subject: [PATCH 102/229] RDFLoader: clear buffer before reading content (#2963) --- moveit_ros/planning/rdf_loader/src/rdf_loader.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp index cf9fdef1ad..d2a43cc9dd 100644 --- a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp +++ b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp @@ -162,6 +162,7 @@ bool rdf_loader::RDFLoader::loadFileToString(std::string& buffer, const std::str bool rdf_loader::RDFLoader::loadXacroFileToString(std::string& buffer, const std::string& path, const std::vector& xacro_args) { + buffer.clear(); if (path.empty()) { ROS_ERROR_NAMED("rdf_loader", "Path is empty"); From d0d76f171288846a00a7b91935e1571f31d154d3 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 16 Nov 2021 14:00:09 -0600 Subject: [PATCH 103/229] Add waypoint duration to the trajectory deep copy unit test (#2961) * Add waypoint duration to the trajectory deep copy test * Slightly more accurate comments --- .../test/test_robot_trajectory.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp index 5826b6249f..ef8ca9dc1c 100644 --- a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp @@ -115,6 +115,14 @@ class RobotTrajectoryTestFixture : public testing::Test std::vector trajectory_first_state_after_update; trajectory_first_waypoint_after_update->copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update); EXPECT_EQ(trajectory_first_state[0], trajectory_first_state_after_update[0]); + + // Modify the first waypoint duration + double trajectory_first_duration_before_update = trajectory->getWayPointDurationFromPrevious(0); + double new_duration = trajectory_first_duration_before_update + 0.1; + trajectory->setWayPointDurationFromPrevious(0, new_duration); + + // Check that the trajectory's first duration was updated + EXPECT_EQ(trajectory->getWayPointDurationFromPrevious(0), new_duration); } void modifyFirstWaypointAndCheckTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory) @@ -212,7 +220,7 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryShallowCopy) trajectory_copy_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_copy_first_state_after_update); - // Check that we updated the value correctly in the trajectory + // Check that we updated the joint position correctly in the trajectory EXPECT_EQ(trajectory_first_state_after_update[0], trajectory_copy_first_state_after_update[0]); } @@ -238,8 +246,10 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDeepCopy) trajectory_copy_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_copy_first_state_after_update); - // Check that we updated the value correctly in the trajectory + // Check that joint positions changed in the original trajectory but not the deep copy EXPECT_NE(trajectory_first_state_after_update[0], trajectory_copy_first_state_after_update[0]); + // Check that the first waypoint duration changed in the original trajectory but not the deep copy + EXPECT_NE(trajectory->getWayPointDurationFromPrevious(0), trajectory_copy->getWayPointDurationFromPrevious(0)); } int main(int argc, char** argv) From 928afabe6cd857e15ab9e5b1c35f621c3f586ae6 Mon Sep 17 00:00:00 2001 From: Colin Kohler <72100281+Colin-Kohler@users.noreply.github.com> Date: Tue, 16 Nov 2021 15:07:54 -0500 Subject: [PATCH 104/229] MoveitCpp - added ability to set path constraints for PlanningComponent. (#2959) --- .../include/moveit/moveit_cpp/planning_component.h | 4 ++++ .../planning/moveit_cpp/src/planning_component.cpp | 9 +++++++++ 2 files changed, 13 insertions(+) diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index 702c275d78..14873f9011 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -175,6 +175,9 @@ class PlanningComponent /** \brief Set the goal constraints generated from a named target state */ bool setGoal(const std::string& named_target); + /** \brief Set the path constraints used for planning */ + bool setPathConstraints(const moveit_msgs::Constraints& path_constraints); + /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using * default parameters. */ PlanSolution plan(); @@ -202,6 +205,7 @@ class PlanningComponent // The start state used in the planning motion request moveit::core::RobotStatePtr considered_start_state_; std::vector current_goal_constraints_; + moveit_msgs::Constraints current_path_constraints_; PlanRequestParameters plan_request_parameters_; moveit_msgs::WorkspaceParameters workspace_parameters_; bool workspace_parameters_set_ = false; diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index dedd3b0dcc..515e186394 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -99,6 +99,12 @@ const std::string& PlanningComponent::getPlanningGroupName() const return group_name_; } +bool PlanningComponent::setPathConstraints(const moveit_msgs::Constraints& path_constraints) +{ + current_path_constraints_ = path_constraints; + return true; +} + PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParameters& parameters) { last_plan_solution_ = std::make_shared(); @@ -147,6 +153,9 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet } req.goal_constraints = current_goal_constraints_; + // Set path constraints + req.path_constraints = current_path_constraints_; + // Run planning attempt ::planning_interface::MotionPlanResponse res; if (planning_pipeline_names_.find(parameters.planning_pipeline) == planning_pipeline_names_.end()) From ed7b877bb9523a6e88ca629358fee79c4c666a0f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 3 Nov 2021 21:51:57 +0100 Subject: [PATCH 105/229] Joints widget: avoid flickering of the nullspace slider Show a (disabled) dummy slider if there is no nullspace. This avoids flickering between zero and one slider, which is the most common case. Also provide some tooltips to explain the usage. --- .../motion_planning_frame_joints_widget.cpp | 3 ++ ...otion_planning_rviz_plugin_frame_joints.ui | 34 +++++++++++++++++-- 2 files changed, 35 insertions(+), 2 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp index 652c2e0e30..0ee9b0127c 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp @@ -335,6 +335,9 @@ void MotionPlanningFrameJointsWidget::updateNullspaceSliders() if (i == 0) nullspace_.resize(0, 0); + // show/hide dummy slider + ui_->dummy_ns_slider_->setVisible(i == 0); + // hide remaining sliders for (; i < ns_sliders_.size(); ++i) ns_sliders_[i]->hide(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui index 389365e9bd..ecde6d0514 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui @@ -43,14 +43,44 @@ - + + + The sliders below allow for jogging the nullspace of the current configuration, +i.e. trigger joint motions that don't affect the end-effector pose. + +Typically, redundant arms (with 7+ joints) offer such a nullspace. +However, also singular configurations provide a nullspace. + +Each basis vector of the (linear) nullspace is represented by a separate slider. + Nullspace exploration: - + + + + + false + + + The slider will become active if the current robot configuration has a nullspace. +That's typically the case for redundant robots, i.e. 7+ joints, or singular configurations. + + + -1 + + + 1 + + + Qt::Horizontal + + + + From 03ce4cf7dd6f679c1f8a65dc357c1aeab60d2642 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 18 Nov 2021 13:40:21 +0100 Subject: [PATCH 106/229] MPD: Avoid flickering of the progress bar The progress bar shows the number of pending background jobs. If there is only one job pending, the progress bar is shown and immediately hidden as soon as the process is finished. Thus, we shouldn't show the progress bar if there is only one job and thus no actual progress to show. Use the default size and color scheme. --- .../src/motion_planning_display.cpp | 22 ++---- .../ui/motion_planning_rviz_plugin_frame.ui | 70 ------------------- 2 files changed, 7 insertions(+), 85 deletions(-) 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 b68d565d50..b09793418b 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 @@ -336,28 +336,20 @@ void MotionPlanningDisplay::updateBackgroundJobProgressBar() if (n == 0) { - p->setValue(p->maximum()); - p->update(); p->hide(); p->setMaximum(0); + p->setValue(0); } else { - if (n == 1) - { - if (p->maximum() == 0) - p->setValue(0); - else - p->setValue(p->maximum() - 1); - } - else + if (p->maximum() < n) // increase max { - if (p->maximum() < n) - p->setMaximum(n); - else - p->setValue(p->maximum() - n); + p->setMaximum(n); + if (n > 1) // only show bar if there will be a progress to show + p->show(); } - p->show(); + else // progress + p->setValue(p->maximum() - n); p->update(); } } 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 221318fd01..346228701e 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 @@ -2042,82 +2042,12 @@ This is usually achieved by random seeding, which can flip the robot configurati 0 - - - 0 - 5 - - 16777215 5 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 0 - 0 - - - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 0 - 0 - - - - - - - - - 240 - 240 - 240 - - - - - - - 255 - 0 - 0 - - - - - - <html><head/><body><p>Progress of background jobs</p></body></html> From ab42a1d7017b27eb6c353fb29331b2da08ab0039 Mon Sep 17 00:00:00 2001 From: Jochen Sprickerhof Date: Thu, 18 Nov 2021 20:59:06 +0100 Subject: [PATCH 107/229] Switch to std::bind (#2967) * boost::bind -> std::bind grep -rlI --exclude-dir=.git "boost::bind" | xargs sed -i 's/boost::bind/std::bind/g' * Convert bind placeholders grep -rlI --exclude-dir=.git " _[0-9]" | xargs sed -i 's/ _\([0-9]\)/ std::placeholders::_\1/g' * Update bind include header grep -rlI --exclude-dir=.git "boost/bind" | xargs sed -i 's#boost/bind.hpp#functional#' --- .../src/background_processing.cpp | 2 +- .../src/collision_matrix.cpp | 4 +- .../collision_detection/src/world_diff.cpp | 14 ++-- .../collision_detection/test/test_world.cpp | 10 +-- .../src/collision_env_bullet.cpp | 14 ++-- .../src/collision_env_fcl.cpp | 12 ++- .../src/collision_env_distance_field.cpp | 14 ++-- .../src/default_constraint_samplers.cpp | 6 +- .../test/test_constraint_samplers.cpp | 6 +- .../src/kinematic_constraint.cpp | 4 +- .../src/planning_request_adapter.cpp | 15 ++-- moveit_core/robot_state/src/robot_state.cpp | 8 +- .../src/kinematics_constraint_aware.cpp | 5 +- .../test/test_kinematics_plugin.cpp | 6 +- .../src/ompl_planner_manager.cpp | 2 +- .../src/move_group_sequence_action.cpp | 10 +-- .../src/trajectory_functions.cpp | 4 +- .../integrationtest_command_list_manager.cpp | 4 +- ...rationtest_sequence_service_capability.cpp | 4 +- .../test/unittest_trajectory_functions.cpp | 4 +- .../src/xml_testdata_loader.cpp | 40 +++++----- .../src/sbpl_meta_interface.cpp | 8 +- .../src/moveit_fake_controllers.cpp | 2 +- .../gripper_controller_handle.h | 9 ++- ...low_joint_trajectory_controller_handle.cpp | 10 ++- .../benchmarks/src/benchmark_execution.cpp | 13 +-- .../src/pick_place_action_capability.cpp | 40 +++++----- .../src/approach_and_translate_stage.cpp | 11 +-- .../pick_place/src/manipulation_pipeline.cpp | 2 +- .../pick_place/src/pick_place.cpp | 4 +- .../pick_place/src/pick_place_params.cpp | 4 +- .../src/reachable_valid_pose_filter.cpp | 7 +- .../cartesian_path_service_capability.cpp | 5 +- .../execute_trajectory_action_capability.cpp | 4 +- .../execute_trajectory_service_capability.cpp | 3 +- .../kinematics_service_capability.cpp | 11 +-- .../move_action_capability.cpp | 20 ++--- .../src/occupancy_map_monitor.cpp | 15 ++-- .../src/occupancy_map_server.cpp | 2 +- .../src/depth_image_octomap_updater.cpp | 3 +- .../src/lazy_free_space_updater.cpp | 4 +- .../src/depth_self_filter_nodelet.cpp | 2 +- .../mesh_filter/test/mesh_filter_test.cpp | 3 +- .../src/pointcloud_octomap_updater.cpp | 9 ++- .../src/kinematics_plugin_loader.cpp | 5 +- .../plan_execution/src/plan_execution.cpp | 13 +-- .../plan_execution/src/plan_with_sensing.cpp | 4 +- .../src/evaluate_collision_checking_speed.cpp | 2 +- .../src/current_state_monitor.cpp | 2 +- .../src/planning_scene_monitor.cpp | 48 +++++------ .../src/trajectory_monitor.cpp | 2 +- .../src/trajectory_execution_manager.cpp | 6 +- .../src/move_group_interface.cpp | 2 +- .../src/interaction_handler.cpp | 14 ++-- .../src/robot_interaction.cpp | 8 +- .../test/locked_robot_state_test.cpp | 2 +- .../src/motion_planning_display.cpp | 36 +++++---- .../src/motion_planning_frame.cpp | 28 +++---- .../src/motion_planning_frame_context.cpp | 17 ++-- .../motion_planning_frame_manipulation.cpp | 15 ++-- .../src/motion_planning_frame_objects.cpp | 22 ++--- .../src/motion_planning_frame_planning.cpp | 18 ++--- .../src/motion_planning_frame_scenes.cpp | 18 ++--- .../src/planning_scene_display.cpp | 13 +-- .../warehouse/src/save_to_warehouse.cpp | 10 ++- .../warehouse/src/warehouse_services.cpp | 12 +-- .../src/tools/compute_default_collisions.cpp | 2 +- .../widgets/configuration_files_widget.cpp | 80 +++++++++---------- .../src/widgets/default_collisions_widget.cpp | 5 +- 69 files changed, 411 insertions(+), 352 deletions(-) diff --git a/moveit_core/background_processing/src/background_processing.cpp b/moveit_core/background_processing/src/background_processing.cpp index 5614aa514a..00df01de05 100644 --- a/moveit_core/background_processing/src/background_processing.cpp +++ b/moveit_core/background_processing/src/background_processing.cpp @@ -46,7 +46,7 @@ BackgroundProcessing::BackgroundProcessing() // spin a thread that will process user events run_processing_thread_ = true; processing_ = false; - processing_thread_ = std::make_unique(boost::bind(&BackgroundProcessing::processingThread, this)); + processing_thread_ = std::make_unique(std::bind(&BackgroundProcessing::processingThread, this)); } BackgroundProcessing::~BackgroundProcessing() diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp index 05ea5b66ba..369774ef18 100644 --- a/moveit_core/collision_detection/src/collision_matrix.cpp +++ b/moveit_core/collision_detection/src/collision_matrix.cpp @@ -35,7 +35,7 @@ /* Author: Ioan Sucan, E. Gil Jones */ #include -#include +#include #include namespace collision_detection @@ -268,7 +268,7 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const else if (!found1 && found2) fn = fn2; else if (found1 && found2) - fn = boost::bind(&andDecideContact, fn1, fn2, _1); + fn = std::bind(&andDecideContact, fn1, fn2, std::placeholders::_1); else return false; return true; diff --git a/moveit_core/collision_detection/src/world_diff.cpp b/moveit_core/collision_detection/src/world_diff.cpp index dcf75b6d4d..c43b9d920a 100644 --- a/moveit_core/collision_detection/src/world_diff.cpp +++ b/moveit_core/collision_detection/src/world_diff.cpp @@ -35,7 +35,7 @@ /* Author: Acorn Pooley, Ioan Sucan */ #include -#include +#include namespace collision_detection { @@ -52,7 +52,8 @@ WorldDiff::WorldDiff() WorldDiff::WorldDiff(const WorldPtr& world) : world_(world) { - observer_handle_ = world->addObserver(boost::bind(&WorldDiff::notify, this, _1, _2)); + observer_handle_ = + world->addObserver(std::bind(&WorldDiff::notify, this, std::placeholders::_1, std::placeholders::_2)); } WorldDiff::WorldDiff(WorldDiff& other) @@ -63,7 +64,8 @@ WorldDiff::WorldDiff(WorldDiff& other) changes_ = other.changes_; WorldWeakPtr(world).swap(world_); - observer_handle_ = world->addObserver(boost::bind(&WorldDiff::notify, this, _1, _2)); + observer_handle_ = + world->addObserver(std::bind(&WorldDiff::notify, this, std::placeholders::_1, std::placeholders::_2)); } } @@ -87,7 +89,8 @@ void WorldDiff::reset(const WorldPtr& world) old_world->removeObserver(observer_handle_); WorldWeakPtr(world).swap(world_); - observer_handle_ = world->addObserver(boost::bind(&WorldDiff::notify, this, _1, _2)); + observer_handle_ = + world->addObserver(std::bind(&WorldDiff::notify, this, std::placeholders::_1, std::placeholders::_2)); } void WorldDiff::setWorld(const WorldPtr& world) @@ -101,7 +104,8 @@ void WorldDiff::setWorld(const WorldPtr& world) WorldWeakPtr(world).swap(world_); - observer_handle_ = world->addObserver(boost::bind(&WorldDiff::notify, this, _1, _2)); + observer_handle_ = + world->addObserver(std::bind(&WorldDiff::notify, this, std::placeholders::_1, std::placeholders::_2)); world->notifyObserverAllObjects(observer_handle_, World::CREATE | World::ADD_SHAPE); } diff --git a/moveit_core/collision_detection/test/test_world.cpp b/moveit_core/collision_detection/test/test_world.cpp index 126143dafc..824ce083f1 100644 --- a/moveit_core/collision_detection/test/test_world.cpp +++ b/moveit_core/collision_detection/test/test_world.cpp @@ -37,7 +37,7 @@ #include #include #include -#include +#include TEST(World, AddRemoveShape) { @@ -236,7 +236,7 @@ TEST(World, TrackChanges) TestAction ta; collision_detection::World::ObserverHandle observer_ta; - observer_ta = world.addObserver(boost::bind(TrackChangesNotify, &ta, _1, _2)); + observer_ta = world.addObserver(std::bind(TrackChangesNotify, &ta, std::placeholders::_1, std::placeholders::_2)); // Create some shapes shapes::ShapePtr ball(new shapes::Sphere(1.0)); @@ -267,7 +267,7 @@ TEST(World, TrackChanges) TestAction ta2; collision_detection::World::ObserverHandle observer_ta2; - observer_ta2 = world.addObserver(boost::bind(TrackChangesNotify, &ta2, _1, _2)); + observer_ta2 = world.addObserver(std::bind(TrackChangesNotify, &ta2, std::placeholders::_1, std::placeholders::_2)); world.addToObject("obj2", cyl, Eigen::Isometry3d::Identity()); @@ -305,7 +305,7 @@ TEST(World, TrackChanges) TestAction ta3; collision_detection::World::ObserverHandle observer_ta3; - observer_ta3 = world.addObserver(boost::bind(TrackChangesNotify, &ta3, _1, _2)); + observer_ta3 = world.addObserver(std::bind(TrackChangesNotify, &ta3, std::placeholders::_1, std::placeholders::_2)); bool rm_good = world.removeShapeFromObject("obj2", cyl); EXPECT_TRUE(rm_good); @@ -371,7 +371,7 @@ TEST(World, ObjectPoseAndSubframes) TestAction ta; collision_detection::World::ObserverHandle observer_ta; - observer_ta = world.addObserver(boost::bind(TrackChangesNotify, &ta, _1, _2)); + observer_ta = world.addObserver(std::bind(TrackChangesNotify, &ta, std::placeholders::_1, std::placeholders::_2)); // Create shapes shapes::ShapePtr ball(new shapes::Sphere(1.0)); diff --git a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp index 3885f721dc..3e2ea9b1ab 100644 --- a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp +++ b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include namespace collision_detection @@ -54,7 +54,8 @@ CollisionEnvBullet::CollisionEnvBullet(const moveit::core::RobotModelConstPtr& m : CollisionEnv(model, padding, scale) { // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvBullet::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); for (const std::pair& link : robot_model_->getURDF()->links_) { @@ -67,7 +68,8 @@ CollisionEnvBullet::CollisionEnvBullet(const moveit::core::RobotModelConstPtr& m : CollisionEnv(model, world, padding, scale) { // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvBullet::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); for (const std::pair& link : robot_model_->getURDF()->links_) { @@ -81,7 +83,8 @@ CollisionEnvBullet::CollisionEnvBullet(const CollisionEnvBullet& other, const Wo : CollisionEnv(other, world) { // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvBullet::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); for (const std::pair& link : other.robot_model_->getURDF()->links_) { @@ -304,7 +307,8 @@ void CollisionEnvBullet::setWorld(const WorldPtr& world) CollisionEnv::setWorld(world); // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvBullet::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); // get notifications any objects already in the new world getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp index 4895b27baf..b549f179fb 100644 --- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp @@ -105,7 +105,8 @@ CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, manager_ = std::make_unique(); // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvFCL::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); } CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding, @@ -140,7 +141,8 @@ CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, manager_ = std::make_unique(); // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvFCL::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); } @@ -162,7 +164,8 @@ CollisionEnvFCL::CollisionEnvFCL(const CollisionEnvFCL& other, const WorldPtr& w // manager_->update(); // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvFCL::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); } void CollisionEnvFCL::getAttachedBodyObjects(const moveit::core::AttachedBody* ab, @@ -403,7 +406,8 @@ void CollisionEnvFCL::setWorld(const WorldPtr& world) CollisionEnv::setWorld(world); // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvFCL::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); // get notifications any objects already in the new world getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); diff --git a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp index dee29b010b..5e2c34bfbe 100644 --- a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include @@ -69,7 +69,8 @@ CollisionEnvDistanceField::CollisionEnvDistanceField( distance_field_cache_entry_world_ = generateDistanceFieldCacheEntryWorld(); // request notifications about changes to world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvDistanceField::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvDistanceField::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); } CollisionEnvDistanceField::CollisionEnvDistanceField( @@ -85,7 +86,8 @@ CollisionEnvDistanceField::CollisionEnvDistanceField( distance_field_cache_entry_world_ = generateDistanceFieldCacheEntryWorld(); // request notifications about changes to world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvDistanceField::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvDistanceField::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); } @@ -108,7 +110,8 @@ CollisionEnvDistanceField::CollisionEnvDistanceField(const CollisionEnvDistanceF planning_scene_ = std::make_shared(robot_model_); // request notifications about changes to world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvDistanceField::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvDistanceField::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); } @@ -1705,7 +1708,8 @@ void CollisionEnvDistanceField::setWorld(const WorldPtr& world) CollisionEnv::setWorld(world); // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvDistanceField::notifyObjectChange, this, _1, _2)); + observer_handle_ = getWorld()->addObserver( + std::bind(&CollisionEnvDistanceField::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); // get notifications any objects already in the new world getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index 55ed6a618a..5d28e21206 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -36,7 +36,7 @@ #include #include -#include +#include namespace constraint_samplers { @@ -565,8 +565,8 @@ bool IKConstraintSampler::sampleHelper(moveit::core::RobotState& state, const mo kinematics::KinematicsBase::IKCallbackFn adapted_ik_validity_callback; if (group_state_validity_callback_) - adapted_ik_validity_callback = - boost::bind(&samplingIkCallbackFnAdapter, &state, jmg_, group_state_validity_callback_, _1, _2, _3); + adapted_ik_validity_callback = std::bind(&samplingIkCallbackFnAdapter, &state, jmg_, group_state_validity_callback_, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); for (unsigned int a = 0; a < max_attempts; ++a) { diff --git a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp index 3cfd3a957f..f01f1e188a 100644 --- a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp @@ -49,7 +49,7 @@ #include #include #include -#include +#include #include "pr2_arm_kinematics_plugin.h" @@ -82,8 +82,8 @@ class LoadPlanningModelsPr2 : public testing::Test pr2_kinematics_plugin_left_arm_->initialize(*robot_model_, "left_arm", "torso_lift_link", { "l_wrist_roll_link" }, .01); - func_right_arm_ = boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverRightArm, this, _1); - func_left_arm_ = boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverLeftArm, this, _1); + func_right_arm_ = std::bind(&LoadPlanningModelsPr2::getKinematicsSolverRightArm, this, std::placeholders::_1); + func_left_arm_ = std::bind(&LoadPlanningModelsPr2::getKinematicsSolverLeftArm, this, std::placeholders::_1); std::map allocators; allocators["right_arm"] = func_right_arm_; diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 2588bd4541..7c3629c3a9 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include @@ -1112,7 +1112,7 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const moveit::core::Robo collision_detection::CollisionRequest req; collision_detection::CollisionResult res; collision_detection::AllowedCollisionMatrix acm; - acm.setDefaultEntry("cone", boost::bind(&VisibilityConstraint::decideContact, this, _1)); + acm.setDefaultEntry("cone", std::bind(&VisibilityConstraint::decideContact, this, std::placeholders::_1)); req.contacts = true; req.verbose = verbose; req.max_contacts = 1; diff --git a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp index aa04e8a8c9..35fac2f52d 100644 --- a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp +++ b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp @@ -35,7 +35,7 @@ /* Author: Ioan Sucan */ #include -#include +#include #include // we could really use some c++11 lambda functions here :) @@ -63,8 +63,9 @@ bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManag planning_interface::MotionPlanResponse& res, std::vector& added_path_index) const { - return adaptAndPlan(boost::bind(&callPlannerInterfaceSolve, planner.get(), _1, _2, _3), planning_scene, req, res, - added_path_index); + return adaptAndPlan(std::bind(&callPlannerInterfaceSolve, planner.get(), std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3), + planning_scene, req, res, added_path_index); } bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, @@ -145,10 +146,12 @@ bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::Planner // if there are adapters, construct a function pointer for each, in order, // so that in the end we have a nested sequence of function pointers that call the adapters in the correct order. - PlanningRequestAdapter::PlannerFn fn = boost::bind(&callAdapter1, adapters_.back().get(), planner, _1, _2, _3, - boost::ref(added_path_index_each.back())); + PlanningRequestAdapter::PlannerFn fn = + std::bind(&callAdapter1, adapters_.back().get(), planner, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::ref(added_path_index_each.back())); for (int i = adapters_.size() - 2; i >= 0; --i) - fn = boost::bind(&callAdapter2, adapters_[i].get(), fn, _1, _2, _3, boost::ref(added_path_index_each[i])); + fn = std::bind(&callAdapter2, adapters_[i].get(), fn, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::ref(added_path_index_each[i])); bool result = fn(planning_scene, req, res); added_path_index.clear(); diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index c60cd509cc..3d74b0a439 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -44,7 +44,7 @@ #include #include #include -#include +#include #include namespace moveit @@ -1780,7 +1780,8 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is // set callback function kinematics::KinematicsBase::IKCallbackFn ik_callback_fn; if (constraint) - ik_callback_fn = boost::bind(&ikCallbackFnAdapter, this, jmg, constraint, _1, _2, _3); + ik_callback_fn = std::bind(&ikCallbackFnAdapter, this, jmg, constraint, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3); // Bijection const std::vector& bij = jmg->getKinematicsSolverJointBijection(); @@ -1923,7 +1924,8 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: std::vector ik_queries(poses_in.size()); kinematics::KinematicsBase::IKCallbackFn ik_callback_fn; if (constraint) - ik_callback_fn = boost::bind(&ikCallbackFnAdapter, this, jmg, constraint, _1, _2, _3); + ik_callback_fn = std::bind(&ikCallbackFnAdapter, this, jmg, constraint, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3); for (std::size_t i = 0; i < transformed_poses.size(); ++i) { diff --git a/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp b/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp index b75ae595d2..b9fdc21666 100644 --- a/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp +++ b/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include namespace kinematics_constraint_aware { @@ -161,7 +161,8 @@ bool KinematicsConstraintAware::getIK(const planning_scene::PlanningSceneConstPt transformPoses(planning_scene, kinematic_state, request.pose_stamped_vector_, kinematic_model_->getModelFrame()); moveit::core::StateValidityCallbackFn constraint_callback_fn = - boost::bind(&KinematicsConstraintAware::validityCallbackFn, this, planning_scene, request, response, _1, _2); + std::bind(&KinematicsConstraintAware::validityCallbackFn, this, planning_scene, request, response, + std::placeholders::_1, std::placeholders::_2); bool result = false; if (has_sub_groups_) diff --git a/moveit_kinematics/test/test_kinematics_plugin.cpp b/moveit_kinematics/test/test_kinematics_plugin.cpp index 7ca176cd0b..60fc35b37f 100644 --- a/moveit_kinematics/test/test_kinematics_plugin.cpp +++ b/moveit_kinematics/test/test_kinematics_plugin.cpp @@ -37,7 +37,7 @@ #include #include -#include +#include #include #include #include @@ -592,7 +592,9 @@ TEST_F(KinematicsTest, searchIKWithCallback) } kinematics_solver_->searchPositionIK(poses[0], fk_values, timeout_, solution, - boost::bind(&KinematicsTest::searchIKCallback, this, _1, _2, _3), error_code); + std::bind(&KinematicsTest::searchIKCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3), + error_code); if (error_code.val == error_code.SUCCESS) success++; else diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp index 199c873ab9..c201a066fe 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp @@ -295,7 +295,7 @@ class OMPLPlannerManager : public planning_interface::PlannerManager pub_valid_states_ = nh_.advertise("ompl_planner_valid_states", 5); pub_valid_traj_ = nh_.advertise("ompl_planner_valid_trajectories", 5); display_random_valid_states_ = true; - // pub_valid_states_thread_.reset(new boost::thread(boost::bind(&OMPLPlannerManager::displayRandomValidStates, + // pub_valid_states_thread_.reset(new boost::thread(std::bind(&OMPLPlannerManager::displayRandomValidStates, // this))); } } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp index be1caeb2c6..d7c0bac0f6 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp @@ -63,8 +63,8 @@ void MoveGroupSequenceAction::initialize() ROS_INFO_STREAM("initialize move group sequence action"); move_action_server_ = std::make_unique>( root_node_handle_, "sequence_move_group", - boost::bind(&MoveGroupSequenceAction::executeSequenceCallback, this, _1), false); - move_action_server_->registerPreemptCallback(boost::bind(&MoveGroupSequenceAction::preemptMoveCallback, this)); + std::bind(&MoveGroupSequenceAction::executeSequenceCallback, this, std::placeholders::_1), false); + move_action_server_->registerPreemptCallback(std::bind(&MoveGroupSequenceAction::preemptMoveCallback, this)); move_action_server_->start(); command_list_manager_ = std::make_unique( @@ -137,10 +137,10 @@ void MoveGroupSequenceAction::executeSequenceCallbackPlanAndExecute( opt.replan_ = goal->planning_options.replan; opt.replan_attempts_ = goal->planning_options.replan_attempts; opt.replan_delay_ = goal->planning_options.replan_delay; - opt.before_execution_callback_ = boost::bind(&MoveGroupSequenceAction::startMoveExecutionCallback, this); + opt.before_execution_callback_ = std::bind(&MoveGroupSequenceAction::startMoveExecutionCallback, this); - opt.plan_callback_ = - boost::bind(&MoveGroupSequenceAction::planUsingSequenceManager, this, boost::cref(goal->request), _1); + opt.plan_callback_ = std::bind(&MoveGroupSequenceAction::planUsingSequenceManager, this, boost::cref(goal->request), + std::placeholders::_1); if (goal->planning_options.look_around && context_->plan_with_sensing_) { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index 93ba27e133..03694adf8f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -74,8 +74,8 @@ bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::Plannin rstate.setVariablePositions(seed); moveit::core::GroupStateValidityCallbackFn ik_constraint_function; - ik_constraint_function = - boost::bind(&pilz_industrial_motion_planner::isStateColliding, check_self_collision, scene, _1, _2, _3); + ik_constraint_function = std::bind(&pilz_industrial_motion_planner::isStateColliding, check_self_collision, scene, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); // call ik if (rstate.setFromIK(robot_model->getJointModelGroup(group_name), pose, link_name, timeout, ik_constraint_function)) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.cpp index e7f1f01b99..fa596dcbc0 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.cpp @@ -624,13 +624,13 @@ TEST_F(IntegrationTestCommandListManager, TestGroupSpecificStartState) seq.erase(4, seq.size()); Gripper& gripper{ seq.getCmd(0) }; - gripper.getStartConfiguration().setCreateJointNameFunc(std::bind(&createGripperJointName, _1)); + gripper.getStartConfiguration().setCreateJointNameFunc(std::bind(&createGripperJointName, std::placeholders::_1)); // By deleting the model we guarantee that the start state only consists // of joints of the gripper group without the manipulator gripper.getStartConfiguration().clearModel(); PtpJointCart& ptp{ seq.getCmd(1) }; - ptp.getStartConfiguration().setCreateJointNameFunc(std::bind(&createManipulatorJointName, _1)); + ptp.getStartConfiguration().setCreateJointNameFunc(std::bind(&createManipulatorJointName, std::placeholders::_1)); // By deleting the model we guarantee that the start state only consists // of joints of the manipulator group without the gripper ptp.getStartConfiguration().clearModel(); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp index 2c04855230..5b5bdc7c73 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp @@ -248,7 +248,9 @@ TEST_F(IntegrationTestSequenceService, TestSecondTrajInvalidStartState) // Set start state using std::placeholders::_1; - JointConfiguration config{ "MyGroupName", { -1., 2., -3., 4., -5., 0. }, std::bind(&createJointName, _1) }; + JointConfiguration config{ "MyGroupName", + { -1., 2., -3., 4., -5., 0. }, + std::bind(&createJointName, std::placeholders::_1) }; req_list.items[1].req.start_state.joint_state = config.toSensorMsg(); moveit_msgs::GetMotionSequence srv; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp index 4d4eb79ec3..30ce75d7a6 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp @@ -456,7 +456,7 @@ TEST_P(TrajectoryFunctionsTestOnlyGripper, testComputePoseIKSelfCollisionForVali std::vector ik_state; std::transform(ik_actual1.begin(), ik_actual1.end(), std::back_inserter(ik_state), - boost::bind(&std::map::value_type::second, _1)); + std::bind(&std::map::value_type::second, std::placeholders::_1)); rstate.setJointGroupPositions(jmg, ik_state); rstate.update(); @@ -477,7 +477,7 @@ TEST_P(TrajectoryFunctionsTestOnlyGripper, testComputePoseIKSelfCollisionForVali std::vector ik_state2; std::transform(ik_actual2.begin(), ik_actual2.end(), std::back_inserter(ik_state2), - boost::bind(&std::map::value_type::second, _1)); + std::bind(&std::map::value_type::second, std::placeholders::_1)); rstate.setJointGroupPositions(jmg, ik_state2); rstate.update(); diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp index 69054bc45d..38b5f4549b 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp @@ -141,27 +141,27 @@ XmlTestdataLoader::XmlTestdataLoader(const std::string& path_filename) : Testdat pt::read_xml(path_filename_, tree_, pt::xml_parser::no_comments); using std::placeholders::_1; - cmd_getter_funcs_["ptp"] = - AbstractCmdGetterUPtr(new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getPtpJoint, this, _1))); + cmd_getter_funcs_["ptp"] = AbstractCmdGetterUPtr( + new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getPtpJoint, this, std::placeholders::_1))); cmd_getter_funcs_["ptp_joint_cart"] = AbstractCmdGetterUPtr( - new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getPtpJointCart, this, _1))); - cmd_getter_funcs_["ptp_cart_cart"] = - AbstractCmdGetterUPtr(new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getPtpCart, this, _1))); - - cmd_getter_funcs_["lin"] = - AbstractCmdGetterUPtr(new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getLinJoint, this, _1))); - cmd_getter_funcs_["lin_cart"] = - AbstractCmdGetterUPtr(new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getLinCart, this, _1))); - - cmd_getter_funcs_["circ_center_cart"] = AbstractCmdGetterUPtr( - new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getCircCartCenterCart, this, _1))); - cmd_getter_funcs_["circ_interim_cart"] = AbstractCmdGetterUPtr( - new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getCircCartInterimCart, this, _1))); - cmd_getter_funcs_["circ_joint_interim_cart"] = AbstractCmdGetterUPtr( - new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getCircJointInterimCart, this, _1))); - - cmd_getter_funcs_["gripper"] = - AbstractCmdGetterUPtr(new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getGripper, this, _1))); + new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getPtpJointCart, this, std::placeholders::_1))); + cmd_getter_funcs_["ptp_cart_cart"] = AbstractCmdGetterUPtr( + new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getPtpCart, this, std::placeholders::_1))); + + cmd_getter_funcs_["lin"] = AbstractCmdGetterUPtr( + new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getLinJoint, this, std::placeholders::_1))); + cmd_getter_funcs_["lin_cart"] = AbstractCmdGetterUPtr( + new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getLinCart, this, std::placeholders::_1))); + + cmd_getter_funcs_["circ_center_cart"] = AbstractCmdGetterUPtr(new CmdGetterAdapter( + std::bind(&XmlTestdataLoader::getCircCartCenterCart, this, std::placeholders::_1))); + cmd_getter_funcs_["circ_interim_cart"] = AbstractCmdGetterUPtr(new CmdGetterAdapter( + std::bind(&XmlTestdataLoader::getCircCartInterimCart, this, std::placeholders::_1))); + cmd_getter_funcs_["circ_joint_interim_cart"] = AbstractCmdGetterUPtr(new CmdGetterAdapter( + std::bind(&XmlTestdataLoader::getCircJointInterimCart, this, std::placeholders::_1))); + + cmd_getter_funcs_["gripper"] = AbstractCmdGetterUPtr( + new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getGripper, this, std::placeholders::_1))); } XmlTestdataLoader::XmlTestdataLoader(const std::string& path_filename, diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp index 65e6dfef33..36debbe2af 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp @@ -57,10 +57,10 @@ bool SBPLMetaInterface::solve(const planning_scene::PlanningSceneConstPtr& plann PlanningParameters param_no_bfs; param_no_bfs.use_bfs_ = false; moveit_msgs::GetMotionPlan::Response res1, res2; - boost::thread thread1(boost::bind(&SBPLMetaInterface::runSolver, this, true, boost::cref(planning_scene), - boost::cref(req), boost::ref(res1), param_bfs)); - boost::thread thread2(boost::bind(&SBPLMetaInterface::runSolver, this, false, boost::cref(planning_scene), - boost::cref(req), boost::ref(res2), param_no_bfs)); + boost::thread thread1(std::bind(&SBPLMetaInterface::runSolver, this, true, boost::cref(planning_scene), + boost::cref(req), boost::ref(res1), param_bfs)); + boost::thread thread2(std::bind(&SBPLMetaInterface::runSolver, this, false, boost::cref(planning_scene), + boost::cref(req), boost::ref(res2), param_no_bfs)); boost::mutex::scoped_lock lock(planner_done_mutex_); planner_done_condition_.wait(lock); diff --git a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.cpp b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.cpp index 156142157e..50c09184a7 100644 --- a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.cpp +++ b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.cpp @@ -124,7 +124,7 @@ bool ThreadedController::sendTrajectory(const moveit_msgs::RobotTrajectory& t) cancelTrajectory(); // cancel any previous fake motion cancel_ = false; status_ = moveit_controller_manager::ExecutionStatus::PREEMPTED; - thread_ = boost::thread(boost::bind(&ThreadedController::execTrajectory, this, t)); + thread_ = boost::thread(std::bind(&ThreadedController::execTrajectory, this, t)); return true; } diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h index aae275a2e2..212992de5b 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h @@ -133,10 +133,11 @@ class GripperControllerHandle : public ActionBasedControllerHandlesendGoal(goal, - boost::bind(&GripperControllerHandle::controllerDoneCallback, this, _1, _2), - boost::bind(&GripperControllerHandle::controllerActiveCallback, this), - boost::bind(&GripperControllerHandle::controllerFeedbackCallback, this, _1)); + controller_action_client_->sendGoal( + goal, + std::bind(&GripperControllerHandle::controllerDoneCallback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&GripperControllerHandle::controllerActiveCallback, this), + std::bind(&GripperControllerHandle::controllerFeedbackCallback, this, std::placeholders::_1)); done_ = false; last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING; diff --git a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp index d911ed6357..93eb3750a6 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp @@ -62,10 +62,12 @@ bool FollowJointTrajectoryControllerHandle::sendTrajectory(const moveit_msgs::Ro control_msgs::FollowJointTrajectoryGoal goal = goal_template_; goal.trajectory = trajectory.joint_trajectory; - controller_action_client_->sendGoal( - goal, boost::bind(&FollowJointTrajectoryControllerHandle::controllerDoneCallback, this, _1, _2), - boost::bind(&FollowJointTrajectoryControllerHandle::controllerActiveCallback, this), - boost::bind(&FollowJointTrajectoryControllerHandle::controllerFeedbackCallback, this, _1)); + controller_action_client_->sendGoal(goal, + std::bind(&FollowJointTrajectoryControllerHandle::controllerDoneCallback, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&FollowJointTrajectoryControllerHandle::controllerActiveCallback, this), + std::bind(&FollowJointTrajectoryControllerHandle::controllerFeedbackCallback, + this, std::placeholders::_1)); done_ = false; last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING; return true; diff --git a/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp b/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp index 373e5e6b8b..9c6d88fc4f 100644 --- a/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp +++ b/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp @@ -1276,11 +1276,11 @@ void moveit_benchmarks::BenchmarkExecution::runGoalExistenceBenchmark(BenchmarkR // Compute IK ROS_INFO_STREAM("Processing goal " << req.motion_plan_request.goal_constraints[0].name << " ..."); ros::WallTime startTime = ros::WallTime::now(); - success = - robot_state.setFromIK(robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose, - req.motion_plan_request.num_planning_attempts, - req.motion_plan_request.allowed_planning_time, - boost::bind(&isIKSolutionCollisionFree, planning_scene_.get(), _1, _2, _3, &reachable)); + success = robot_state.setFromIK(robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose, + req.motion_plan_request.num_planning_attempts, + req.motion_plan_request.allowed_planning_time, + std::bind(&isIKSolutionCollisionFree, planning_scene_.get(), std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3, &reachable)); if (success) { ROS_INFO(" Success!"); @@ -1370,7 +1370,8 @@ void moveit_benchmarks::BenchmarkExecution::runGoalExistenceBenchmark(BenchmarkR robot_state.setFromIK(robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose, req.motion_plan_request.num_planning_attempts, req.motion_plan_request.allowed_planning_time, - boost::bind(&isIKSolutionCollisionFree, planning_scene_.get(), _1, _2, _3, &reachable)); + std::bind(&isIKSolutionCollisionFree, planning_scene_.get(), std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3, &reachable)); double duration = (ros::WallTime::now() - startTime).toSec(); if (success) diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp index 21268b5b99..91796b7215 100644 --- a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp +++ b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp @@ -54,14 +54,16 @@ void move_group::MoveGroupPickPlaceAction::initialize() // start the pickup action server pickup_action_server_ = std::make_unique>( - root_node_handle_, PICKUP_ACTION, boost::bind(&MoveGroupPickPlaceAction::executePickupCallback, this, _1), false); - pickup_action_server_->registerPreemptCallback(boost::bind(&MoveGroupPickPlaceAction::preemptPickupCallback, this)); + root_node_handle_, PICKUP_ACTION, + std::bind(&MoveGroupPickPlaceAction::executePickupCallback, this, std::placeholders::_1), false); + pickup_action_server_->registerPreemptCallback(std::bind(&MoveGroupPickPlaceAction::preemptPickupCallback, this)); pickup_action_server_->start(); // start the place action server place_action_server_ = std::make_unique>( - root_node_handle_, PLACE_ACTION, boost::bind(&MoveGroupPickPlaceAction::executePlaceCallback, this, _1), false); - place_action_server_->registerPreemptCallback(boost::bind(&MoveGroupPickPlaceAction::preemptPlaceCallback, this)); + root_node_handle_, PLACE_ACTION, + std::bind(&MoveGroupPickPlaceAction::executePlaceCallback, this, std::placeholders::_1), false); + place_action_server_->registerPreemptCallback(std::bind(&MoveGroupPickPlaceAction::preemptPlaceCallback, this)); place_action_server_->start(); } @@ -259,17 +261,18 @@ void move_group::MoveGroupPickPlaceAction::executePickupCallbackPlanAndExecute( opt.replan_ = goal->planning_options.replan; opt.replan_attempts_ = goal->planning_options.replan_attempts; opt.replan_delay_ = goal->planning_options.replan_delay; - opt.before_execution_callback_ = boost::bind(&MoveGroupPickPlaceAction::startPickupExecutionCallback, this); + opt.before_execution_callback_ = std::bind(&MoveGroupPickPlaceAction::startPickupExecutionCallback, this); - opt.plan_callback_ = - boost::bind(&MoveGroupPickPlaceAction::planUsingPickPlacePickup, this, boost::cref(*goal), &action_res, _1); + opt.plan_callback_ = std::bind(&MoveGroupPickPlaceAction::planUsingPickPlacePickup, this, boost::cref(*goal), + &action_res, std::placeholders::_1); if (goal->planning_options.look_around && context_->plan_with_sensing_) { - opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(), - _1, opt.plan_callback_, goal->planning_options.look_around_attempts, - goal->planning_options.max_safe_execution_cost); + opt.plan_callback_ = + std::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(), + std::placeholders::_1, opt.plan_callback_, goal->planning_options.look_around_attempts, + goal->planning_options.max_safe_execution_cost); context_->plan_with_sensing_->setBeforeLookCallback( - boost::bind(&MoveGroupPickPlaceAction::startPickupLookCallback, this)); + std::bind(&MoveGroupPickPlaceAction::startPickupLookCallback, this)); } plan_execution::ExecutableMotionPlan plan; @@ -290,16 +293,17 @@ void move_group::MoveGroupPickPlaceAction::executePlaceCallbackPlanAndExecute(co opt.replan_ = goal->planning_options.replan; opt.replan_attempts_ = goal->planning_options.replan_attempts; opt.replan_delay_ = goal->planning_options.replan_delay; - opt.before_execution_callback_ = boost::bind(&MoveGroupPickPlaceAction::startPlaceExecutionCallback, this); - opt.plan_callback_ = - boost::bind(&MoveGroupPickPlaceAction::planUsingPickPlacePlace, this, boost::cref(*goal), &action_res, _1); + opt.before_execution_callback_ = std::bind(&MoveGroupPickPlaceAction::startPlaceExecutionCallback, this); + opt.plan_callback_ = std::bind(&MoveGroupPickPlaceAction::planUsingPickPlacePlace, this, boost::cref(*goal), + &action_res, std::placeholders::_1); if (goal->planning_options.look_around && context_->plan_with_sensing_) { - opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(), - _1, opt.plan_callback_, goal->planning_options.look_around_attempts, - goal->planning_options.max_safe_execution_cost); + opt.plan_callback_ = + std::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(), + std::placeholders::_1, opt.plan_callback_, goal->planning_options.look_around_attempts, + goal->planning_options.max_safe_execution_cost); context_->plan_with_sensing_->setBeforeLookCallback( - boost::bind(&MoveGroupPickPlaceAction::startPlaceLookCallback, this)); + std::bind(&MoveGroupPickPlaceAction::startPlaceLookCallback, this)); } plan_execution::ExecutableMotionPlan plan; diff --git a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp index 4a667fd489..66b58f3119 100644 --- a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp +++ b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp @@ -181,7 +181,8 @@ void addGripperTrajectory(const ManipulationPlanPtr& plan, plan_execution::ExecutableTrajectory et(ee_closed_traj, name); // Add a callback to attach the object to the EE after closing the gripper - et.effect_on_success_ = boost::bind(&executeAttachObject, plan->shared_data_, plan->approach_posture_, _1); + et.effect_on_success_ = + std::bind(&executeAttachObject, plan->shared_data_, plan->approach_posture_, std::placeholders::_1); et.allowed_collision_matrix_ = collision_matrix; plan->trajectories_.push_back(et); } @@ -225,8 +226,8 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const // state validity checking during the approach must ensure that the gripper posture is that for pre-grasping moveit::core::GroupStateValidityCallbackFn approach_valid_callback = - boost::bind(&isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_, - &plan->approach_posture_, _1, _2, _3); + std::bind(&isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_, + &plan->approach_posture_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); plan->goal_sampler_->setVerbose(verbose_); std::size_t attempted_possible_goal_states = 0; do // continously sample possible goal states @@ -276,8 +277,8 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const // state validity checking during the retreat after the grasp must ensure the gripper posture is that of the // actual grasp moveit::core::GroupStateValidityCallbackFn retreat_valid_callback = - boost::bind(&isStateCollisionFree, planning_scene_after_approach.get(), collision_matrix_.get(), verbose_, - &plan->retreat_posture_, _1, _2, _3); + std::bind(&isStateCollisionFree, planning_scene_after_approach.get(), collision_matrix_.get(), verbose_, + &plan->retreat_posture_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); // try to compute a straight line path that moves from the goal in a desired direction moveit::core::RobotStatePtr last_retreat_state( diff --git a/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp b/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp index 29cab837f5..b8512d5ab6 100644 --- a/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp +++ b/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp @@ -114,7 +114,7 @@ void ManipulationPipeline::start() stage->resetStopSignal(); for (std::size_t i = 0; i < processing_threads_.size(); ++i) if (!processing_threads_[i]) - processing_threads_[i] = new boost::thread(boost::bind(&ManipulationPipeline::processingThread, this, i)); + processing_threads_[i] = new boost::thread(std::bind(&ManipulationPipeline::processingThread, this, i)); } void ManipulationPipeline::signalStop() diff --git a/moveit_ros/manipulation/pick_place/src/pick_place.cpp b/moveit_ros/manipulation/pick_place/src/pick_place.cpp index 6b0b2eca8e..21d4bb87e2 100644 --- a/moveit_ros/manipulation/pick_place/src/pick_place.cpp +++ b/moveit_ros/manipulation/pick_place/src/pick_place.cpp @@ -51,8 +51,8 @@ const double PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION = 7.0; // sec PickPlacePlanBase::PickPlacePlanBase(const PickPlaceConstPtr& pick_place, const std::string& name) : pick_place_(pick_place), pipeline_(name, 4), last_plan_time_(0.0), done_(false) { - pipeline_.setSolutionCallback(boost::bind(&PickPlacePlanBase::foundSolution, this)); - pipeline_.setEmptyQueueCallback(boost::bind(&PickPlacePlanBase::emptyQueue, this)); + pipeline_.setSolutionCallback(std::bind(&PickPlacePlanBase::foundSolution, this)); + pipeline_.setEmptyQueueCallback(std::bind(&PickPlacePlanBase::emptyQueue, this)); } PickPlacePlanBase::~PickPlacePlanBase() = default; diff --git a/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp b/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp index addc270d44..7102d23715 100644 --- a/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp +++ b/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp @@ -49,8 +49,8 @@ class DynamicReconfigureImpl public: DynamicReconfigureImpl() : dynamic_reconfigure_server_(ros::NodeHandle("~/pick_place")) { - dynamic_reconfigure_server_.setCallback( - boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2)); + dynamic_reconfigure_server_.setCallback(std::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, + std::placeholders::_1, std::placeholders::_2)); } const PickPlaceParams& getParams() const diff --git a/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp b/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp index 986c05602e..e30e58543c 100644 --- a/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp +++ b/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp @@ -37,7 +37,7 @@ #include #include #include -#include +#include #include pick_place::ReachableAndValidPoseFilter::ReachableAndValidPoseFilter( @@ -132,8 +132,9 @@ bool pick_place::ReachableAndValidPoseFilter::evaluate(const ManipulationPlanPtr constraints_sampler_manager_->selectSampler(planning_scene_, planning_group, plan->goal_constraints_); if (plan->goal_sampler_) { - plan->goal_sampler_->setGroupStateValidityCallback(boost::bind( - &isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_, plan.get(), _1, _2, _3)); + plan->goal_sampler_->setGroupStateValidityCallback( + std::bind(&isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_, plan.get(), + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); plan->goal_sampler_->setVerbose(verbose_); if (plan->goal_sampler_->sample(*token_state, plan->shared_data_->max_goal_sampling_attempts_)) { diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index b281d8493d..ad8f373907 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -136,10 +136,11 @@ bool MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath ls = std::make_unique(context_->planning_scene_monitor_); kset = std::make_unique((*ls)->getRobotModel()); kset->add(req.path_constraints, (*ls)->getTransforms()); - constraint_fn = boost::bind( + constraint_fn = std::bind( &isStateValid, req.avoid_collisions ? static_cast(*ls).get() : nullptr, - kset->empty() ? nullptr : kset.get(), _1, _2, _3); + kset->empty() ? nullptr : kset.get(), std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3); } bool global_frame = !moveit::core::Transforms::sameFrame(link_name, req.header.frame_id); ROS_INFO_NAMED(getName(), diff --git a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp index 1003e8d0fe..cf7e900dbe 100644 --- a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp @@ -52,9 +52,9 @@ void MoveGroupExecuteTrajectoryAction::initialize() // start the move action server execute_action_server_ = std::make_unique>( root_node_handle_, EXECUTE_ACTION_NAME, - boost::bind(&MoveGroupExecuteTrajectoryAction::executePathCallback, this, _1), false); + std::bind(&MoveGroupExecuteTrajectoryAction::executePathCallback, this, std::placeholders::_1), false); execute_action_server_->registerPreemptCallback( - boost::bind(&MoveGroupExecuteTrajectoryAction::preemptExecuteTrajectoryCallback, this)); + std::bind(&MoveGroupExecuteTrajectoryAction::preemptExecuteTrajectoryCallback, this)); execute_action_server_->start(); } diff --git a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_service_capability.cpp index 9e61129856..6da4e85e9e 100644 --- a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_service_capability.cpp @@ -60,7 +60,8 @@ void MoveGroupExecuteService::initialize() // Hence, we use our own asynchronous spinner listening to our own callback queue. ros::AdvertiseServiceOptions ops; ops.template init( - EXECUTE_SERVICE_NAME, boost::bind(&MoveGroupExecuteService::executeTrajectoryService, this, _1, _2)); + EXECUTE_SERVICE_NAME, std::bind(&MoveGroupExecuteService::executeTrajectoryService, this, std::placeholders::_1, + std::placeholders::_2)); ops.callback_queue = &callback_queue_; execute_service_ = root_node_handle_.advertiseService(ops); spinner_.start(); diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp index 78b03d2c31..279fc8aaa1 100644 --- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp @@ -158,11 +158,12 @@ bool MoveGroupKinematicsService::computeIKService(moveit_msgs::GetPositionIK::Re moveit::core::RobotState rs = ls->getCurrentState(); kset.add(req.ik_request.constraints, ls->getTransforms()); computeIK(req.ik_request, res.solution, res.error_code, rs, - boost::bind(&isIKSolutionValid, - req.ik_request.avoid_collisions ? - static_cast(ls).get() : - nullptr, - kset.empty() ? nullptr : &kset, _1, _2, _3)); + std::bind(&isIKSolutionValid, + req.ik_request.avoid_collisions ? + static_cast(ls).get() : + nullptr, + kset.empty() ? nullptr : &kset, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); } else { diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp index a6793cf255..4c353cd40d 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp @@ -55,8 +55,9 @@ void MoveGroupMoveAction::initialize() { // start the move action server move_action_server_ = std::make_unique>( - root_node_handle_, MOVE_ACTION, boost::bind(&MoveGroupMoveAction::executeMoveCallback, this, _1), false); - move_action_server_->registerPreemptCallback(boost::bind(&MoveGroupMoveAction::preemptMoveCallback, this)); + root_node_handle_, MOVE_ACTION, std::bind(&MoveGroupMoveAction::executeMoveCallback, this, std::placeholders::_1), + false); + move_action_server_->registerPreemptCallback(std::bind(&MoveGroupMoveAction::preemptMoveCallback, this)); move_action_server_->start(); } @@ -132,16 +133,17 @@ void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const moveit_msgs::M opt.replan_ = goal->planning_options.replan; opt.replan_attempts_ = goal->planning_options.replan_attempts; opt.replan_delay_ = goal->planning_options.replan_delay; - opt.before_execution_callback_ = boost::bind(&MoveGroupMoveAction::startMoveExecutionCallback, this); + opt.before_execution_callback_ = std::bind(&MoveGroupMoveAction::startMoveExecutionCallback, this); - opt.plan_callback_ = - boost::bind(&MoveGroupMoveAction::planUsingPlanningPipeline, this, boost::cref(motion_plan_request), _1); + opt.plan_callback_ = std::bind(&MoveGroupMoveAction::planUsingPlanningPipeline, this, + boost::cref(motion_plan_request), std::placeholders::_1); if (goal->planning_options.look_around && context_->plan_with_sensing_) { - opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(), - _1, opt.plan_callback_, goal->planning_options.look_around_attempts, - goal->planning_options.max_safe_execution_cost); - context_->plan_with_sensing_->setBeforeLookCallback(boost::bind(&MoveGroupMoveAction::startMoveLookCallback, this)); + opt.plan_callback_ = + std::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(), + std::placeholders::_1, opt.plan_callback_, goal->planning_options.look_around_attempts, + goal->planning_options.max_safe_execution_cost); + context_->plan_with_sensing_->setBeforeLookCallback(std::bind(&MoveGroupMoveAction::startMoveLookCallback, this)); } plan_execution::ExecutableMotionPlan plan; diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp index ed494f9f2d..390ab5db71 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp @@ -200,14 +200,17 @@ void OccupancyMapMonitor::addUpdater(const OccupancyMapUpdaterPtr& updater) // when we had one updater only, we passed direcly the transform cache callback to that updater if (map_updaters_.size() == 2) { - map_updaters_[0]->setTransformCacheCallback( - boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 0, _1, _2, _3)); - map_updaters_[1]->setTransformCacheCallback( - boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 1, _1, _2, _3)); + map_updaters_[0]->setTransformCacheCallback(std::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 0, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); + map_updaters_[1]->setTransformCacheCallback(std::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 1, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); } else - map_updaters_.back()->setTransformCacheCallback( - boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, map_updaters_.size() - 1, _1, _2, _3)); + map_updaters_.back()->setTransformCacheCallback(std::bind(&OccupancyMapMonitor::getShapeTransformCache, this, + map_updaters_.size() - 1, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); } else updater->setTransformCacheCallback(transform_cache_callback_); diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp index b91b591d07..655d7916e3 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp @@ -72,7 +72,7 @@ int main(int argc, char** argv) std::shared_ptr buffer = std::make_shared(ros::Duration(5.0)); std::shared_ptr listener = std::make_shared(*buffer, nh); occupancy_map_monitor::OccupancyMapMonitor server(buffer); - server.setUpdateCallback(boost::bind(&publishOctomap, &octree_binary_pub, &server)); + server.setUpdateCallback(std::bind(&publishOctomap, &octree_binary_pub, &server)); server.startMonitor(); ros::spin(); diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp index c391989f90..3b5b907987 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp +++ b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp @@ -129,7 +129,8 @@ bool DepthImageOctomapUpdater::initialize() mesh_filter_->setShadowThreshold(shadow_threshold_); mesh_filter_->setPaddingOffset(padding_offset_); mesh_filter_->setPaddingScale(padding_scale_); - mesh_filter_->setTransformCallback(boost::bind(&DepthImageOctomapUpdater::getShapeTransform, this, _1, _2)); + mesh_filter_->setTransformCallback( + std::bind(&DepthImageOctomapUpdater::getShapeTransform, this, std::placeholders::_1, std::placeholders::_2)); return true; } diff --git a/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp b/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp index 7040e5fdeb..34e7e56923 100644 --- a/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp +++ b/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp @@ -48,8 +48,8 @@ LazyFreeSpaceUpdater::LazyFreeSpaceUpdater(const collision_detection::OccMapTree , max_sensor_delta_(1e-3) // 1mm , process_occupied_cells_set_(nullptr) , process_model_cells_set_(nullptr) - , update_thread_(boost::bind(&LazyFreeSpaceUpdater::lazyUpdateThread, this)) - , process_thread_(boost::bind(&LazyFreeSpaceUpdater::processThread, this)) + , update_thread_(std::bind(&LazyFreeSpaceUpdater::lazyUpdateThread, this)) + , process_thread_(std::bind(&LazyFreeSpaceUpdater::processThread, this)) { } diff --git a/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp b/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp index acad8248c5..5bbe882391 100644 --- a/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp +++ b/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp @@ -91,7 +91,7 @@ void mesh_filter::DepthSelfFiltering::onInit() model_label_ptr_ = std::make_shared(); mesh_filter_ = std::make_shared>( - bind(&TransformProvider::getTransform, &transform_provider_, _1, _2), + bind(&TransformProvider::getTransform, &transform_provider_, std::placeholders::_1, std::placeholders::_2), mesh_filter::StereoCameraModel::REGISTERED_PSDK_PARAMS); mesh_filter_->parameters().setDepthRange(near_clipping_plane_distance_, far_clipping_plane_distance_); mesh_filter_->setShadowThreshold(shadow_threshold_); diff --git a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp index 7ba637ba4f..c6ba027510 100644 --- a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp +++ b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp @@ -118,7 +118,8 @@ MeshFilterTest::MeshFilterTest(unsigned width, unsigned height, double nea , shadow_(shadow) , epsilon_(epsilon) , sensor_parameters_(width, height, near_, far_, width >> 1, height >> 1, width >> 1, height >> 1, 0.1, 0.1) - , filter_(std::bind(&MeshFilterTest::transformCallback, this, _1, _2), sensor_parameters_) + , filter_(std::bind(&MeshFilterTest::transformCallback, this, std::placeholders::_1, std::placeholders::_2), + sensor_parameters_) , sensor_data_(width_ * height_) , distance_(0.0) { diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp index fb37bbf1a0..0295ace054 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp +++ b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp @@ -99,7 +99,8 @@ bool PointCloudOctomapUpdater::initialize() tf_buffer_ = std::make_shared(); tf_listener_ = std::make_shared(*tf_buffer_, root_nh_); shape_mask_ = std::make_unique(); - shape_mask_->setTransformCallback(boost::bind(&PointCloudOctomapUpdater::getShapeTransform, this, _1, _2)); + shape_mask_->setTransformCallback( + std::bind(&PointCloudOctomapUpdater::getShapeTransform, this, std::placeholders::_1, std::placeholders::_2)); std::string prefix = ""; if (!ns_.empty()) @@ -120,13 +121,15 @@ void PointCloudOctomapUpdater::start() { point_cloud_filter_ = new tf2_ros::MessageFilter(*point_cloud_subscriber_, *tf_buffer_, monitor_->getMapFrame(), 5, root_nh_); - point_cloud_filter_->registerCallback(boost::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, _1)); + point_cloud_filter_->registerCallback( + std::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, std::placeholders::_1)); ROS_INFO_NAMED(LOGNAME, "Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(), point_cloud_filter_->getTargetFramesString().c_str()); } else { - point_cloud_subscriber_->registerCallback(boost::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, _1)); + point_cloud_subscriber_->registerCallback( + std::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, std::placeholders::_1)); ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", point_cloud_topic_.c_str()); } } diff --git a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp index eb01773539..2ec58d243a 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp +++ b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp @@ -258,7 +258,7 @@ moveit::core::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction() moveit::tools::Profiler::ScopedBlock prof_block("KinematicsPluginLoader::getLoaderFunction"); if (loader_) - return boost::bind(&KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), _1); + return std::bind(&KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), std::placeholders::_1); rdf_loader::RDFLoader rml(robot_description_); robot_description_ = rml.getRobotDescription(); @@ -447,6 +447,7 @@ moveit::core::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction(const iksolver_to_tip_links); } - return boost::bind(&KinematicsPluginLoader::KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), _1); + return std::bind(&KinematicsPluginLoader::KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), + std::placeholders::_1); } } // namespace kinematics_plugin_loader diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index 577c5a02ba..aff65e3a98 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -54,8 +54,8 @@ class PlanExecution::DynamicReconfigureImpl DynamicReconfigureImpl(PlanExecution* owner) : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle("~/plan_execution")) { - dynamic_reconfigure_server_.setCallback( - boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2)); + dynamic_reconfigure_server_.setCallback(std::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, + std::placeholders::_1, std::placeholders::_2)); } private: @@ -86,7 +86,8 @@ plan_execution::PlanExecution::PlanExecution( new_scene_update_ = false; // we want to be notified when new information is available - planning_scene_monitor_->addUpdateCallback(boost::bind(&PlanExecution::planningSceneUpdatedCallback, this, _1)); + planning_scene_monitor_->addUpdateCallback( + std::bind(&PlanExecution::planningSceneUpdatedCallback, this, std::placeholders::_1)); // start the dynamic-reconfigure server reconfigure_impl_ = new DynamicReconfigureImpl(this); @@ -404,9 +405,9 @@ moveit_msgs::MoveItErrorCodes plan_execution::PlanExecution::executeAndMonitor(E trajectory_monitor_->startTrajectoryMonitor(); // start a trajectory execution thread - trajectory_execution_manager_->execute(boost::bind(&PlanExecution::doneWithTrajectoryExecution, this, _1), - boost::bind(&PlanExecution::successfulTrajectorySegmentExecution, this, &plan, - _1)); + trajectory_execution_manager_->execute( + std::bind(&PlanExecution::doneWithTrajectoryExecution, this, std::placeholders::_1), + std::bind(&PlanExecution::successfulTrajectorySegmentExecution, this, &plan, std::placeholders::_1)); // wait for path to be done, while checking that the path does not become invalid ros::Rate r(100); path_became_invalid_ = false; diff --git a/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp b/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp index 9eb12255de..3b1cca9e7d 100644 --- a/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp @@ -52,8 +52,8 @@ class PlanWithSensing::DynamicReconfigureImpl DynamicReconfigureImpl(PlanWithSensing* owner) : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle("~/sense_for_plan")) { - dynamic_reconfigure_server_.setCallback( - boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2)); + dynamic_reconfigure_server_.setCallback(std::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, + std::placeholders::_1, std::placeholders::_2)); } private: diff --git a/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp b/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp index b8530d349e..fae2970a20 100644 --- a/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp +++ b/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp @@ -118,7 +118,7 @@ int main(int argc, char** argv) for (unsigned int i = 0; i < states.size(); ++i) threads.push_back(new boost::thread( - boost::bind(&runCollisionDetection, i, trials, psm.getPlanningScene().get(), states[i].get()))); + std::bind(&runCollisionDetection, i, trials, psm.getPlanningScene().get(), states[i].get()))); for (unsigned int i = 0; i < states.size(); ++i) { diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp index 415906fef2..125dc1b983 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp @@ -148,7 +148,7 @@ void CurrentStateMonitor::startStateMonitor(const std::string& joint_states_topi if (tf_buffer_ && !robot_model_->getMultiDOFJointModels().empty()) { tf_connection_ = std::make_shared( - tf_buffer_->_addTransformsChangedListener(boost::bind(&CurrentStateMonitor::tfCallback, this))); + tf_buffer_->_addTransformsChangedListener(std::bind(&CurrentStateMonitor::tfCallback, this))); } state_monitor_started_ = true; monitor_start_time_ = ros::Time::now(); diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index f1edc7be71..773da3da8a 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -62,8 +62,8 @@ class PlanningSceneMonitor::DynamicReconfigureImpl DynamicReconfigureImpl(PlanningSceneMonitor* owner) : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle(decideNamespace(owner->getName()))) { - dynamic_reconfigure_server_.setCallback( - boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2)); + dynamic_reconfigure_server_.setCallback(std::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, + std::placeholders::_1, std::placeholders::_2)); } private: @@ -223,10 +223,10 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc { // The scene_ is loaded on the collision loader only if it was correctly instantiated collision_loader_.setupScene(nh_, scene_); - scene_->setAttachedBodyUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2)); - scene_->setCollisionObjectUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2)); + scene_->setAttachedBodyUpdateCallback(std::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, + this, std::placeholders::_1, std::placeholders::_2)); + scene_->setCollisionObjectUpdateCallback(std::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, + std::placeholders::_1, std::placeholders::_2)); } } else @@ -271,10 +271,10 @@ void PlanningSceneMonitor::monitorDiffs(bool flag) parent_scene_ = scene_; scene_ = parent_scene_->diff(); scene_const_ = scene_; - scene_->setAttachedBodyUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2)); - scene_->setCollisionObjectUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2)); + scene_->setAttachedBodyUpdateCallback(std::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, + this, std::placeholders::_1, std::placeholders::_2)); + scene_->setCollisionObjectUpdateCallback(std::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, + this, std::placeholders::_1, std::placeholders::_2)); } } else @@ -327,7 +327,7 @@ void PlanningSceneMonitor::startPublishingPlanningScene(SceneUpdateType update_t ROS_INFO_NAMED(LOGNAME, "Publishing maintained planning scene on '%s'", planning_scene_topic.c_str()); monitorDiffs(true); publish_planning_scene_ = - std::make_unique(boost::bind(&PlanningSceneMonitor::scenePublishingThread, this)); + std::make_unique(std::bind(&PlanningSceneMonitor::scenePublishingThread, this)); } } @@ -385,10 +385,10 @@ void PlanningSceneMonitor::scenePublishingThread() scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn()); scene_->pushDiffs(parent_scene_); scene_->clearDiffs(); - scene_->setAttachedBodyUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2)); - scene_->setCollisionObjectUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2)); + scene_->setAttachedBodyUpdateCallback(std::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, + this, std::placeholders::_1, std::placeholders::_2)); + scene_->setCollisionObjectUpdateCallback(std::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, + this, std::placeholders::_1, std::placeholders::_2)); if (octomap_monitor_) { excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in @@ -593,10 +593,10 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningSc parent_scene_ = scene_; scene_ = parent_scene_->diff(); scene_const_ = scene_; - scene_->setAttachedBodyUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2)); - scene_->setCollisionObjectUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2)); + scene_->setAttachedBodyUpdateCallback(std::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, + this, std::placeholders::_1, std::placeholders::_2)); + scene_->setCollisionObjectUpdateCallback(std::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, + std::placeholders::_1, std::placeholders::_2)); } if (octomap_monitor_) { @@ -1097,9 +1097,10 @@ void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collisio excludeAttachedBodiesFromOctree(); excludeWorldObjectsFromOctree(); - octomap_monitor_->setTransformCacheCallback( - boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, _1, _2, _3)); - octomap_monitor_->setUpdateCallback(boost::bind(&PlanningSceneMonitor::octomapUpdateCallback, this)); + octomap_monitor_->setTransformCacheCallback(std::bind(&PlanningSceneMonitor::getShapeTransformCache, this, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); + octomap_monitor_->setUpdateCallback(std::bind(&PlanningSceneMonitor::octomapUpdateCallback, this)); } octomap_monitor_->startMonitor(); } @@ -1129,7 +1130,8 @@ void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_top { if (!current_state_monitor_) current_state_monitor_ = std::make_shared(getRobotModel(), tf_buffer_, root_nh_); - current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, _1)); + current_state_monitor_->addUpdateCallback( + std::bind(&PlanningSceneMonitor::onStateUpdate, this, std::placeholders::_1)); current_state_monitor_->startStateMonitor(joint_states_topic); { diff --git a/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp index a6cea18d91..9449d1a573 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp @@ -77,7 +77,7 @@ void planning_scene_monitor::TrajectoryMonitor::startTrajectoryMonitor() { if (sampling_frequency_ > std::numeric_limits::epsilon() && !record_states_thread_) { - record_states_thread_ = std::make_unique(boost::bind(&TrajectoryMonitor::recordStates, this)); + record_states_thread_ = std::make_unique(std::bind(&TrajectoryMonitor::recordStates, this)); ROS_DEBUG_NAMED(LOGNAME, "Started trajectory monitor"); } } diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index e5ef47bb5a..abe969aa11 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -60,8 +60,8 @@ class TrajectoryExecutionManager::DynamicReconfigureImpl DynamicReconfigureImpl(TrajectoryExecutionManager* owner) : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle("~/trajectory_execution")) { - dynamic_reconfigure_server_.setCallback( - boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2)); + dynamic_reconfigure_server_.setCallback(std::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, + std::placeholders::_1, std::placeholders::_2)); } private: @@ -360,7 +360,7 @@ bool TrajectoryExecutionManager::pushAndExecute(const moveit_msgs::RobotTrajecto continuous_execution_queue_.push_back(context); if (!continuous_execution_thread_) continuous_execution_thread_ = - std::make_unique(boost::bind(&TrajectoryExecutionManager::continuousExecutionThread, this)); + std::make_unique(std::bind(&TrajectoryExecutionManager::continuousExecutionThread, this)); } last_execution_status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED; continuous_execution_condition_.notify_all(); 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 c1ceab384b..81db35e044 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 @@ -1247,7 +1247,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (constraints_init_thread_) constraints_init_thread_->join(); constraints_init_thread_ = std::make_unique( - boost::bind(&MoveGroupInterfaceImpl::initializeConstraintsStorageThread, this, host, port)); + std::bind(&MoveGroupInterfaceImpl::initializeConstraintsStorageThread, this, host, port)); } void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz) diff --git a/moveit_ros/robot_interaction/src/interaction_handler.cpp b/moveit_ros/robot_interaction/src/interaction_handler.cpp index e26ccdc182..18868cb7c6 100644 --- a/moveit_ros/robot_interaction/src/interaction_handler.cpp +++ b/moveit_ros/robot_interaction/src/interaction_handler.cpp @@ -207,7 +207,7 @@ void InteractionHandler::handleGeneric(const GenericInteraction& g, StateChangeCallbackFn callback; // modify the RobotState in-place with the state_lock_ held. LockedRobotState::modifyState( - boost::bind(&InteractionHandler::updateStateGeneric, this, _1, &g, &feedback, &callback)); + std::bind(&InteractionHandler::updateStateGeneric, this, std::placeholders::_1, &g, &feedback, &callback)); // This calls update_callback_ to notify client that state changed. if (callback) @@ -238,8 +238,8 @@ void InteractionHandler::handleEndEffector(const EndEffectorInteraction& eef, // modify the RobotState in-place with state_lock_ held. // This locks state_lock_ before calling updateState() - LockedRobotState::modifyState( - boost::bind(&InteractionHandler::updateStateEndEffector, this, _1, &eef, &tpose.pose, &callback)); + LockedRobotState::modifyState(std::bind(&InteractionHandler::updateStateEndEffector, this, std::placeholders::_1, + &eef, &tpose.pose, &callback)); // This calls update_callback_ to notify client that state changed. if (callback) @@ -270,7 +270,7 @@ void InteractionHandler::handleJoint(const JointInteraction& vj, // modify the RobotState in-place with state_lock_ held. // This locks state_lock_ before calling updateState() LockedRobotState::modifyState( - boost::bind(&InteractionHandler::updateStateJoint, this, _1, &vj, &tpose.pose, &callback)); + std::bind(&InteractionHandler::updateStateJoint, this, std::placeholders::_1, &vj, &tpose.pose, &callback)); // This calls update_callback_ to notify client that state changed. if (callback) @@ -285,7 +285,7 @@ void InteractionHandler::updateStateGeneric(moveit::core::RobotState* state, con bool ok = g->process_feedback(*state, *feedback); bool error_state_changed = setErrorState(g->marker_name_suffix, !ok); if (update_callback_) - *callback = boost::bind(update_callback_, _1, error_state_changed); + *callback = std::bind(update_callback_, std::placeholders::_1, error_state_changed); } // MUST hold state_lock_ when calling this! @@ -299,7 +299,7 @@ void InteractionHandler::updateStateEndEffector(moveit::core::RobotState* state, bool ok = kinematic_options.setStateFromIK(*state, eef->parent_group, eef->parent_link, *pose); bool error_state_changed = setErrorState(eef->parent_group, !ok); if (update_callback_) - *callback = boost::bind(update_callback_, _1, error_state_changed); + *callback = std::bind(update_callback_, std::placeholders::_1, error_state_changed); } // MUST hold state_lock_ when calling this! @@ -316,7 +316,7 @@ void InteractionHandler::updateStateJoint(moveit::core::RobotState* state, const state->update(); if (update_callback_) - *callback = boost::bind(update_callback_, _1, false); + *callback = std::bind(update_callback_, std::placeholders::_1, false); } bool InteractionHandler::inError(const EndEffectorInteraction& eef) const diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index 2e555b5aec..d4128f0dc2 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -70,7 +70,7 @@ RobotInteraction::RobotInteraction(const moveit::core::RobotModelConstPtr& robot // spin a thread that will process feedback events run_processing_thread_ = true; - processing_thread_ = std::make_unique(boost::bind(&RobotInteraction::processingThread, this)); + processing_thread_ = std::make_unique(std::bind(&RobotInteraction::processingThread, this)); } RobotInteraction::~RobotInteraction() @@ -539,7 +539,8 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle for (const visualization_msgs::InteractiveMarker& im : ims) { int_marker_server_->insert(im); - int_marker_server_->setCallback(im.name, boost::bind(&RobotInteraction::processInteractiveMarkerFeedback, this, _1)); + int_marker_server_->setCallback(im.name, std::bind(&RobotInteraction::processInteractiveMarkerFeedback, this, + std::placeholders::_1)); // Add menu handler to all markers that this interaction handler creates. if (std::shared_ptr mh = handler->getMenuHandler()) @@ -569,7 +570,8 @@ void RobotInteraction::toggleMoveInteractiveMarkerTopic(bool enable) std::string topic_name = int_marker_move_topics_[i]; std::string marker_name = int_marker_names_[i]; int_marker_move_subscribers_.push_back(nh.subscribe( - topic_name, 1, boost::bind(&RobotInteraction::moveInteractiveMarker, this, marker_name, _1))); + topic_name, 1, + std::bind(&RobotInteraction::moveInteractiveMarker, this, marker_name, std::placeholders::_1))); } } } diff --git a/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp b/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp index 8ff126b8c4..3e5b155768 100644 --- a/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp +++ b/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp @@ -433,7 +433,7 @@ void MyInfo::modifyThreadFunc(robot_interaction::LockedRobotState* locked_state, { val += 0.0001; - locked_state->modifyState(boost::bind(&MyInfo::modifyFunc, this, _1, val)); + locked_state->modifyState(std::bind(&MyInfo::modifyFunc, this, std::placeholders::_1, val)); } cnt_lock_.lock(); 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 b68d565d50..aa343aead9 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 @@ -174,7 +174,8 @@ MotionPlanningDisplay::MotionPlanningDisplay() trajectory_visual_ = std::make_shared(path_category_, this); // Start background jobs - background_process_.setJobUpdateEvent(boost::bind(&MotionPlanningDisplay::backgroundJobUpdate, this, _1, _2)); + background_process_.setJobUpdateEvent( + std::bind(&MotionPlanningDisplay::backgroundJobUpdate, this, std::placeholders::_1, std::placeholders::_2)); } // ****************************************************************************************** @@ -285,7 +286,7 @@ void MotionPlanningDisplay::toggleSelectPlanningGroupSubscription(bool enable) void MotionPlanningDisplay::selectPlanningGroupCallback(const std_msgs::StringConstPtr& msg) { // synchronize ROS callback with main loop - addMainLoopJob(boost::bind(&MotionPlanningDisplay::changePlanningGroup, this, msg->data)); + addMainLoopJob(std::bind(&MotionPlanningDisplay::changePlanningGroup, this, msg->data)); } void MotionPlanningDisplay::reset() @@ -324,7 +325,7 @@ void MotionPlanningDisplay::setName(const QString& name) void MotionPlanningDisplay::backgroundJobUpdate(moveit::tools::BackgroundProcessing::JobEvent /*unused*/, const std::string& /*unused*/) { - addMainLoopJob(boost::bind(&MotionPlanningDisplay::updateBackgroundJobProgressBar, this)); + addMainLoopJob(std::bind(&MotionPlanningDisplay::updateBackgroundJobProgressBar, this)); } void MotionPlanningDisplay::updateBackgroundJobProgressBar() @@ -722,7 +723,7 @@ void MotionPlanningDisplay::changedQueryStartState() setStatusTextColor(query_start_color_property_->getColor()); addStatusText("Changed start state"); drawQueryStartState(); - addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, true), + addBackgroundJob(std::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, true), "publishInteractiveMarkers"); } @@ -733,7 +734,7 @@ void MotionPlanningDisplay::changedQueryGoalState() setStatusTextColor(query_goal_color_property_->getColor()); addStatusText("Changed goal state"); drawQueryGoalState(); - addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, true), + addBackgroundJob(std::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, true), "publishInteractiveMarkers"); } @@ -806,7 +807,7 @@ void MotionPlanningDisplay::resetInteractiveMarkers() { query_start_state_->clearError(); query_goal_state_->clearError(); - addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, false), + addBackgroundJob(std::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, false), "publishInteractiveMarkers"); } @@ -912,7 +913,7 @@ void MotionPlanningDisplay::scheduleDrawQueryStartState(robot_interaction::Inter { if (!planning_scene_monitor_) return; - addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, !error_state_changed), + addBackgroundJob(std::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, !error_state_changed), "publishInteractiveMarkers"); updateQueryStartState(); } @@ -922,7 +923,7 @@ void MotionPlanningDisplay::scheduleDrawQueryGoalState(robot_interaction::Intera { if (!planning_scene_monitor_) return; - addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, !error_state_changed), + addBackgroundJob(std::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, !error_state_changed), "publishInteractiveMarkers"); updateQueryGoalState(); } @@ -931,7 +932,7 @@ void MotionPlanningDisplay::updateQueryStartState() { queryStartStateChanged(); recomputeQueryStartStateMetrics(); - addMainLoopJob(boost::bind(&MotionPlanningDisplay::changedQueryStartState, this)); + addMainLoopJob(std::bind(&MotionPlanningDisplay::changedQueryStartState, this)); context_->queueRender(); } @@ -939,7 +940,7 @@ void MotionPlanningDisplay::updateQueryGoalState() { queryGoalStateChanged(); recomputeQueryGoalStateMetrics(); - addMainLoopJob(boost::bind(&MotionPlanningDisplay::changedQueryGoalState, this)); + addMainLoopJob(std::bind(&MotionPlanningDisplay::changedQueryGoalState, this)); context_->queueRender(); } @@ -1049,7 +1050,7 @@ void MotionPlanningDisplay::changedPlanningGroup() if (frame_) frame_->changePlanningGroup(); - addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, false), + addBackgroundJob(std::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, false), "publishInteractiveMarkers"); } @@ -1119,7 +1120,7 @@ void MotionPlanningDisplay::populateMenuHandler(std::shared_ptrinsert(menu_states, state_name, - boost::bind(&MotionPlanningDisplay::setQueryStateHelper, this, is_start, state_name)); + std::bind(&MotionPlanningDisplay::setQueryStateHelper, this, is_start, state_name)); } // // Group commands, which end up being the same for both interaction handlers @@ -1127,7 +1128,7 @@ void MotionPlanningDisplay::populateMenuHandler(std::shared_ptrinsert("Planning Group", immh::FeedbackCallback()); // for (int i = 0; i < group_names.size(); ++i) // mh->insert(menu_groups, group_names[i], - // boost::bind(&MotionPlanningDisplay::changePlanningGroup, this, group_names[i])); + // std::bind(&MotionPlanningDisplay::changePlanningGroup, this, group_names[i])); } void MotionPlanningDisplay::onRobotModelLoaded() @@ -1138,7 +1139,8 @@ void MotionPlanningDisplay::onRobotModelLoaded() robot_interaction_ = std::make_shared(getRobotModel(), "rviz_moveit_motion_planning_display"); robot_interaction::KinematicOptions o; - o.state_validity_callback_ = boost::bind(&MotionPlanningDisplay::isIKSolutionCollisionFree, this, _1, _2, _3); + o.state_validity_callback_ = std::bind(&MotionPlanningDisplay::isIKSolutionCollisionFree, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3); robot_interaction_->getKinematicOptionsMap()->setOptions( robot_interaction::KinematicOptionsMap::ALL, o, robot_interaction::KinematicOptions::STATE_VALIDITY_CALLBACK); @@ -1153,8 +1155,10 @@ void MotionPlanningDisplay::onRobotModelLoaded() robot_interaction_, "start", *previous_state_, planning_scene_monitor_->getTFClient()); query_goal_state_ = std::make_shared( robot_interaction_, "goal", *previous_state_, planning_scene_monitor_->getTFClient()); - query_start_state_->setUpdateCallback(boost::bind(&MotionPlanningDisplay::scheduleDrawQueryStartState, this, _1, _2)); - query_goal_state_->setUpdateCallback(boost::bind(&MotionPlanningDisplay::scheduleDrawQueryGoalState, this, _1, _2)); + query_start_state_->setUpdateCallback(std::bind(&MotionPlanningDisplay::scheduleDrawQueryStartState, this, + std::placeholders::_1, std::placeholders::_2)); + query_goal_state_->setUpdateCallback(std::bind(&MotionPlanningDisplay::scheduleDrawQueryGoalState, this, + std::placeholders::_1, std::placeholders::_2)); // Interactive marker menus populateMenuHandler(menu_handler_start_); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 8c627d9345..5775ea432f 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -34,6 +34,8 @@ /* Author: Ioan Sucan */ +#include + #include #include #include @@ -211,7 +213,7 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: semantic_world_.reset(); if (semantic_world_) { - semantic_world_->addTableCallback(boost::bind(&MotionPlanningFrame::updateTables, this)); + semantic_world_->addTableCallback(std::bind(&MotionPlanningFrame::updateTables, this)); } } catch (std::exception& ex) @@ -334,14 +336,13 @@ void MotionPlanningFrame::changePlanningGroupHelper() if (!planning_display_->getPlanningSceneMonitor()) return; - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::fillStateSelectionOptions, this)); - planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::populateConstraintsList, this, std::vector())); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::fillStateSelectionOptions, this)); + planning_display_->addMainLoopJob([this]() { populateConstraintsList(std::vector()); }); const moveit::core::RobotModelConstPtr& robot_model = planning_display_->getRobotModel(); std::string group = planning_display_->getCurrentPlanningGroup(); planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningParamWidget::setGroupName, ui_->planner_param_treeview, group)); + std::bind(&MotionPlanningParamWidget::setGroupName, ui_->planner_param_treeview, group)); planning_display_->addMainLoopJob( [=]() { ui_->planning_group_combo_box->setCurrentText(QString::fromStdString(group)); }); @@ -373,7 +374,7 @@ void MotionPlanningFrame::changePlanningGroupHelper() ROS_ERROR("%s", ex.what()); } planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningParamWidget::setMoveGroup, ui_->planner_param_treeview, move_group_)); + std::bind(&MotionPlanningParamWidget::setMoveGroup, ui_->planner_param_treeview, move_group_)); if (move_group_) { move_group_->allowLooking(ui_->allow_looking->isChecked()); @@ -382,9 +383,8 @@ void MotionPlanningFrame::changePlanningGroupHelper() planning_display_->addMainLoopJob([=]() { ui_->use_cartesian_path->setEnabled(has_unique_endeffector); }); std::vector desc; if (move_group_->getInterfaceDescriptions(desc)) - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlannersList, this, desc)); - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::populateConstraintsList, this), - "populateConstraintsList"); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populatePlannersList, this, desc)); + planning_display_->addBackgroundJob([this]() { populateConstraintsList(); }, "populateConstraintsList"); if (first_time_) { @@ -399,7 +399,7 @@ void MotionPlanningFrame::changePlanningGroupHelper() planning_display_->useApproximateIK(ui_->approximate_ik->isChecked()); if (ui_->allow_external_program->isChecked()) planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::allowExternalProgramCommunication, this, true)); + std::bind(&MotionPlanningFrame::allowExternalProgramCommunication, this, true)); } } } @@ -407,7 +407,7 @@ void MotionPlanningFrame::changePlanningGroupHelper() void MotionPlanningFrame::changePlanningGroup() { - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::changePlanningGroupHelper, this), + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::changePlanningGroupHelper, this), "Frame::changePlanningGroup"); joints_tab_->changePlanningGroup(planning_display_->getCurrentPlanningGroup(), planning_display_->getQueryStartStateHandler(), @@ -417,7 +417,7 @@ void MotionPlanningFrame::changePlanningGroup() void MotionPlanningFrame::sceneUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) { if (update_type & planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY) - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); } void MotionPlanningFrame::addSceneObject() @@ -494,11 +494,11 @@ void MotionPlanningFrame::addSceneObject() } setLocalSceneEdited(); - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); // Automatically select the inserted object so that its IM is displayed planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::setItemSelectionInList, this, shape_name, true, ui_->collision_objects_list)); + std::bind(&MotionPlanningFrame::setItemSelectionInList, this, shape_name, true, ui_->collision_objects_list)); planning_display_->queueRenderSceneGeometry(); } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp index 358c8452da..f429794f02 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp @@ -53,7 +53,7 @@ namespace moveit_rviz_plugin { void MotionPlanningFrame::databaseConnectButtonClicked() { - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClicked, this), + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClicked, this), "connect to database"); } @@ -108,7 +108,7 @@ void MotionPlanningFrame::resetDbButtonClicked() return; planning_display_->addBackgroundJob( - boost::bind(&MotionPlanningFrame::computeResetDbButtonClicked, this, response.toStdString()), "reset database"); + std::bind(&MotionPlanningFrame::computeResetDbButtonClicked, this, response.toStdString()), "reset database"); } void MotionPlanningFrame::computeDatabaseConnectButtonClicked() @@ -119,12 +119,12 @@ void MotionPlanningFrame::computeDatabaseConnectButtonClicked() robot_state_storage_.reset(); constraints_storage_.reset(); planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 1)); + std::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 1)); } else { planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 2)); + std::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 2)); try { warehouse_ros::DatabaseConnection::Ptr conn = moveit_warehouse::loadDatabase(); @@ -138,19 +138,19 @@ void MotionPlanningFrame::computeDatabaseConnectButtonClicked() else { planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 3)); + std::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 3)); return; } } catch (std::exception& ex) { planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 3)); + std::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 3)); ROS_ERROR("%s", ex.what()); return; } planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 4)); + std::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 4)); } } @@ -204,8 +204,7 @@ void MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper(int mode) if (move_group_) { move_group_->setConstraintsDatabase(ui_->database_host->text().toStdString(), ui_->database_port->value()); - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::populateConstraintsList, this), - "populateConstraintsList"); + planning_display_->addBackgroundJob([this]() { populateConstraintsList(); }, "populateConstraintsList"); } } } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp index 76e4c0d6da..80ec560597 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp @@ -59,11 +59,10 @@ void MotionPlanningFrame::detectObjectsButtonClicked() } if (semantic_world_) { - semantic_world_->addTableCallback(boost::bind(&MotionPlanningFrame::updateTables, this)); + semantic_world_->addTableCallback(std::bind(&MotionPlanningFrame::updateTables, this)); } } - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::triggerObjectDetection, this), - "detect objects"); + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::triggerObjectDetection, this), "detect objects"); } void MotionPlanningFrame::processDetectedObjects() @@ -167,7 +166,7 @@ void MotionPlanningFrame::triggerObjectDetection() void MotionPlanningFrame::listenDetectedObjects(const object_recognition_msgs::RecognizedObjectArrayPtr& /*msg*/) { ros::Duration(1.0).sleep(); - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::processDetectedObjects, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::processDetectedObjects, this)); } void MotionPlanningFrame::updateDetectedObjectsList(const std::vector& object_ids) @@ -197,13 +196,13 @@ void MotionPlanningFrame::updateDetectedObjectsList(const std::vectoraddBackgroundJob(boost::bind(&MotionPlanningFrame::publishTables, this), "publish tables"); + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::publishTables, this), "publish tables"); } void MotionPlanningFrame::publishTables() { semantic_world_->addTablesToCollisionWorld(); - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::updateSupportSurfacesList, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::updateSupportSurfacesList, this)); } void MotionPlanningFrame::selectedSupportSurfaceChanged() @@ -296,7 +295,7 @@ void MotionPlanningFrame::pickObjectButtonClicked() } ROS_INFO("Trying to pick up object %s from support surface %s", pick_object_name_[group_name].c_str(), support_surface_name_.c_str()); - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::pickObject, this), "pick"); + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::pickObject, this), "pick"); } void MotionPlanningFrame::placeObjectButtonClicked() @@ -342,7 +341,7 @@ void MotionPlanningFrame::placeObjectButtonClicked() upright_orientation, 0.1); planning_display_->visualizePlaceLocations(place_poses_); place_object_name_ = attached_bodies[0]->getName(); - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::placeObject, this), "place"); + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::placeObject, this), "place"); } void MotionPlanningFrame::pickObject() diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index a196132170..d3a0e706dc 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -146,7 +146,7 @@ void MotionPlanningFrame::clearScene() ps->getPlanningSceneMsg(msg); planning_scene_publisher_.publish(msg); setLocalSceneEdited(false); - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); planning_display_->queueRenderSceneGeometry(); } } @@ -228,7 +228,7 @@ void MotionPlanningFrame::removeSceneObject() ps->getCurrentStateNonConst().clearAttachedBody(sel[i]->text().toStdString()); scene_marker_.reset(); setLocalSceneEdited(); - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); planning_display_->queueRenderSceneGeometry(); } } @@ -498,7 +498,7 @@ void MotionPlanningFrame::copySelectedCollisionObject() ROS_DEBUG("Copied collision object to '%s'", name.c_str()); } setLocalSceneEdited(); - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); } void MotionPlanningFrame::computeSaveSceneButtonClicked() @@ -517,7 +517,7 @@ void MotionPlanningFrame::computeSaveSceneButtonClicked() ROS_ERROR("%s", ex.what()); } - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); } } @@ -538,7 +538,7 @@ void MotionPlanningFrame::computeSaveQueryButtonClicked(const std::string& scene ROS_ERROR("%s", ex.what()); } - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); } } @@ -575,7 +575,7 @@ void MotionPlanningFrame::computeDeleteSceneButtonClicked() ROS_ERROR("%s", ex.what()); } } - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); } } } @@ -601,7 +601,7 @@ void MotionPlanningFrame::computeDeleteQueryButtonClicked() ROS_ERROR("%s", ex.what()); } planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::computeDeleteQueryButtonClickedHelper, this, s)); + std::bind(&MotionPlanningFrame::computeDeleteQueryButtonClickedHelper, this, s)); } } } @@ -847,7 +847,7 @@ void MotionPlanningFrame::renameCollisionObject(QListWidgetItem* item) if (scene_marker_) { scene_marker_.reset(); - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::createSceneInteractiveMarker, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::createSceneInteractiveMarker, this)); } } } @@ -998,7 +998,7 @@ void MotionPlanningFrame::exportGeometryAsTextButtonClicked() QFileDialog::getSaveFileName(this, tr("Export Scene Geometry"), tr(""), tr("Scene Geometry (*.scene)")); if (!path.isEmpty()) planning_display_->addBackgroundJob( - boost::bind(&MotionPlanningFrame::computeExportGeometryAsText, this, path.toStdString()), "export as text"); + std::bind(&MotionPlanningFrame::computeExportGeometryAsText, this, path.toStdString()), "export as text"); } void MotionPlanningFrame::computeExportGeometryAsText(const std::string& path) @@ -1028,7 +1028,7 @@ void MotionPlanningFrame::computeImportGeometryFromText(const std::string& path) if (ps->loadGeometryFromStream(fin)) { ROS_INFO("Loaded scene geometry from '%s'", path.c_str()); - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); planning_display_->queueRenderSceneGeometry(); setLocalSceneEdited(); } @@ -1047,6 +1047,6 @@ void MotionPlanningFrame::importGeometryFromTextButtonClicked() QFileDialog::getOpenFileName(this, tr("Import Scene Geometry"), tr(""), tr("Scene Geometry (*.scene)")); if (!path.isEmpty()) planning_display_->addBackgroundJob( - boost::bind(&MotionPlanningFrame::computeImportGeometryFromText, this, path.toStdString()), "import from text"); + std::bind(&MotionPlanningFrame::computeImportGeometryFromText, this, path.toStdString()), "import from text"); } } // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index f5d116cbf2..c8767bef3a 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -53,15 +53,14 @@ namespace moveit_rviz_plugin void MotionPlanningFrame::planButtonClicked() { publishSceneIfNeeded(); - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computePlanButtonClicked, this), - "compute plan"); + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computePlanButtonClicked, this), "compute plan"); } void MotionPlanningFrame::executeButtonClicked() { ui_->execute_button->setEnabled(false); // execution is done in a separate thread, to not block other background jobs by blocking for synchronous execution - planning_display_->spawnBackgroundJob(boost::bind(&MotionPlanningFrame::computeExecuteButtonClicked, this)); + planning_display_->spawnBackgroundJob(std::bind(&MotionPlanningFrame::computeExecuteButtonClicked, this)); } void MotionPlanningFrame::planAndExecuteButtonClicked() @@ -70,13 +69,13 @@ void MotionPlanningFrame::planAndExecuteButtonClicked() ui_->plan_and_execute_button->setEnabled(false); ui_->execute_button->setEnabled(false); // execution is done in a separate thread, to not block other background jobs by blocking for synchronous execution - planning_display_->spawnBackgroundJob(boost::bind(&MotionPlanningFrame::computePlanAndExecuteButtonClicked, this)); + planning_display_->spawnBackgroundJob(std::bind(&MotionPlanningFrame::computePlanAndExecuteButtonClicked, this)); } void MotionPlanningFrame::stopButtonClicked() { ui_->stop_button->setEnabled(false); // avoid clicking again - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeStopButtonClicked, this), "stop"); + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeStopButtonClicked, this), "stop"); } void MotionPlanningFrame::allowReplanningToggled(bool checked) @@ -269,8 +268,8 @@ void MotionPlanningFrame::onNewPlanningSceneState() void MotionPlanningFrame::startStateTextChanged(const QString& start_state) { // use background job: fetching the current state might take up to a second - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::startStateTextChangedExec, this, - start_state.toStdString()), + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::startStateTextChangedExec, this, + start_state.toStdString()), "update start state"); } @@ -285,7 +284,7 @@ void MotionPlanningFrame::goalStateTextChanged(const QString& goal_state) { // use background job: fetching the current state might take up to a second planning_display_->addBackgroundJob( - boost::bind(&MotionPlanningFrame::goalStateTextChangedExec, this, goal_state.toStdString()), "update goal state"); + std::bind(&MotionPlanningFrame::goalStateTextChangedExec, this, goal_state.toStdString()), "update goal state"); } void MotionPlanningFrame::goalStateTextChangedExec(const std::string& goal_state) @@ -454,8 +453,7 @@ void MotionPlanningFrame::populatePlannerDescription(const moveit_msgs::PlannerI void MotionPlanningFrame::populateConstraintsList() { if (move_group_) - planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::populateConstraintsList, this, move_group_->getKnownConstraints())); + planning_display_->addMainLoopJob([this]() { populateConstraintsList(move_group_->getKnownConstraints()); }); } void MotionPlanningFrame::populateConstraintsList(const std::vector& constr) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp index 430e9cfbd3..b1c2e0849e 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp @@ -108,7 +108,7 @@ void MotionPlanningFrame::saveSceneButtonClicked() } } - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeSaveSceneButtonClicked, this), + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeSaveSceneButtonClicked, this), "save scene"); } } @@ -132,7 +132,7 @@ void MotionPlanningFrame::saveQueryButtonClicked() { std::string scene = s->text(0).toStdString(); planning_display_->addBackgroundJob( - boost::bind(&MotionPlanningFrame::computeSaveQueryButtonClicked, this, scene, ""), "save query"); + std::bind(&MotionPlanningFrame::computeSaveQueryButtonClicked, this, scene, ""), "save query"); } else { @@ -177,7 +177,7 @@ void MotionPlanningFrame::saveQueryButtonClicked() } } planning_display_->addBackgroundJob( - boost::bind(&MotionPlanningFrame::computeSaveQueryButtonClicked, this, scene, query_name), "save query"); + std::bind(&MotionPlanningFrame::computeSaveQueryButtonClicked, this, scene, query_name), "save query"); } } } @@ -185,25 +185,25 @@ void MotionPlanningFrame::saveQueryButtonClicked() void MotionPlanningFrame::deleteSceneButtonClicked() { - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeDeleteSceneButtonClicked, this), + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeDeleteSceneButtonClicked, this), "delete scene"); } void MotionPlanningFrame::deleteQueryButtonClicked() { - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeDeleteQueryButtonClicked, this), + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeDeleteQueryButtonClicked, this), "delete query"); } void MotionPlanningFrame::loadSceneButtonClicked() { - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeLoadSceneButtonClicked, this), + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeLoadSceneButtonClicked, this), "load scene"); } void MotionPlanningFrame::loadQueryButtonClicked() { - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeLoadQueryButtonClicked, this), + planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeLoadQueryButtonClicked, this), "load query"); } @@ -221,7 +221,7 @@ void MotionPlanningFrame::warehouseItemNameChanged(QTreeWidgetItem* item, int co if (planning_scene_storage->hasPlanningScene(new_name)) { - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); QMessageBox::warning(this, "Scene not renamed", QString("The scene name '").append(item->text(column)).append("' already exists")); return; @@ -239,7 +239,7 @@ void MotionPlanningFrame::warehouseItemNameChanged(QTreeWidgetItem* item, int co std::string new_name = item->text(column).toStdString(); if (planning_scene_storage->hasPlanningQuery(scene, new_name)) { - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); + planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); QMessageBox::warning(this, "Query not renamed", QString("The query name '") .append(item->text(column)) 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 bea40fafa0..cb9603c505 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 @@ -210,7 +210,7 @@ void PlanningSceneDisplay::reset() Display::reset(); if (isEnabled()) - addBackgroundJob(boost::bind(&PlanningSceneDisplay::loadRobotModel, this), "loadRobotModel"); + addBackgroundJob(std::bind(&PlanningSceneDisplay::loadRobotModel, this), "loadRobotModel"); if (planning_scene_robot_) { @@ -386,7 +386,7 @@ void PlanningSceneDisplay::changedPlanningSceneTopic() service_name = ros::names::append(getMoveGroupNS(), service_name); auto bg_func = [=]() { if (planning_scene_monitor_->requestPlanningSceneState(service_name)) - addMainLoopJob(boost::bind(&PlanningSceneDisplay::onNewPlanningSceneState, this)); + addMainLoopJob(std::bind(&PlanningSceneDisplay::onNewPlanningSceneState, this)); else setStatus(rviz::StatusProperty::Warn, "PlanningScene", "Requesting initial scene failed"); }; @@ -529,7 +529,7 @@ void PlanningSceneDisplay::loadRobotModel() // we need to make sure the clearing of the robot model is in the main thread, // so that rendering operations do not have data removed from underneath, // so the next function is executed in the main loop - addMainLoopJob(boost::bind(&PlanningSceneDisplay::clearRobotModel, this)); + addMainLoopJob(std::bind(&PlanningSceneDisplay::clearRobotModel, this)); waitForAllMainLoopJobs(); @@ -537,8 +537,9 @@ void PlanningSceneDisplay::loadRobotModel() if (psm->getPlanningScene()) { planning_scene_monitor_.swap(psm); - planning_scene_monitor_->addUpdateCallback(boost::bind(&PlanningSceneDisplay::sceneMonitorReceivedUpdate, this, _1)); - addMainLoopJob(boost::bind(&PlanningSceneDisplay::onRobotModelLoaded, this)); + planning_scene_monitor_->addUpdateCallback( + std::bind(&PlanningSceneDisplay::sceneMonitorReceivedUpdate, this, std::placeholders::_1)); + addMainLoopJob(std::bind(&PlanningSceneDisplay::onRobotModelLoaded, this)); waitForAllMainLoopJobs(); } else @@ -598,7 +599,7 @@ void PlanningSceneDisplay::onEnable() { Display::onEnable(); - addBackgroundJob(boost::bind(&PlanningSceneDisplay::loadRobotModel, this), "loadRobotModel"); + addBackgroundJob(std::bind(&PlanningSceneDisplay::loadRobotModel, this), "loadRobotModel"); if (planning_scene_robot_) { diff --git a/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp b/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp index 4b1afa32a3..b24612c183 100644 --- a/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp +++ b/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp @@ -186,14 +186,16 @@ int main(int argc, char** argv) ROS_INFO(" * %s", name.c_str()); } - psm.addUpdateCallback(boost::bind(&onSceneUpdate, &psm, &pss)); + psm.addUpdateCallback(std::bind(&onSceneUpdate, &psm, &pss)); boost::function callback1 = - boost::bind(&onMotionPlanRequest, _1, &psm, &pss); + std::bind(&onMotionPlanRequest, std::placeholders::_1, &psm, &pss); ros::Subscriber mplan_req_sub = nh.subscribe("motion_plan_request", 100, callback1); - boost::function callback2 = boost::bind(&onConstraints, _1, &cs); + boost::function callback2 = + std::bind(&onConstraints, std::placeholders::_1, &cs); ros::Subscriber constr_sub = nh.subscribe("constraints", 100, callback2); - boost::function callback3 = boost::bind(&onRobotState, _1, &rs); + boost::function callback3 = + std::bind(&onRobotState, std::placeholders::_1, &rs); ros::Subscriber state_sub = nh.subscribe("robot_state", 100, callback3); std::vector topics; diff --git a/moveit_ros/warehouse/warehouse/src/warehouse_services.cpp b/moveit_ros/warehouse/warehouse/src/warehouse_services.cpp index cc1614f668..2bf03d529c 100644 --- a/moveit_ros/warehouse/warehouse/src/warehouse_services.cpp +++ b/moveit_ros/warehouse/warehouse/src/warehouse_services.cpp @@ -181,27 +181,27 @@ int main(int argc, char** argv) boost::function - save_cb = boost::bind(&storeState, _1, _2, &rs); + save_cb = std::bind(&storeState, std::placeholders::_1, std::placeholders::_2, &rs); boost::function - list_cb = boost::bind(&listStates, _1, _2, &rs); + list_cb = std::bind(&listStates, std::placeholders::_1, std::placeholders::_2, &rs); boost::function - get_cb = boost::bind(&getState, _1, _2, &rs); + get_cb = std::bind(&getState, std::placeholders::_1, std::placeholders::_2, &rs); boost::function - has_cb = boost::bind(&hasState, _1, _2, &rs); + has_cb = std::bind(&hasState, std::placeholders::_1, std::placeholders::_2, &rs); boost::function - rename_cb = boost::bind(&renameState, _1, _2, &rs); + rename_cb = std::bind(&renameState, std::placeholders::_1, std::placeholders::_2, &rs); boost::function - delete_cb = boost::bind(&deleteState, _1, _2, &rs); + delete_cb = std::bind(&deleteState, std::placeholders::_1, std::placeholders::_2, &rs); ros::ServiceServer save_state_server = node.advertiseService("save_robot_state", save_cb); ros::ServiceServer list_states_server = node.advertiseService("list_robot_states", list_cb); diff --git a/moveit_setup_assistant/src/tools/compute_default_collisions.cpp b/moveit_setup_assistant/src/tools/compute_default_collisions.cpp index 58421c7b40..ab57be15d4 100644 --- a/moveit_setup_assistant/src/tools/compute_default_collisions.cpp +++ b/moveit_setup_assistant/src/tools/compute_default_collisions.cpp @@ -564,7 +564,7 @@ unsigned int disableNeverInCollision(const unsigned int num_trials, planning_sce for (int i = 0; i < num_threads; ++i) { ThreadComputation tc(scene, req, i, num_trials / num_threads, &links_seen_colliding, &lock, progress); - bgroup.create_thread(boost::bind(&disableNeverInCollisionThread, tc)); + bgroup.create_thread(std::bind(&disableNeverInCollisionThread, tc)); } try diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 55458bbf89..2685d6d1d8 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -226,7 +226,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = file.file_name_; template_path = config_data_->appendPaths(config_data_->template_package_path_, "package.xml.template"); file.description_ = "Defines a ROS package"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = MoveItConfigData::AUTHOR_INFO; gen_files_.push_back(file); @@ -235,7 +235,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = file.file_name_; template_path = config_data_->appendPaths(config_data_->template_package_path_, file.file_name_); file.description_ = "CMake build system configuration file"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -249,7 +249,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = file.file_name_; file.description_ = "Folder containing all MoveIt configuration files for your robot. This folder is required and " "cannot be disabled."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::createFolder, this, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::createFolder, this, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -263,7 +263,7 @@ bool ConfigurationFilesWidget::loadGenFiles() "representation of semantic information about robots. This format is intended to represent " "information about the robot that is not in the URDF file, but it is useful for a variety of " "applications. The intention is to include information that has a semantic aspect to it."; - file.gen_func_ = boost::bind(&srdf::SRDFWriter::writeSRDF, config_data_->srdf_, _1); + file.gen_func_ = std::bind(&srdf::SRDFWriter::writeSRDF, config_data_->srdf_, std::placeholders::_1); file.write_on_changes = MoveItConfigData::SRDF; gen_files_.push_back(file); // special step required so the generated .setup_assistant yaml has this value @@ -280,7 +280,7 @@ bool ConfigurationFilesWidget::loadGenFiles() "planner_configs tag. While defining a planner configuration, the only mandatory parameter is " "'type', which is the name of the motion planner to be used. Any other planner-specific " "parameters can be defined but are optional."; - file.gen_func_ = boost::bind(&MoveItConfigData::outputOMPLPlanningYAML, config_data_, _1); + file.gen_func_ = std::bind(&MoveItConfigData::outputOMPLPlanningYAML, config_data_, std::placeholders::_1); file.write_on_changes = MoveItConfigData::GROUPS; gen_files_.push_back(file); @@ -288,7 +288,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.file_name_ = "chomp_planning.yaml"; file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); file.description_ = "Specifies which chomp planning plugin parameters to be used for the CHOMP planner"; - file.gen_func_ = boost::bind(&MoveItConfigData::outputCHOMPPlanningYAML, config_data_, _1); + file.gen_func_ = std::bind(&MoveItConfigData::outputCHOMPPlanningYAML, config_data_, std::placeholders::_1); file.write_on_changes = MoveItConfigData::GROUPS; // need to double check if this is actually correct! gen_files_.push_back(file); @@ -297,7 +297,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); file.description_ = "Specifies which kinematic solver plugin to use for each planning group in the SRDF, as well as " "the kinematic solver search resolution."; - file.gen_func_ = boost::bind(&MoveItConfigData::outputKinematicsYAML, config_data_, _1); + file.gen_func_ = std::bind(&MoveItConfigData::outputKinematicsYAML, config_data_, std::placeholders::_1); file.write_on_changes = MoveItConfigData::GROUPS | MoveItConfigData::GROUP_KINEMATICS; gen_files_.push_back(file); @@ -309,7 +309,7 @@ bool ConfigurationFilesWidget::loadGenFiles() "and acceleration than those contained in your URDF. This information is used by our trajectory " "filtering system to assign reasonable velocities and timing for the trajectory before it is " "passed to the robots controllers."; - file.gen_func_ = boost::bind(&MoveItConfigData::outputJointLimitsYAML, config_data_, _1); + file.gen_func_ = std::bind(&MoveItConfigData::outputJointLimitsYAML, config_data_, std::placeholders::_1); file.write_on_changes = 0; // Can they be changed? gen_files_.push_back(file); @@ -320,7 +320,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.description_ = "Cartesian velocity for planning in the workspace." "The velocity is used by pilz industrial motion planner as maximum velocity for cartesian " "planning requests scaled by the velocity scaling factor of an individual planning request."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; // Can they be changed? gen_files_.push_back(file); @@ -329,7 +329,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); file.description_ = "Creates dummy configurations for controllers that correspond to defined groups. This is mostly " "useful for testing."; - file.gen_func_ = boost::bind(&MoveItConfigData::outputFakeControllersYAML, config_data_, _1); + file.gen_func_ = std::bind(&MoveItConfigData::outputFakeControllersYAML, config_data_, std::placeholders::_1); file.write_on_changes = MoveItConfigData::GROUPS; gen_files_.push_back(file); @@ -337,7 +337,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.file_name_ = "simple_moveit_controllers.yaml"; file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); file.description_ = "Creates controller configuration for SimpleMoveItControllerManager"; - file.gen_func_ = boost::bind(&MoveItConfigData::outputSimpleControllersYAML, config_data_, _1); + file.gen_func_ = std::bind(&MoveItConfigData::outputSimpleControllersYAML, config_data_, std::placeholders::_1); file.write_on_changes = MoveItConfigData::GROUPS; gen_files_.push_back(file); @@ -346,14 +346,14 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); template_path = config_data_->appendPaths(config_data_->template_package_path_, file.rel_path_); file.description_ = "Configuration of Gazebo controllers"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); gen_files_.push_back(file); // ros_controllers.yaml -------------------------------------------------------------------------------------- file.file_name_ = "ros_controllers.yaml"; file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); file.description_ = "Creates controller configurations for ros_control."; - file.gen_func_ = boost::bind(&MoveItConfigData::outputROSControllersYAML, config_data_, _1); + file.gen_func_ = std::bind(&MoveItConfigData::outputROSControllersYAML, config_data_, std::placeholders::_1); file.write_on_changes = MoveItConfigData::GROUPS; gen_files_.push_back(file); @@ -361,7 +361,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.file_name_ = "sensors_3d.yaml"; file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); file.description_ = "Creates configurations 3d sensors."; - file.gen_func_ = boost::bind(&MoveItConfigData::output3DSensorPluginYAML, config_data_, _1); + file.gen_func_ = std::bind(&MoveItConfigData::output3DSensorPluginYAML, config_data_, std::placeholders::_1); file.write_on_changes = MoveItConfigData::SENSORS_CONFIG; gen_files_.push_back(file); @@ -376,7 +376,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = file.file_name_; file.description_ = "Folder containing all MoveIt launch files for your robot. " "This folder is required and cannot be disabled."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::createFolder, this, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::createFolder, this, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -387,7 +387,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.description_ = "Launches the move_group node that provides the MoveGroup action and other parameters MoveGroup action"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -397,7 +397,7 @@ bool ConfigurationFilesWidget::loadGenFiles() template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Loads settings for the ROS parameter server, required for running MoveIt. This includes the " "SRDF, joints_limits.yaml file, ompl_planning.yaml file, optionally the URDF, etc"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -407,7 +407,7 @@ bool ConfigurationFilesWidget::loadGenFiles() template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Visualize in Rviz the robot's planning groups running with interactive markers that allow goal " "states to be set."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -419,7 +419,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.description_ = "Intended to be included in other launch files that require the OMPL planning plugin. Defines " "the proper plugin name on the parameter server and a default selection of planning request " "adapters."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -431,7 +431,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.description_ = "Intended to be included in other launch files that require the Pilz industrial motion planner " "planning plugin. Defines the proper plugin name on the parameter server and a default selection " "of planning request adapters."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -443,7 +443,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.description_ = "Intended to be included in other launch files that require the CHOMP planning plugin. Defines " "the proper plugin name on the parameter server and a default selection of planning request " "adapters."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -452,7 +452,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Helper launch file that can choose between different planning pipelines to be loaded."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -461,7 +461,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Helper launch file that specifies default settings for MongoDB."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -470,7 +470,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Launch file for starting MongoDB."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -479,7 +479,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Launch file for starting the warehouse with a default MongoDB."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -488,7 +488,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Launch file for benchmarking OMPL planners"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -497,7 +497,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Helper launch file that can choose between different sensor managers to be loaded."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -506,7 +506,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, "moveit_sensor_manager.launch.xml"); file.description_ = "Placeholder for settings specific to the MoveIt sensor manager implemented for you robot."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -516,7 +516,7 @@ bool ConfigurationFilesWidget::loadGenFiles() template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Loads settings for the ROS parameter server required for executing trajectories using the " "trajectory_execution_manager::TrajectoryExecutionManager."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -524,7 +524,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Loads the fake controller plugin."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -532,7 +532,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Loads the default controller plugin."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -540,7 +540,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Loads the ros_control controller plugin."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -549,7 +549,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Run a demo of MoveIt."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -558,7 +558,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Run a demo of MoveIt with Gazebo and Rviz"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -568,7 +568,7 @@ bool ConfigurationFilesWidget::loadGenFiles() template_path = config_data_->appendPaths(template_launch_path, "gazebo.launch"); file.description_ = "Gazebo launch file which also launches ros_controllers and sends robot urdf to param server, " "then using gazebo_ros pkg the robot is spawned to Gazebo"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); gen_files_.push_back(file); // joystick_control.launch ------------------------------------------------------------------ @@ -576,7 +576,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, file.file_name_); file.description_ = "Control the Rviz Motion Planning Plugin with a joystick"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -588,7 +588,7 @@ bool ConfigurationFilesWidget::loadGenFiles() // up with the SA's real launch file file.description_ = "Launch file for easily re-starting the MoveIt Setup Assistant to edit this robot's generated " "configuration package."; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -597,7 +597,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, "ros_controllers.launch"); file.description_ = "ros_controllers launch file"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = MoveItConfigData::GROUPS; gen_files_.push_back(file); @@ -607,7 +607,7 @@ bool ConfigurationFilesWidget::loadGenFiles() template_path = config_data_->appendPaths(template_launch_path, "moveit.rviz"); file.description_ = "Configuration file for Rviz with the Motion Planning Plugin already setup. Used by passing " "roslaunch moveit_rviz.launch config:=true"; - file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -619,7 +619,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.file_name_ = SETUP_ASSISTANT_FILE; file.rel_path_ = file.file_name_; file.description_ = "MoveIt Setup Assistant's hidden settings file. You should not need to edit this file."; - file.gen_func_ = boost::bind(&MoveItConfigData::outputSetupAssistantFile, config_data_, _1); + file.gen_func_ = std::bind(&MoveItConfigData::outputSetupAssistantFile, config_data_, std::placeholders::_1); file.write_on_changes = -1; // write on any changes gen_files_.push_back(file); diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp index 76be30b8db..cb548414e4 100644 --- a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp +++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp @@ -235,7 +235,8 @@ void DefaultCollisionsWidget::startGeneratingCollisionTable() btn_revert_->setEnabled(true); // allow to interrupt and revert // create a MonitorThread running generateCollisionTable() in a worker thread and monitoring the progress - worker_ = new MonitorThread(boost::bind(&DefaultCollisionsWidget::generateCollisionTable, this, _1), progress_bar_); + worker_ = new MonitorThread(std::bind(&DefaultCollisionsWidget::generateCollisionTable, this, std::placeholders::_1), + progress_bar_); connect(worker_, SIGNAL(finished()), this, SLOT(finishGeneratingCollisionTable())); worker_->start(); // start after having finished() signal connected } @@ -820,7 +821,7 @@ moveit_setup_assistant::MonitorThread::MonitorThread(const boost::function Date: Sun, 28 Nov 2021 20:18:39 +0100 Subject: [PATCH 108/229] Add marker for subgroups even if no endeffector is defined for them (#2977) For single groups, the old logic fell back to add a marker for the last link if IK is supported for it and no endeffector is defined. That (quite reasonable) fallback did not yet work for subgroups though. --- .../src/robot_interaction.cpp | 59 +++++++++---------- 1 file changed, 28 insertions(+), 31 deletions(-) diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index d4128f0dc2..7d3787ac5a 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -280,56 +280,53 @@ void RobotInteraction::decideActiveEndEffectors(const std::string& group, Intera const std::pair& smap = jmg->getGroupKinematics(); - // if we have an IK solver for the selected group, we check if there are any end effectors attached to this group - if (smap.first) - { + auto add_active_end_effectors_for_single_group = [&](const moveit::core::JointModelGroup* single_group) { + bool found_eef{ false }; for (const srdf::Model::EndEffector& eef : eefs) - if ((jmg->hasLinkModel(eef.parent_link_) || jmg->getName() == eef.parent_group_) && - jmg->canSetStateFromIK(eef.parent_link_)) + if ((single_group->hasLinkModel(eef.parent_link_) || single_group->getName() == eef.parent_group_) && + single_group->canSetStateFromIK(eef.parent_link_)) { // We found an end-effector whose parent is the group. EndEffectorInteraction ee; - ee.parent_group = group; + ee.parent_group = single_group->getName(); ee.parent_link = eef.parent_link_; ee.eef_group = eef.component_group_; ee.interaction = style; active_eef_.push_back(ee); + found_eef = true; } - // No end effectors found. Use last link in group as the "end effector". - if (active_eef_.empty() && !jmg->getLinkModelNames().empty()) + // No end effectors found. Use last link in group as the "end effector". + if (!found_eef && !single_group->getLinkModelNames().empty()) { - EndEffectorInteraction ee; - ee.parent_group = group; - ee.parent_link = jmg->getLinkModelNames().back(); - ee.eef_group = group; - ee.interaction = style; - active_eef_.push_back(ee); + std::string last_link{ single_group->getLinkModelNames().back() }; + + if (single_group->canSetStateFromIK(last_link)) + { + EndEffectorInteraction ee; + ee.parent_group = single_group->getName(); + ee.parent_link = last_link; + ee.eef_group = single_group->getName(); + ee.interaction = style; + active_eef_.push_back(ee); + } } + }; + + // if we have an IK solver for the selected group, we check if there are any end effectors attached to this group + if (smap.first) + { + add_active_end_effectors_for_single_group(jmg); } + // if the group contains subgroups with IK, add markers for them individually else if (!smap.second.empty()) { for (const std::pair& it : smap.second) - { - for (const srdf::Model::EndEffector& eef : eefs) - { - if ((it.first->hasLinkModel(eef.parent_link_) || jmg->getName() == eef.parent_group_) && - it.first->canSetStateFromIK(eef.parent_link_)) - { - // We found an end-effector whose parent is a subgroup of the group. (May be more than one) - EndEffectorInteraction ee; - ee.parent_group = it.first->getName(); - ee.parent_link = eef.parent_link_; - ee.eef_group = eef.component_group_; - ee.interaction = style; - active_eef_.push_back(ee); - break; - } - } - } + add_active_end_effectors_for_single_group(it.first); } + // lastly determine automatic marker sizes for (EndEffectorInteraction& eef : active_eef_) { // if we have a separate group for the eef, we compute the scale based on it; From fb0a5b5239c4c1364b12b76629eaa3b9af2d8739 Mon Sep 17 00:00:00 2001 From: Simon Schmeisser Date: Tue, 7 Dec 2021 15:17:28 +0100 Subject: [PATCH 109/229] Use termination condition for simplification step (#2981) ... to allow canceling the simplification step --- .../ompl_interface/src/model_based_planning_context.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) 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 691ab5a9e8..7bb0d51b54 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 @@ -403,8 +403,12 @@ void ompl_interface::ModelBasedPlanningContext::setPlanningVolume(const moveit_m void ompl_interface::ModelBasedPlanningContext::simplifySolution(double timeout) { - ompl_simple_setup_->simplifySolution(timeout); + ompl::time::point start = ompl::time::now(); + ob::PlannerTerminationCondition ptc = constructPlannerTerminationCondition(timeout, start); + registerTerminationCondition(ptc); + ompl_simple_setup_->simplifySolution(ptc); last_simplify_time_ = ompl_simple_setup_->getLastSimplificationTime(); + unregisterTerminationCondition(); } void ompl_interface::ModelBasedPlanningContext::interpolateSolution() From d2c40f280061c7806cc260c8cb1060c00e6c2d7c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ga=C3=ABl=20=C3=89corchard?= Date: Thu, 9 Dec 2021 12:36:34 +0100 Subject: [PATCH 110/229] Use emulated time in action-based controller MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Gaël Écorchard --- .../action_based_controller_handle.h | 29 ++++++++++++++++--- 1 file changed, 25 insertions(+), 4 deletions(-) diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h index 1187a5c38d..f36f571168 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h @@ -45,6 +45,8 @@ namespace moveit_simple_controller_manager { +using namespace std::chrono_literals; + /* * This exist solely to inject addJoint/getJoints into base non-templated class. */ @@ -136,11 +138,30 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase } else { - std::future_status status = result_future.wait_for(timeout.to_chrono>()); - if (status == std::future_status::timeout) + const bool use_sim_time = node_->get_parameter("use_sim_time").as_bool(); + if (use_sim_time) + { + std::future_status status; + const auto start = node_->now(); + do + { + RCLCPP_WARN(LOGGER, "waitForExecution, wait_for"); + status = result_future.wait_for(50ms); + if ((status == std::future_status::timeout) and ((node_->now() - start) > timeout)) + { + RCLCPP_WARN(LOGGER, "waitForExecution timed out"); + return false; + } + } while (status == std::future_status::timeout); + } + else { - RCLCPP_WARN(LOGGER, "waitForExecution timed out"); - return false; + std::future_status status = result_future.wait_for(timeout.to_chrono>()); + if (status == std::future_status::timeout) + { + RCLCPP_WARN(LOGGER, "waitForExecution timed out"); + return false; + } } } // To accommodate for the delay after the future for the result is ready and the time controllerDoneCallback takes to finish From 9d78be4705fad661a9cd2ef90b0f9d933d60b325 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Fri, 10 Dec 2021 14:15:29 +0000 Subject: [PATCH 111/229] Add backwards compatibility for old scene serialization format (#2986) * [moveit_core] test_planning_scene: Add failing unit test for old scene format The serialization format for the .scene files changed in https://github.com/ros-planning/moveit/pull/2037. This commits a testcase using the old scene format. It will fail and a subsequent commit to introduce backwards compatibility to the scene-file parsing will make it pass. * [moveit_core] PlanningScene: Add backwards compatibility for old scene version format This commit adds a mechanism for detecting the version of the scene file format to enable the loadGeometryFromStream method to read old version scene files without having to migrate them. To detect the version of the scene format, we use the content of the line following the start of an object: In the old version of the format, this specified the number of shapes in the object (a single int). In the new version of the format, it is the translational part of the pose of the object (i.e. three double values separated by spaces). To detect the format, we check for the number of spaces after trimming the string. * Simplify code: Avoid reading full stream Co-authored-by: Robert Haschke --- .../planning_scene/src/planning_scene.cpp | 48 +++++++++++++------ .../test/test_planning_scene.cpp | 30 +++++++++++- 2 files changed, 63 insertions(+), 15 deletions(-) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 43639be742..d59d952fa8 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -1007,7 +1007,23 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet ROS_ERROR_NAMED(LOGNAME, "Bad input stream when loading scene geometry"); return false; } + // Read scene name std::getline(in, name_); + + // Identify scene format version for backwards compatibility of parser + auto pos = in.tellg(); // remember current stream position + std::string line; + do + { + std::getline(in, line); + } while (in.good() && !in.eof() && (line.empty() || line[0] != '*')); // read * marker + std::getline(in, line); // next line determines format + boost::algorithm::trim(line); + // new format: line specifies position of object, with spaces as delimiter -> spaces indicate new format + // old format: line specifies number of shapes + bool uses_new_scene_format = line.find(' ') != std::string::npos; + in.seekg(pos); + Eigen::Isometry3d pose; // Transient do { @@ -1029,8 +1045,9 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet } boost::algorithm::trim(object_id); - // Read in object pose - if (!readPoseFromText(in, pose)) + // Read in object pose (added in the new scene format) + pose.setIdentity(); + if (uses_new_scene_format && !readPoseFromText(in, pose)) { ROS_ERROR_NAMED(LOGNAME, "Failed to read object pose from scene file"); return false; @@ -1075,22 +1092,25 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet } } - // Read in subframes - moveit::core::FixedTransformsMap subframes; - unsigned int subframe_count; - in >> subframe_count; - for (std::size_t i = 0; i < subframe_count && in.good() && !in.eof(); ++i) + // Read in subframes (added in the new scene format) + if (uses_new_scene_format) { - std::string subframe_name; - in >> subframe_name; - if (!readPoseFromText(in, pose)) + moveit::core::FixedTransformsMap subframes; + unsigned int subframe_count; + in >> subframe_count; + for (std::size_t i = 0; i < subframe_count && in.good() && !in.eof(); ++i) { - ROS_ERROR_NAMED(LOGNAME, "Failed to read subframe pose from scene file"); - return false; + std::string subframe_name; + in >> subframe_name; + if (!readPoseFromText(in, pose)) + { + ROS_ERROR_NAMED(LOGNAME, "Failed to read subframe pose from scene file"); + return false; + } + subframes[subframe_name] = pose; } - subframes[subframe_name] = pose; + world_->setSubframesOfObject(object_id, subframes); } - world_->setSubframesOfObject(object_id, subframes); } else if (marker == ".") { diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp index 2afaa788fb..86df646a23 100644 --- a/moveit_core/planning_scene/test/test_planning_scene.cpp +++ b/moveit_core/planning_scene/test/test_planning_scene.cpp @@ -164,7 +164,7 @@ TEST(PlanningScene, isStateValid) } } -TEST(PlanningScene, loadGoodSceneGeometry) +TEST(PlanningScene, loadGoodSceneGeometryNewFormat) { moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2"); auto ps = std::make_shared(robot_model->getURDF(), robot_model->getSRDF()); @@ -199,6 +199,32 @@ TEST(PlanningScene, loadGoodSceneGeometry) EXPECT_FALSE(ps->getWorld()->hasObject("baz")); // Sanity check. } +TEST(PlanningScene, loadGoodSceneGeometryOldFormat) +{ + moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2"); + auto ps = std::make_shared(robot_model->getURDF(), robot_model->getSRDF()); + + std::istringstream good_scene_geometry; + good_scene_geometry.str("foobar_scene\n" + "* foo\n" + "2\n" + "box\n" + ".77 0.39 0.05\n" + "0 0 0.025\n" + "0 0 0 1\n" + "0.82 0.75 0.60 1\n" + "box\n" + ".77 0.39 0.05\n" + "0 0 1.445\n" + "0 0 0 1\n" + "0.82 0.75 0.60 1\n" + ".\n"); + EXPECT_TRUE(ps->loadGeometryFromStream(good_scene_geometry)); + EXPECT_EQ(ps->getName(), "foobar_scene"); + EXPECT_TRUE(ps->getWorld()->hasObject("foo")); + EXPECT_FALSE(ps->getWorld()->hasObject("baz")); // Sanity check. +} + TEST(PlanningScene, loadBadSceneGeometry) { moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2"); @@ -211,6 +237,8 @@ TEST(PlanningScene, loadBadSceneGeometry) std::istringstream malformed_scene_geometry; malformed_scene_geometry.str("malformed_scene_geometry\n" "* foo\n" + "0 0 0\n" + "0 0 0 1\n" "1\n" "box\n" "2.58 1.36\n" /* Only two tokens; should be 3 */ From 9bfb2c73cf70acef7ba9d127698831d3fe14bd04 Mon Sep 17 00:00:00 2001 From: Simon Schmeisser Date: Fri, 10 Dec 2021 15:20:49 +0100 Subject: [PATCH 112/229] Allow restricting collision pairs to a group (#2987) --- .../moveit/planning_scene/planning_scene.h | 29 +++++++++++-------- .../planning_scene/src/planning_scene.cpp | 4 ++- 2 files changed, 20 insertions(+), 13 deletions(-) diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index 7187b47893..fb8c5336ec 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -560,37 +560,42 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from getCollidingPairs(contacts, getCurrentState(), getAllowedCollisionMatrix()); } - /** \brief Get the names of the links that are involved in collisions for the state \e robot_state */ + /** \brief Get the names of the links that are involved in collisions for the state \e robot_state. + * Can be restricted to links part of or updated by \e group_name */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, - const moveit::core::RobotState& robot_state) const + const moveit::core::RobotState& robot_state, const std::string& group_name = "") const { - getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix()); + getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix(), group_name); } /** \brief Get the names of the links that are involved in collisions for the state \e robot_state. - Update the link transforms for \e robot_state if needed. */ + Update the link transforms for \e robot_state if needed. + Can be restricted to links part of or updated by \e group_name */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, - moveit::core::RobotState& robot_state) const + moveit::core::RobotState& robot_state, const std::string& group_name = "") const { robot_state.updateCollisionBodyTransforms(); - getCollidingPairs(contacts, static_cast(robot_state), getAllowedCollisionMatrix()); + getCollidingPairs(contacts, static_cast(robot_state), getAllowedCollisionMatrix(), + group_name); } /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the - allowed collision matrix (\e acm). Update the link transforms for \e robot_state if needed. */ + allowed collision matrix (\e acm). Update the link transforms for \e robot_state if needed. + Can be restricted to links part of or updated by \e group_name*/ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, - moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const + moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm, + const std::string& group_name = "") const { robot_state.updateCollisionBodyTransforms(); - getCollidingPairs(contacts, static_cast(robot_state), acm); + getCollidingPairs(contacts, static_cast(robot_state), acm, group_name); } /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the - allowed collision matrix (\e acm) */ + allowed collision matrix (\e acm). Can be restricted to links part of or updated by \e group_name */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, const moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const; + const collision_detection::AllowedCollisionMatrix& acm, + const std::string& group_name = "") const; /**@}*/ diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index d59d952fa8..96db06e223 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -539,12 +539,14 @@ void PlanningScene::getCollidingPairs(collision_detection::CollisionResult::Cont void PlanningScene::getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, const moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const + const collision_detection::AllowedCollisionMatrix& acm, + const std::string& group_name) const { collision_detection::CollisionRequest req; req.contacts = true; req.max_contacts = getRobotModel()->getLinkModelsWithCollisionGeometry().size() + 1; req.max_contacts_per_pair = 1; + req.group_name = group_name; collision_detection::CollisionResult res; checkCollision(req, res, robot_state, acm); res.contacts.swap(contacts); From 983f494f083c4773efc409b4190ff973a8c72c36 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 13 Dec 2021 01:09:12 -0700 Subject: [PATCH 113/229] Increase ccache size (#2990) Increase ccache size to 10G. This is particularly needed for ccov's debug build, which uses 8G according to the stats. Co-authored-by: Robert Haschke --- .github/workflows/ci.yaml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 23274bd83e..5620d6d59b 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -36,6 +36,10 @@ jobs: # Pull any updates to the upstream workspace (after restoring it from cache) AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src AFTER_SETUP_DOWNSTREAM_WORKSPACE: vcs pull $BASEDIR/downstream_ws/src + # Clear the ccache stats before and log the stats after the build + AFTER_SETUP_CCACHE: ccache --zero-stats --max-size=10.0G + AFTER_BUILD_TARGET_WORKSPACE: ccache --show-stats + AFTER_BUILD_DOWNSTREAM_WORKSPACE: ccache --show-stats # Compile CCOV with Debug. Enable -Werror (except CLANG_TIDY=pedantic, which makes the clang-tidy step fail on warnings) TARGET_CMAKE_ARGS: > -DCMAKE_BUILD_TYPE=${{ matrix.env.CCOV && 'Debug' || 'Release'}} From 70b0368eeab50fee4e7cd5dcffedd02b724d713f Mon Sep 17 00:00:00 2001 From: Wolf Vollprecht Date: Mon, 13 Dec 2021 09:45:17 +0100 Subject: [PATCH 114/229] RoboStack CI: Pin tinyxml2 version (#2993) --- .github/robostack_env.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/robostack_env.yaml b/.github/robostack_env.yaml index e74f96701e..4e7c7259b7 100644 --- a/.github/robostack_env.yaml +++ b/.github/robostack_env.yaml @@ -16,3 +16,5 @@ dependencies: - orocos-kdl 1.5.0 - python-orocos-kdl 1.5.0 - openssl 1.1.1* + # we need the static library of this build + - tinyxml2 9.0.0 *_1 From 953f58d650cf070ce48c75eb029612fe4cce26e6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Tue, 14 Dec 2021 11:12:58 +0100 Subject: [PATCH 115/229] quietly use backward_cpp/ros if available (#2988) This is simply convenient and you always need it when you did not explicitly add it. Follow @tylerjw's initiative to add it to MoveIt2: https://github.com/ros-planning/moveit2/pull/794 --- moveit_core/cmake/moveit.cmake | 2 ++ 1 file changed, 2 insertions(+) diff --git a/moveit_core/cmake/moveit.cmake b/moveit_core/cmake/moveit.cmake index 831c218aef..8baa8f96aa 100644 --- a/moveit_core/cmake/moveit.cmake +++ b/moveit_core/cmake/moveit.cmake @@ -5,6 +5,8 @@ macro(moveit_build_options) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) + find_package(backward_ros QUIET) + if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) message("${PROJECT_NAME}: You did not request a specific build type: Choosing 'Release' for maximum performance") set(CMAKE_BUILD_TYPE Release) From a4e023b50c0813fa54be271a97d928e0f1433ab4 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 14 Dec 2021 12:06:43 +0100 Subject: [PATCH 116/229] add comment to mention loading of old scene files Followup to 9d78be4705fad661a9cd2ef90b0f9d933d60b325 --- MIGRATION.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MIGRATION.md b/MIGRATION.md index aaee781532..432fafe365 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -6,7 +6,7 @@ API changes in MoveIt releases - `CollisionObject` messages are now defined with a `Pose`, and shapes and subframes are defined relative to the object's pose. This makes it easier to place objects with subframes and multiple shapes in the scene. This causes several changes: - `getFrameTransform()` now returns this pose instead of the first shape's pose. - The Rviz plugin's manipulation tab now uses the object's pose instead of the shape pose to evaluate if object's are in the region of interest. - - Planning scene geometry text files (`.scene`) have changed format. Add a line `0 0 0 0 0 0 1` under each line with an asterisk to upgrade old files if required. + - Planning scene geometry text files (`.scene`) have changed format. Loading old files is still supported. You can add a line `0 0 0 0 0 0 1` under each line with an asterisk to upgrade old files. - add API for passing RNG to setToRandomPositionsNearBy - Static member variable interface of the CollisionDetectorAllocatorTemplate for the string NAME was replaced with a virtual method `getName`. - RobotModel no longer overrides empty URDF collision geometry by matching the visual geometry of the link. From 125d5e775f0b23334a2f9e1d222ebd38e64c4f33 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 15 Dec 2021 09:28:32 +0100 Subject: [PATCH 117/229] Provide MOVEIT_VERSION_CHECK macro (#2997) - Rename MOVEIT_VERSION -> MOVEIT_VERSION_STR - MOVEIT_VERSION becomes a numeric identifier - Use like: #if MOVEIT_VERSION >= MOVEIT_VERSION_CHECK(1, 0, 0) --- moveit_core/version/version.cpp | 2 +- moveit_core/version/version.h.in | 8 +++++++- moveit_ros/benchmarks/src/BenchmarkExecutor.cpp | 2 +- 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/moveit_core/version/version.cpp b/moveit_core/version/version.cpp index 75ce900180..c71fca5b81 100644 --- a/moveit_core/version/version.cpp +++ b/moveit_core/version/version.cpp @@ -39,6 +39,6 @@ int main(int /*argc*/, char** /*argv*/) { - printf("%s\n", MOVEIT_VERSION); + printf("%s\n", MOVEIT_VERSION_STR); return 0; } diff --git a/moveit_core/version/version.h.in b/moveit_core/version/version.h.in index 573a2e72b5..8e596d8f9f 100644 --- a/moveit_core/version/version.h.in +++ b/moveit_core/version/version.h.in @@ -35,11 +35,17 @@ #ifndef MOVEIT_VERSION_ #define MOVEIT_VERSION_ -#define MOVEIT_VERSION "@MOVEIT_VERSION@" +#define MOVEIT_VERSION_STR "@MOVEIT_VERSION@" #define MOVEIT_VERSION_MAJOR @MOVEIT_VERSION_MAJOR@ #define MOVEIT_VERSION_MINOR @MOVEIT_VERSION_MINOR@ #define MOVEIT_VERSION_PATCH @MOVEIT_VERSION_PATCH@ #define MOVEIT_VERSION_EXTRA "@MOVEIT_VERSION_EXTRA@" +/// MOVEIT_VERSION is (major << 16) + (minor << 8) + patch. +#define MOVEIT_VERSION MOVEIT_VERSION_CHECK(MOVEIT_VERSION_MAJOR, MOVEIT_VERSION_MINOR, MOVEIT_VERSION_PATCH) + +/// Use like: #if MOVEIT_VERSION >= MOVEIT_VERSION_CHECK(1, 0, 0) +#define MOVEIT_VERSION_CHECK(major, minor, patch) ((major << 16) | (minor << 8) | (patch)) + #endif diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 35e7e98a18..ddaf611c3c 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -1077,7 +1077,7 @@ void BenchmarkExecutor::writeOutput(const BenchmarkRequest& brequest, const std: return; } - out << "MoveIt version " << MOVEIT_VERSION << std::endl; + out << "MoveIt version " << MOVEIT_VERSION_STR << std::endl; out << "Experiment " << brequest.name << std::endl; out << "Running on " << hostname << std::endl; out << "Starting at " << start_time << std::endl; From 38b84dedb8d8da5678df744f8a12637fc4255956 Mon Sep 17 00:00:00 2001 From: Rick Staa Date: Wed, 15 Dec 2021 10:23:06 +0100 Subject: [PATCH 118/229] feat(simple_controller_manager): add `max_effort` parameter to GripperCommand action (#2984) This commit adds the `max_effort` parameter to the GripperCommand declaration in the `controller_list` (see issue #2956). This value is only used when effort is set in the requested gripper trajectory. Co-authored-by: Jafar Abdi --- .../gripper_controller_handle.h | 16 ++++++++++++++-- .../src/moveit_simple_controller_manager.cpp | 5 ++++- 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h index 212992de5b..ecdce0fdeb 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h @@ -51,10 +51,11 @@ class GripperControllerHandle : public ActionBasedControllerHandle(name, ns) , allow_failure_(false) , parallel_jaw_gripper_(false) + , max_effort_(max_effort) { } @@ -110,7 +111,6 @@ class GripperControllerHandle : public ActionBasedControllerHandle idx) + { goal.command.max_effort = trajectory.joint_trajectory.points[tpoint].effort[idx]; + } + else + { + goal.command.max_effort = max_effort_; + } } controller_action_client_->sendGoal( @@ -202,6 +208,12 @@ class GripperControllerHandle : public ActionBasedControllerHandle(name, action_ns); + const double max_effort = + controller_list[i].hasMember("max_effort") ? double(controller_list[i]["max_effort"]) : 0.0; + + new_handle = std::make_shared(name, action_ns, max_effort); if (static_cast(new_handle.get())->isConnected()) { if (controller_list[i].hasMember("parallel")) From 3471bc125695ebbbb96a4c862d6459d3edf1b229 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Wed, 15 Dec 2021 17:04:05 +0300 Subject: [PATCH 119/229] Remove all remaining usage of robot_model --- .../src/motion_planning_display.cpp | 3 +++ .../src/motion_planning_frame_joints_widget.cpp | 2 ++ .../rviz_plugin_render_tools/trajectory_visualization.h | 1 + .../src/trajectory_visualization.cpp | 6 ++++++ 4 files changed, 12 insertions(+) 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 9a9d65fecc..45359555fe 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 @@ -1145,11 +1145,14 @@ void MotionPlanningDisplay::clearRobotModel() // Invalidate all references to the RobotModel ... if (frame_) frame_->clearRobotModel(); + if (trajectory_visual_) + trajectory_visual_->clearRobotModel(); previous_state_.reset(); query_start_state_.reset(); query_goal_state_.reset(); kinematics_metrics_.reset(); robot_interaction_.reset(); + dynamics_solver_.clear(); // ... before calling the parent's method, which finally destroys the creating RobotModelLoader. PlanningSceneDisplay::clearRobotModel(); } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp index fdd7f0fe9f..7c9bf7e904 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp @@ -201,6 +201,8 @@ MotionPlanningFrameJointsWidget::~MotionPlanningFrameJointsWidget() void MotionPlanningFrameJointsWidget::clearRobotModel() { ui_->joints_view_->setModel(nullptr); + start_state_handler_.reset(); + goal_state_handler_.reset(); start_state_model_.reset(); goal_state_model_.reset(); } diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h index 6dad72927e..2ec0174d39 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h @@ -89,6 +89,7 @@ class TrajectoryVisualization : public QObject virtual void reset(); void onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context, const ros::NodeHandle& update_nh); + void clearRobotModel(); void onRobotModelLoaded(const moveit::core::RobotModelConstPtr& robot_model); void onEnable(); void onDisable(); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp index 854c872312..976ba5a6cf 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp @@ -605,4 +605,10 @@ void TrajectoryVisualization::trajectorySliderPanelVisibilityChange(bool enable) trajectory_slider_panel_->onDisable(); } +void TrajectoryVisualization::clearRobotModel() +{ + robot_model_.reset(); + robot_state_.reset(); +} + } // namespace moveit_rviz_plugin From 5ddb027492f16f714fb22d80e012372f1e262d9e Mon Sep 17 00:00:00 2001 From: Tomislav Bazina Date: Wed, 15 Dec 2021 17:15:47 +0100 Subject: [PATCH 120/229] round_collada_numbers.py: python 2/3 compatibility (#2983) Python3 requires the files to be opened in binary mode read a bytes object instead of a string, which is needed in turn by etree.parse(). Co-authored-by: Robert Haschke --- .../scripts/round_collada_numbers.py | 19 +++---------------- 1 file changed, 3 insertions(+), 16 deletions(-) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py b/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py index d72ab98cd8..a3d1bef186 100755 --- a/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py +++ b/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py @@ -44,7 +44,6 @@ from lxml import etree import shlex import sys -import io def doRound(values, decimal_places): @@ -82,19 +81,8 @@ def doRound(values, decimal_places): print("\nCollada Number Rounder") print("Rounding numbers to", decimal_places, "decimal places\n") - # Read string from file - f = open(input_file, "r") - xml = f.read() - - # Parse XML - # doc = etree.fromstring(xml) - # print(doc.tag) - # doc = etree.parse(io.BytesIO(xml)) - # element=doc.xpath('//ns:asset',namespaces={'ns','http://www.collada.org/2008/03/COLLADASchema'}) - # print(element) - namespace = "http://www.collada.org/2008/03/COLLADASchema" - dom = etree.parse(io.BytesIO(xml)) + dom = etree.parse(input_file) # find elements of particular name elements = dom.xpath("//ns:translate", namespaces={"ns": namespace}) @@ -122,6 +110,5 @@ def doRound(values, decimal_places): elements[i].text = doRound(elements[i].text, decimal_places) # save changes - f = open(output_file, "w") - f.write(etree.tostring(dom)) - f.close() + with open(output_file, "wb") as f: + dom.write(f, encoding="utf-8") From c32bbd2f0481fed32bd7fbba7f0fc8df7b6f46c6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Tue, 21 Dec 2021 10:51:39 +0100 Subject: [PATCH 121/229] Do not assert on printTransform with non-isometry (#3005) instead print a tag and the matrix building a Quaternion from non-isometries is undefined behavior in Eigen, thus the split. --- moveit_core/robot_state/src/robot_state.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 3d74b0a439..c870d945e8 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -2194,11 +2194,20 @@ void RobotState::printStateInfo(std::ostream& out) const void RobotState::printTransform(const Eigen::Isometry3d& transform, std::ostream& out) const { - ASSERT_ISOMETRY(transform) // unsanitized input, could contain a non-isometry - Eigen::Quaterniond q(transform.linear()); - out << "T.xyz = [" << transform.translation().x() << ", " << transform.translation().y() << ", " - << transform.translation().z() << "], Q.xyzw = [" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() - << "]" << std::endl; + if (checkIsometry(transform, CHECK_ISOMETRY_PRECISION, false)) + { + Eigen::Quaterniond q(transform.linear()); + out << "T.xyz = [" << transform.translation().x() << ", " << transform.translation().y() << ", " + << transform.translation().z() << "], Q.xyzw = [" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() + << "]"; + } + else + { + out << "[NON-ISOMETRY] " + << transform.matrix().format( + Eigen::IOFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "; ", "", "", "[", "]")); + } + out << std::endl; } void RobotState::printTransforms(std::ostream& out) const From 6a46aa0f0cdee6bda87fca8c0b93fd1d1d0adc6d Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 15 Dec 2021 11:28:53 +0100 Subject: [PATCH 122/229] add pilz planner to moveit_planners dependency It should have been there for quite some time now... --- moveit_planners/moveit_planners/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_planners/moveit_planners/package.xml b/moveit_planners/moveit_planners/package.xml index 4d366b59e1..84d8c332db 100644 --- a/moveit_planners/moveit_planners/package.xml +++ b/moveit_planners/moveit_planners/package.xml @@ -19,6 +19,7 @@ chomp_motion_planner moveit_planners_chomp moveit_planners_ompl + pilz_industrial_motion_planner From f6dffc43a505e6127cf465daceec3e193f368402 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 15 Dec 2021 11:28:24 +0100 Subject: [PATCH 123/229] pilz: report joint name with invalid limits in start state - remove unused utility method it does not provide enough feedback, is almost trivial and does redundant checks in the single case it's called from. --- .../joint_limits_container.h | 9 --------- .../src/joint_limits_container.cpp | 19 ------------------- .../src/trajectory_generator.cpp | 18 +++++++++++++++--- .../test/unittest_joint_limits_container.cpp | 19 ------------------- 4 files changed, 15 insertions(+), 50 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h index 7a91ffe545..5543dea2d4 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h @@ -140,15 +140,6 @@ class JointLimitsContainer */ bool verifyPositionLimit(const std::string& joint_name, const double& joint_position) const; - /** - * @brief verify position limits of multiple joints - * @param joint_names - * @param joint_positions - * @return - */ - bool verifyPositionLimits(const std::vector& joint_names, - const std::vector& joint_positions) const; - private: /** * @brief update the most strict limit with given joint limit diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp index 7bdb1d983f..d5a6a142e8 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp @@ -117,25 +117,6 @@ bool JointLimitsContainer::verifyPositionLimit(const std::string& joint_name, co (joint_position < getLimit(joint_name).min_position || joint_position > getLimit(joint_name).max_position))); } -bool JointLimitsContainer::verifyPositionLimits(const std::vector& joint_names, - const std::vector& joint_positions) const -{ - if (joint_names.size() != joint_positions.size()) - { - throw std::out_of_range("joint_names vector has a different size than joint_positions vector."); - } - - for (std::size_t i = 0; i < joint_names.size(); ++i) - { - if (!verifyPositionLimit(joint_names.at(i), joint_positions.at(i))) - { - return false; - } - } - - return true; -} - void JointLimitsContainer::updateCommonLimit(const JointLimit& joint_limit, JointLimit& common_limit) { // check position limits diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp index aebdfaf90d..4626eab04c 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -36,6 +36,8 @@ #include +#include + #include #include @@ -93,10 +95,20 @@ void TrajectoryGenerator::checkStartState(const moveit_msgs::RobotState& start_s throw SizeMismatchInStartState("Joint state name and position do not match in start state"); } - if (!planner_limits_.getJointLimitContainer().verifyPositionLimits(start_state.joint_state.name, - start_state.joint_state.position)) + // verify joint position limits + const JointLimitsContainer& limits{ planner_limits_.getJointLimitContainer() }; + std::string error_msg; + for (auto joint : boost::combine(start_state.joint_state.name, start_state.joint_state.position)) + { + if (!limits.verifyPositionLimit(joint.get<0>(), joint.get<1>())) + { + error_msg.append(error_msg.empty() ? "start state joints outside their position limits: " : ", "); + error_msg.append(joint.get<0>()); + } + } + if (!error_msg.empty()) { - throw JointsOfStartStateOutOfRange("Joint state out of range in start state"); + throw JointsOfStartStateOutOfRange(error_msg); } // does not allow start velocity diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_container.cpp index 2f7f7b80a4..b46e73d628 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_container.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_container.cpp @@ -203,25 +203,6 @@ TEST_F(JointLimitsContainerTest, FirstPositionEmpty) EXPECT_EQ(-1, limits.min_position); } -/** - * @brief Check position limits - */ -TEST_F(JointLimitsContainerTest, CheckVerifyPositionLimits) -{ - // positive check: inside limits - std::vector joint_names{ "joint1", "joint2" }; - std::vector joint_positions{ 0.5, 0.5 }; - EXPECT_TRUE(container_.verifyPositionLimits(joint_names, joint_positions)); - - // outside limit2 - joint_positions[1] = 7; - EXPECT_FALSE(container_.verifyPositionLimits(joint_names, joint_positions)); - - // invalid size - std::vector joint_positions1{ 0. }; - EXPECT_THROW(container_.verifyPositionLimits(joint_names, joint_positions1), std::out_of_range); -} - int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From 38395dce8f1b6ee7f7d3308d010cc01634d2e3d0 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 15 Dec 2021 12:39:59 +0100 Subject: [PATCH 124/229] pilz: restrict start state check to active group --- .../trajectory_generator.h | 7 +++- .../src/trajectory_generator.cpp | 34 ++++++++++++++++--- 2 files changed, 36 insertions(+), 5 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h index 4cb7d261ea..bf6b1598ed 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h @@ -223,7 +223,7 @@ class TrajectoryGenerator * - The start state velocity is below * TrajectoryGenerator::VELOCITY_TOLERANCE */ - void checkStartState(const moveit_msgs::RobotState& start_state) const; + void checkStartState(const moveit_msgs::RobotState& start_state, const std::string& group) const; void checkGoalConstraints(const moveit_msgs::MotionPlanRequest::_goal_constraints_type& goal_constraints, const std::vector& expected_joint_names, const std::string& group_name) const; @@ -235,6 +235,11 @@ class TrajectoryGenerator void checkCartesianGoalConstraint(const moveit_msgs::Constraints& constraint, const std::string& group_name) const; private: + /** + * @return joint state message including only active joints in group + */ + sensor_msgs::JointState filterGroupValues(const sensor_msgs::JointState& robot_state, const std::string& group) const; + /** * @return True if scaling factor is valid, otherwise false. */ diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp index 4626eab04c..5bc83360a6 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -45,6 +45,30 @@ namespace pilz_industrial_motion_planner { +sensor_msgs::JointState TrajectoryGenerator::filterGroupValues(const sensor_msgs::JointState& robot_state, + const std::string& group) const +{ + const std::vector& group_joints{ robot_model_->getJointModelGroup(group)->getActiveJointModelNames() }; + sensor_msgs::JointState group_state; + group_state.name.reserve(group_joints.size()); + group_state.position.reserve(group_joints.size()); + group_state.velocity.reserve(group_joints.size()); + + for (size_t i = 0; i < robot_state.name.size(); ++i) + { + if (std::find(group_joints.begin(), group_joints.end(), robot_state.name.at(i)) != group_joints.end()) + { + group_state.name.push_back(robot_state.name.at(i)); + group_state.position.push_back(robot_state.position.at(i)); + if (i < robot_state.velocity.size()) + { + group_state.velocity.push_back(robot_state.velocity.at(i)); + } + } + } + return group_state; +} + void TrajectoryGenerator::cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& /*req*/) const { // Empty implementation, in case the derived class does not want @@ -83,7 +107,7 @@ void TrajectoryGenerator::checkForValidGroupName(const std::string& group_name) } } -void TrajectoryGenerator::checkStartState(const moveit_msgs::RobotState& start_state) const +void TrajectoryGenerator::checkStartState(const moveit_msgs::RobotState& start_state, const std::string& group) const { if (start_state.joint_state.name.empty()) { @@ -95,10 +119,12 @@ void TrajectoryGenerator::checkStartState(const moveit_msgs::RobotState& start_s throw SizeMismatchInStartState("Joint state name and position do not match in start state"); } + sensor_msgs::JointState group_start_state{ filterGroupValues(start_state.joint_state, group) }; + // verify joint position limits const JointLimitsContainer& limits{ planner_limits_.getJointLimitContainer() }; std::string error_msg; - for (auto joint : boost::combine(start_state.joint_state.name, start_state.joint_state.position)) + for (auto joint : boost::combine(group_start_state.name, group_start_state.position)) { if (!limits.verifyPositionLimit(joint.get<0>(), joint.get<1>())) { @@ -112,7 +138,7 @@ void TrajectoryGenerator::checkStartState(const moveit_msgs::RobotState& start_s } // does not allow start velocity - if (!std::all_of(start_state.joint_state.velocity.begin(), start_state.joint_state.velocity.end(), + if (!std::all_of(group_start_state.velocity.begin(), group_start_state.velocity.end(), [this](double v) { return std::fabs(v) < this->VELOCITY_TOLERANCE; })) { throw NonZeroVelocityInStartState("Trajectory Generator does not allow non-zero start velocity"); @@ -222,7 +248,7 @@ void TrajectoryGenerator::validateRequest(const planning_interface::MotionPlanRe checkVelocityScaling(req.max_velocity_scaling_factor); checkAccelerationScaling(req.max_acceleration_scaling_factor); checkForValidGroupName(req.group_name); - checkStartState(req.start_state); + checkStartState(req.start_state, req.group_name); checkGoalConstraints(req.goal_constraints, req.start_state.joint_state.name, req.group_name); } From 7bb9d12bbd0dcbcca4dc442adb532fe71721d72d Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 11 Nov 2021 17:08:33 +0000 Subject: [PATCH 125/229] Add API stress tests for TOTG --- .../trajectory_processing/CMakeLists.txt | 2 +- ...est_time_optimal_trajectory_generation.cpp | 121 ++++++++++++++++++ 2 files changed, 122 insertions(+), 1 deletion(-) diff --git a/moveit_core/trajectory_processing/CMakeLists.txt b/moveit_core/trajectory_processing/CMakeLists.txt index e595724de0..3ff7fe4a16 100644 --- a/moveit_core/trajectory_processing/CMakeLists.txt +++ b/moveit_core/trajectory_processing/CMakeLists.txt @@ -22,5 +22,5 @@ if(CATKIN_ENABLE_TESTING) catkin_add_gtest(test_time_parameterization test/test_time_parameterization.cpp) target_link_libraries(test_time_parameterization moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) catkin_add_gtest(test_time_optimal_trajectory_generation test/test_time_optimal_trajectory_generation.cpp) - target_link_libraries(test_time_optimal_trajectory_generation ${catkin_LIBRARIES} ${console_bridge_LIBRARIES} ${MOVEIT_LIB_NAME}) + target_link_libraries(test_time_optimal_trajectory_generation moveit_test_utils ${catkin_LIBRARIES} ${console_bridge_LIBRARIES} ${MOVEIT_LIB_NAME}) endif() 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 7413afac44..a504f7d45f 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 @@ -38,8 +38,10 @@ #include #include +#include using trajectory_processing::Path; +using trajectory_processing::TimeOptimalTrajectoryGeneration; using trajectory_processing::Trajectory; TEST(time_optimal_trajectory_generation, test1) @@ -227,6 +229,125 @@ TEST(time_optimal_trajectory_generation, testLargeAccel) } } +TEST(time_optimal_trajectory_generation, testPluginAPI) +{ + constexpr auto robot_name{ "panda" }; + constexpr auto group_name{ "panda_arm" }; + + auto robot_model = moveit::core::loadTestingRobotModel(robot_name); + ASSERT_TRUE((bool)robot_model) << "Failed to load robot model" << robot_name; + auto group = robot_model->getJointModelGroup(group_name); + ASSERT_TRUE((bool)group) << "Failed to load joint model group " << group_name; + moveit::core::RobotState waypoint_state(robot_model); + waypoint_state.setToDefaultValues(); + + robot_trajectory::RobotTrajectory trajectory(robot_model, group); + waypoint_state.setJointGroupPositions(group, std::vector{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 }); + trajectory.addSuffixWayPoint(waypoint_state, 0.1); + waypoint_state.setJointGroupPositions(group, std::vector{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 }); + trajectory.addSuffixWayPoint(waypoint_state, 0.1); + waypoint_state.setJointGroupPositions(group, std::vector{ 0.0, -3.5, 1.4, -1.2, -1.0, -0.2, 0.0 }); + trajectory.addSuffixWayPoint(waypoint_state, 0.1); + waypoint_state.setJointGroupPositions(group, std::vector{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 }); + trajectory.addSuffixWayPoint(waypoint_state, 0.1); + waypoint_state.setJointGroupPositions(group, std::vector{ 0.0, -3.5, 1.4, -1.2, -1.0, -0.2, 0.0 }); + trajectory.addSuffixWayPoint(waypoint_state, 0.1); + waypoint_state.setJointGroupPositions(group, std::vector{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 }); + trajectory.addSuffixWayPoint(waypoint_state, 0.1); + + // Number TOTG iterations + constexpr size_t totg_iterations = 10; + + // Test computing the dynamics repeatedly with the same totg instance + moveit_msgs::RobotTrajectory first_trajectory_msg_start, first_trajectory_msg_end; + { + robot_trajectory::RobotTrajectory test_trajectory(trajectory, true /* deep copy */); + + // Test if the trajectory was copied correctly + ASSERT_EQ(test_trajectory.getDuration(), trajectory.getDuration()); + moveit::core::JointBoundsVector test_bounds = test_trajectory.getRobotModel()->getActiveJointModelsBounds(); + moveit::core::JointBoundsVector original_bounds = trajectory.getRobotModel()->getActiveJointModelsBounds(); + ASSERT_EQ(test_bounds.size(), original_bounds.size()); + for (size_t bound_idx = 0; bound_idx < test_bounds.at(0)->size(); ++bound_idx) + { + ASSERT_EQ(test_bounds.at(0)->at(bound_idx).max_velocity_, original_bounds.at(0)->at(bound_idx).max_velocity_); + ASSERT_EQ(test_bounds.at(0)->at(bound_idx).min_velocity_, original_bounds.at(0)->at(bound_idx).min_velocity_); + ASSERT_EQ(test_bounds.at(0)->at(bound_idx).velocity_bounded_, + original_bounds.at(0)->at(bound_idx).velocity_bounded_); + + ASSERT_EQ(test_bounds.at(0)->at(bound_idx).max_acceleration_, + original_bounds.at(0)->at(bound_idx).max_acceleration_); + ASSERT_EQ(test_bounds.at(0)->at(bound_idx).min_acceleration_, + original_bounds.at(0)->at(bound_idx).min_acceleration_); + ASSERT_EQ(test_bounds.at(0)->at(bound_idx).acceleration_bounded_, + original_bounds.at(0)->at(bound_idx).acceleration_bounded_); + } + ASSERT_EQ(test_trajectory.getWayPointDurationFromPrevious(1), trajectory.getWayPointDurationFromPrevious(1)); + + TimeOptimalTrajectoryGeneration totg; + ASSERT_TRUE(totg.computeTimeStamps(test_trajectory)) << "Failed to compute time stamps"; + + test_trajectory.getRobotTrajectoryMsg(first_trajectory_msg_start); + + // Iteratively recompute timestamps with same totg instance + for (size_t i = 0; i < totg_iterations; ++i) + { + bool totg_success = totg.computeTimeStamps(test_trajectory); + EXPECT_TRUE(totg_success) << "Failed to compute time stamps with a same TOTG instance in iteration " << i; + } + + test_trajectory.getRobotTrajectoryMsg(first_trajectory_msg_end); + } + + // Test computing the dynamics repeatedly with one TOTG instance per call + moveit_msgs::RobotTrajectory second_trajectory_msg_start, second_trajectory_msg_end; + { + robot_trajectory::RobotTrajectory test_trajectory(trajectory, true /* deep copy */); + + { + TimeOptimalTrajectoryGeneration totg; + ASSERT_TRUE(totg.computeTimeStamps(test_trajectory)) << "Failed to compute time stamps"; + } + + test_trajectory.getRobotTrajectoryMsg(second_trajectory_msg_start); + + // Iteratively recompute timestamps with new totg instances + for (size_t i = 0; i < totg_iterations; ++i) + { + TimeOptimalTrajectoryGeneration totg; + bool totg_success = totg.computeTimeStamps(test_trajectory); + EXPECT_TRUE(totg_success) << "Failed to compute time stamps with a new TOTG instance in iteration " << i; + } + + test_trajectory.getRobotTrajectoryMsg(second_trajectory_msg_end); + } + + // Make sure trajectories produce equal waypoints independent of TOTG instances + ASSERT_EQ(first_trajectory_msg_start, second_trajectory_msg_start); + ASSERT_EQ(first_trajectory_msg_end, second_trajectory_msg_end); + + // Iterate on the original trajectory again + moveit_msgs::RobotTrajectory third_trajectory_msg_end; + + { + TimeOptimalTrajectoryGeneration totg; + ASSERT_TRUE(totg.computeTimeStamps(trajectory)) << "Failed to compute time stamps"; + } + + for (size_t i = 0; i < totg_iterations; ++i) + { + TimeOptimalTrajectoryGeneration totg; + bool totg_success = totg.computeTimeStamps(trajectory); + ASSERT_TRUE(totg_success) << "Failed to compute timestamps on a new TOTG instance in iteration " << i; + } + + // Compare with previous work + trajectory.getRobotTrajectoryMsg(third_trajectory_msg_end); + + // Make sure trajectories produce equal waypoints independent of TOTG instances + ASSERT_EQ(first_trajectory_msg_end, third_trajectory_msg_end); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From 2827369b901425dbd2b0f99a6ec7621d9b204652 Mon Sep 17 00:00:00 2001 From: Jochen Sprickerhof Date: Wed, 17 Nov 2021 09:47:48 +0100 Subject: [PATCH 126/229] TOTG: catch division by 0 This bug is already in the original implementation: https://github.com/tobiaskunz/trajectories/blob/master/Path.cpp In case the dot product between the two vectors is close to +/-1, angle becomes +/-PI and cos/tan of 0.5 * PI in the lines below will produce a division by 0. This happens easily if a optimal trajectory is processed by TOTG, i.e., if a trajectory is processed by TOTG twice. --- .../src/time_optimal_trajectory_generation.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) 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 299d080905..15aff331cd 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -106,8 +106,10 @@ class CircularPathSegment : public PathSegment const Eigen::VectorXd start_direction = (intersection - start).normalized(); const Eigen::VectorXd end_direction = (end - intersection).normalized(); + const double start_dot_end = start_direction.dot(end_direction); - if ((start_direction - end_direction).norm() < 0.000001) + // catch division by 0 in computations below + if (start_dot_end > 0.999999 || start_dot_end < -0.999999) { length_ = 0.0; radius = 1.0; @@ -117,8 +119,7 @@ class CircularPathSegment : public PathSegment return; } - // directions must be different at this point so angle is always non-zero - const double angle = acos(std::max(-1.0, start_direction.dot(end_direction))); + const double angle = acos(start_dot_end); const double start_distance = (start - intersection).norm(); const double end_distance = (end - intersection).norm(); From a25515b73d682df03ed3eccd839110c296aa79fc Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Thu, 23 Dec 2021 11:56:30 -0500 Subject: [PATCH 127/229] Fix MoveGroupInterface uninitialized RobotState (#3008) --- .../move_group_interface/src/move_group_interface.cpp | 3 +++ 1 file changed, 3 insertions(+) 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 81db35e044..7017f4bb54 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 @@ -1597,7 +1597,10 @@ void MoveGroupInterface::setStartState(const moveit_msgs::RobotState& start_stat if (start_state.is_diff) impl_->getCurrentState(rs); else + { rs = std::make_shared(getRobotModel()); + rs->setToDefaultValues(); // initialize robot state values for conversion + } moveit::core::robotStateMsgToRobotState(start_state, *rs); setStartState(*rs); } From dffc178a754d5bae3b99a19e2b45c6104994df22 Mon Sep 17 00:00:00 2001 From: pvanlaar <22322118+pvanlaar@users.noreply.github.com> Date: Tue, 28 Dec 2021 08:46:24 +0100 Subject: [PATCH 128/229] RobotState::attachBody: Migrate to unique_ptr argument (#3011) ... to indicate transfer of ownership and simplify pointer handling --- MIGRATION.md | 1 + .../test/test_collision_distance_field.cpp | 10 +-- .../include/moveit/robot_state/robot_state.h | 24 +++-- moveit_core/robot_state/src/robot_state.cpp | 88 +++++++++---------- .../robot_state/test/robot_state_test.cpp | 8 +- .../test/test_kinematic_complex.cpp | 20 ++--- .../src/motion_planning_frame_objects.cpp | 9 +- 7 files changed, 81 insertions(+), 79 deletions(-) diff --git a/MIGRATION.md b/MIGRATION.md index 432fafe365..6d275e5871 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -28,6 +28,7 @@ API changes in MoveIt releases - Removed deprecated header `moveit/macros/deprecation.h`. Use `[[deprecated]]` instead. - All uses of `MOVEIT_CLASS_FORWARD` et. al. must now be followed by a semicolon for consistency (and to get -pedantic builds to pass for the codebase). - In case you start RViz in a namespace, the default topic for the trajectory visualization display now uses the relative instead of the absolute namespace (i.e. `/move_group/display_planned_path` instead of `/move_group/display_planned_path`). +- `RobotState::attachBody()` now takes a unique_ptr instead of an owning raw pointer. ## ROS Melodic diff --git a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp index 663911659c..991418cb2d 100644 --- a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp +++ b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp @@ -299,10 +299,9 @@ TEST_F(DistanceFieldCollisionDetectionTester, AttachedBodyTester) poses.push_back(identity); std::set touch_links; trajectory_msgs::JointTrajectory empty_state; - moveit::core::AttachedBody* attached_body = new moveit::core::AttachedBody( - robot_state.getLinkModel("r_gripper_palm_link"), "box", identity, shapes, poses, touch_links, empty_state); - robot_state.attachBody(attached_body); + robot_state.attachBody(std::make_unique( + robot_state.getLinkModel("r_gripper_palm_link"), "box", identity, shapes, poses, touch_links, empty_state)); res = collision_detection::CollisionResult(); cenv_->checkSelfCollision(req, res, robot_state, *acm_); @@ -314,9 +313,8 @@ TEST_F(DistanceFieldCollisionDetectionTester, AttachedBodyTester) touch_links.insert("r_gripper_palm_link"); shapes[0] = std::make_shared(.1, .1, .1); - moveit::core::AttachedBody* attached_body_1 = new moveit::core::AttachedBody( - robot_state.getLinkModel("r_gripper_palm_link"), "box", identity, shapes, poses, touch_links, empty_state); - robot_state.attachBody(attached_body_1); + robot_state.attachBody(std::make_unique( + robot_state.getLinkModel("r_gripper_palm_link"), "box", identity, shapes, poses, touch_links, empty_state)); res = collision_detection::CollisionResult(); cenv_->checkSelfCollision(req, res, robot_state, *acm_); diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index dd8859dbad..2998821eb0 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -1068,12 +1068,12 @@ class RobotState @param timeout The timeout passed to the kinematics solver on each attempt @param constraint A state validity constraint to be required for IK solutions */ bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, - const std::vector& tips, const std::vector >& consistency_limits, + const std::vector& tips, const std::vector>& consistency_limits, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); [[deprecated("The attempts argument is not supported anymore.")]] bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, - const std::vector& tips, const std::vector >& consistency_limits, + const std::vector& tips, const std::vector>& consistency_limits, unsigned int /* attempts */, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) @@ -1091,12 +1091,12 @@ class RobotState @param constraint A state validity constraint to be required for IK solutions */ bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, const std::vector& tips, - const std::vector >& consistency_limits, double timeout = 0.0, + const std::vector>& consistency_limits, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); [[deprecated("The attempts argument is not supported anymore.")]] bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, - const std::vector& tips, const std::vector >& consistency_limits, + const std::vector& tips, const std::vector>& consistency_limits, unsigned int /* attempts */, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) @@ -1628,6 +1628,18 @@ class RobotState * @{ */ + /** \brief Add an attached body to this state. + * + * This only adds the given body to this RobotState + * instance. It does not change anything about other + * representations of the object elsewhere in the system. So if the + * body represents an object in a collision_detection::World (like + * from a planning_scene::PlanningScene), you will likely need to remove the + * corresponding object from that world to avoid having collisions + * detected against it. + **/ + void attachBody(std::unique_ptr attached_body); + /** \brief Add an attached body to this state. Ownership of the * memory for the attached body is assumed by the state. * @@ -1647,7 +1659,7 @@ class RobotState * the body positions will get corrupted. You need to make a fresh * copy of the AttachedBody object for each RobotState you attach it * to.*/ - void attachBody(AttachedBody* attached_body); + [[deprecated("Deprecated. Pass a unique_ptr instead")]] void attachBody(AttachedBody* attached_body); /** @brief Add an attached body to a link * @param id The string id associated with the attached body @@ -1951,7 +1963,7 @@ class RobotState unsigned char* dirty_joint_transforms_; /** \brief All attached bodies that are part of this state, indexed by their name */ - std::map attached_body_map_; + std::map> attached_body_map_; /** \brief This event is called when there is a change in the attached bodies for this state; The event specifies the body that changed and whether it was just attached or about to be detached. */ diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index c870d945e8..8ac0e1561a 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -176,8 +176,8 @@ void RobotState::copyFrom(const RobotState& other) // copy attached bodies clearAttachedBodies(); - for (const std::pair& it : other.attached_body_map_) - attachBody(new AttachedBody(*it.second)); + for (const auto& attached_body : other.attached_body_map_) + attachBody(std::make_unique(*attached_body.second)); } bool RobotState::checkJointTransforms(const JointModel* joint) const @@ -740,9 +740,9 @@ void RobotState::updateLinkTransformsInternal(const JointModel* start) } // update attached bodies tf; these are usually very few, so we update them all - for (std::map::const_iterator it = attached_body_map_.begin(); - it != attached_body_map_.end(); ++it) - it->second->computeTransform(global_link_transforms_[it->second->getAttachedLink()->getLinkIndex()]); + for (const auto& attached_body : attached_body_map_) + attached_body.second->computeTransform( + global_link_transforms_[attached_body.second->getAttachedLink()->getLinkIndex()]); } void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isometry3d& transform, bool backward) @@ -792,9 +792,9 @@ void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isome } // update attached bodies tf; these are usually very few, so we update them all - for (std::map::const_iterator it = attached_body_map_.begin(); - it != attached_body_map_.end(); ++it) - it->second->computeTransform(global_link_transforms_[it->second->getAttachedLink()->getLinkIndex()]); + for (const auto& attached_body : attached_body_map_) + attached_body.second->computeTransform( + global_link_transforms_[attached_body.second->getAttachedLink()->getLinkIndex()]); } const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::string& frame) const @@ -978,25 +978,30 @@ bool RobotState::hasAttachedBody(const std::string& id) const const AttachedBody* RobotState::getAttachedBody(const std::string& id) const { - std::map::const_iterator it = attached_body_map_.find(id); + const auto it = attached_body_map_.find(id); if (it == attached_body_map_.end()) { ROS_ERROR_NAMED(LOGNAME, "Attached body '%s' not found", id.c_str()); return nullptr; } else - return it->second; + return it->second.get(); } -void RobotState::attachBody(AttachedBody* attached_body) +void RobotState::attachBody(std::unique_ptr attached_body) { // If an attached body with the same id exists, remove it clearAttachedBody(attached_body->getName()); - attached_body_map_[attached_body->getName()] = attached_body; attached_body->computeTransform(getGlobalLinkTransform(attached_body->getAttachedLink())); if (attached_body_update_callback_) - attached_body_update_callback_(attached_body, true); + attached_body_update_callback_(attached_body.get(), true); + attached_body_map_[attached_body->getName()] = std::move(attached_body); +} + +void RobotState::attachBody(AttachedBody* attached_body) +{ + attachBody(std::unique_ptr(attached_body)); } void RobotState::attachBody(const std::string& id, const Eigen::Isometry3d& pose, @@ -1005,90 +1010,81 @@ void RobotState::attachBody(const std::string& id, const Eigen::Isometry3d& pose const std::string& link, const trajectory_msgs::JointTrajectory& detach_posture, const moveit::core::FixedTransformsMap& subframe_poses) { - const LinkModel* l = robot_model_->getLinkModel(link); - attachBody(new AttachedBody(l, id, pose, shapes, shape_poses, touch_links, detach_posture, subframe_poses)); + attachBody(std::make_unique(robot_model_->getLinkModel(link), id, pose, shapes, shape_poses, + touch_links, detach_posture, subframe_poses)); } void RobotState::getAttachedBodies(std::vector& attached_bodies) const { attached_bodies.clear(); attached_bodies.reserve(attached_body_map_.size()); - for (const std::pair& it : attached_body_map_) - attached_bodies.push_back(it.second); + for (const auto& it : attached_body_map_) + attached_bodies.push_back(it.second.get()); } void RobotState::getAttachedBodies(std::vector& attached_bodies, const JointModelGroup* group) const { attached_bodies.clear(); - for (const std::pair& it : attached_body_map_) + for (const auto& it : attached_body_map_) if (group->hasLinkModel(it.second->getAttachedLinkName())) - attached_bodies.push_back(it.second); + attached_bodies.push_back(it.second.get()); } void RobotState::getAttachedBodies(std::vector& attached_bodies, const LinkModel* link_model) const { attached_bodies.clear(); - for (const std::pair& it : attached_body_map_) + for (const auto& it : attached_body_map_) if (it.second->getAttachedLink() == link_model) - attached_bodies.push_back(it.second); + attached_bodies.push_back(it.second.get()); } void RobotState::clearAttachedBodies() { - for (std::map::const_iterator it = attached_body_map_.begin(); - it != attached_body_map_.end(); ++it) + for (const auto& it : attached_body_map_) { if (attached_body_update_callback_) - attached_body_update_callback_(it->second, false); - delete it->second; + attached_body_update_callback_(it.second.get(), false); } attached_body_map_.clear(); } void RobotState::clearAttachedBodies(const LinkModel* link) { - std::map::iterator it = attached_body_map_.begin(); - while (it != attached_body_map_.end()) + for (auto it = attached_body_map_.cbegin(); it != attached_body_map_.cend(); ++it) { if (it->second->getAttachedLink() != link) { - ++it; continue; } if (attached_body_update_callback_) - attached_body_update_callback_(it->second, false); - delete it->second; - std::map::iterator del = it++; + attached_body_update_callback_(it->second.get(), false); + const auto del = it++; attached_body_map_.erase(del); } } void RobotState::clearAttachedBodies(const JointModelGroup* group) { - std::map::iterator it = attached_body_map_.begin(); - while (it != attached_body_map_.end()) + for (auto it = attached_body_map_.cbegin(); it != attached_body_map_.cend(); ++it) { if (!group->hasLinkModel(it->second->getAttachedLinkName())) { - ++it; continue; } if (attached_body_update_callback_) - attached_body_update_callback_(it->second, false); - delete it->second; - std::map::iterator del = it++; + attached_body_update_callback_(it->second.get(), false); + const auto del = it++; attached_body_map_.erase(del); } } bool RobotState::clearAttachedBody(const std::string& id) { - std::map::iterator it = attached_body_map_.find(id); + const auto it = attached_body_map_.find(id); if (it != attached_body_map_.end()) { if (attached_body_update_callback_) - attached_body_update_callback_(it->second, false); - delete it->second; + attached_body_update_callback_(it->second.get(), false); attached_body_map_.erase(it); return true; } @@ -1137,7 +1133,7 @@ const Eigen::Isometry3d& RobotState::getFrameInfo(const std::string& frame_id, c robot_link = nullptr; // Check names of the attached bodies - std::map::const_iterator jt = attached_body_map_.find(frame_id); + const auto jt = attached_body_map_.find(frame_id); if (jt != attached_body_map_.end()) { const Eigen::Isometry3d& transform = jt->second->getGlobalPose(); @@ -1148,7 +1144,7 @@ const Eigen::Isometry3d& RobotState::getFrameInfo(const std::string& frame_id, c } // Check if an AttachedBody has a subframe with name frame_id - for (const std::pair& body : attached_body_map_) + for (const auto& body : attached_body_map_) { const Eigen::Isometry3d& transform = body.second->getGlobalSubframeTransform(frame_id, &frame_found); if (frame_found) @@ -1172,12 +1168,12 @@ bool RobotState::knowsFrameTransform(const std::string& frame_id) const return true; // Check if an AttachedBody with name frame_id exists - std::map::const_iterator it = attached_body_map_.find(frame_id); + const auto it = attached_body_map_.find(frame_id); if (it != attached_body_map_.end()) return !it->second->getGlobalCollisionBodyTransforms().empty(); // Check if an AttachedBody has a subframe with name frame_id - for (const std::pair& body : attached_body_map_) + for (const auto& body : attached_body_map_) { if (body.second->hasSubframeTransform(frame_id)) return true; @@ -1212,7 +1208,7 @@ void RobotState::getRobotMarkers(visualization_msgs::MarkerArray& arr, const std if (!link_model) continue; if (include_attached) - for (const std::pair& it : attached_body_map_) + for (const auto& it : attached_body_map_) if (it.second->getAttachedLink() == link_model) { for (std::size_t j = 0; j < it.second->getShapes().size(); ++j) @@ -2055,7 +2051,7 @@ void RobotState::computeAABB(std::vector& aabb) const transform.translate(link->getCenteredBoundingBoxOffset()); bounding_box.extendWithTransformedBox(transform, extents); } - for (const std::pair& it : attached_body_map_) + for (const auto& it : attached_body_map_) { const EigenSTL::vector_Isometry3d& transforms = it.second->getGlobalCollisionBodyTransforms(); const std::vector& shapes = it.second->getShapes(); diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index 7313a94aed..f156275ce2 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -170,10 +170,8 @@ TEST(LoadingAndFK, SimpleRobot) state.setVariableAcceleration("base_joint/x", 0.0); - // making sure that values get copied - moveit::core::RobotState* new_state = new moveit::core::RobotState(state); + const auto new_state = std::make_unique(state); // making sure that values get copied EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(10, 8, 0)); - delete new_state; std::vector jv(state.getVariableCount(), 0.0); jv[state.getRobotModel()->getVariableIndex("base_joint/x")] = 5.0; @@ -364,7 +362,7 @@ class OneRobot : public testing::Test ""; urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2); - srdf::ModelSharedPtr srdf_model(new srdf::Model()); + srdf::ModelSharedPtr srdf_model = std::make_shared(); srdf_model->initString(*urdf_model, SMODEL2); robot_model_ = std::make_shared(urdf_model, srdf_model); } @@ -692,7 +690,7 @@ TEST_F(OneRobot, rigidlyConnectedParent) EXPECT_EQ(state.getRigidlyConnectedParentLinkModel("link_b"), link_a); // attach "object" with "subframe" to link_b - state.attachBody(new moveit::core::AttachedBody( + state.attachBody(std::make_unique( link_b, "object", Eigen::Isometry3d::Identity(), std::vector{}, EigenSTL::vector_Isometry3d{}, std::set{}, trajectory_msgs::JointTrajectory{}, moveit::core::FixedTransformsMap{ { "subframe", Eigen::Isometry3d::Identity() } })); diff --git a/moveit_core/robot_state/test/test_kinematic_complex.cpp b/moveit_core/robot_state/test/test_kinematic_complex.cpp index 4d8ba22afe..b95a1ccb57 100644 --- a/moveit_core/robot_state/test/test_kinematic_complex.cpp +++ b/moveit_core/robot_state/test/test_kinematic_complex.cpp @@ -243,9 +243,9 @@ TEST_F(LoadPlanningModelsPr2, FullTest) std::set touch_links; trajectory_msgs::JointTrajectory empty_state; - moveit::core::AttachedBody* attached_body = new moveit::core::AttachedBody( - robot_model->getLinkModel("r_gripper_palm_link"), "box", identity, shapes, poses, touch_links, empty_state); - ks.attachBody(attached_body); + + ks.attachBody(std::make_unique(robot_model->getLinkModel("r_gripper_palm_link"), "box", + identity, shapes, poses, touch_links, empty_state)); std::vector attached_bodies_1; ks.getAttachedBodies(attached_bodies_1); @@ -286,14 +286,12 @@ TEST_F(LoadPlanningModelsPr2, ObjectPoseAndSubframes) subframes["frame1"] = Eigen::Isometry3d(Eigen::Translation3d(0, 0, 1)); trajectory_msgs::JointTrajectory empty_state; - moveit::core::AttachedBody* attached_body_a = - new moveit::core::AttachedBody(robot_model->getLinkModel("r_gripper_palm_link"), "boxA", pose_a, shapes, poses, - touch_links, empty_state, subframes); - moveit::core::AttachedBody* attached_body_b = - new moveit::core::AttachedBody(robot_model->getLinkModel("r_gripper_palm_link"), "boxB", pose_b, shapes, poses, - touch_links, empty_state, subframes); - ks.attachBody(attached_body_a); - ks.attachBody(attached_body_b); + ks.attachBody(std::make_unique(robot_model->getLinkModel("r_gripper_palm_link"), "boxA", + pose_a, shapes, poses, touch_links, empty_state, + subframes)); + ks.attachBody(std::make_unique(robot_model->getLinkModel("r_gripper_palm_link"), "boxB", + pose_b, shapes, poses, touch_links, empty_state, + subframes)); // Check position of shape in each body Eigen::Isometry3d p; diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index d3a0e706dc..aeee8c5b2e 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -860,12 +860,11 @@ void MotionPlanningFrame::renameCollisionObject(QListWidgetItem* item) if (ab) { known_collision_objects_[item->type()].first = item_text; - moveit::core::AttachedBody* new_ab = - new moveit::core::AttachedBody(ab->getAttachedLink(), known_collision_objects_[item->type()].first, - ab->getPose(), ab->getShapes(), ab->getShapePoses(), ab->getTouchLinks(), - ab->getDetachPosture(), ab->getSubframes()); + auto new_ab = std::make_unique( + ab->getAttachedLink(), known_collision_objects_[item->type()].first, ab->getPose(), ab->getShapes(), + ab->getShapePoses(), ab->getTouchLinks(), ab->getDetachPosture(), ab->getSubframes()); cs.clearAttachedBody(ab->getName()); - cs.attachBody(new_ab); + cs.attachBody(std::move(new_ab)); } } setLocalSceneEdited(); From 872d33dd20f1b8e947549d6535d3d46c95450136 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 28 Dec 2021 17:05:26 +0100 Subject: [PATCH 129/229] Disable (flaky) timing tests in DEBUG mode (#3012) --- .../test_collision_common_pr2.h | 20 ++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h index 0deef007d7..9abeeae064 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h @@ -87,6 +87,12 @@ class CollisionDetectorTest : public ::testing::Test std::string kinect_dae_resource_; }; +#ifdef NDEBUG +#define EXPECT_TIME_LT(EXPR, VAL) EXPECT_LT(EXPR, VAL) +#else // Don't perform timing checks in Debug mode (but evaluate expression) +#define EXPECT_TIME_LT(EXPR, VAL) (void)(EXPR) +#endif + TYPED_TEST_CASE_P(CollisionDetectorTest); TYPED_TEST_P(CollisionDetectorTest, InitOK) @@ -361,7 +367,7 @@ TYPED_TEST_P(CollisionDetectorTest, DiffSceneTester) new_cenv->checkSelfCollision(req, res, robot_state); double second_check = (ros::WallTime::now() - before).toSec(); - EXPECT_LT(fabs(first_check - second_check), .05); + EXPECT_TIME_LT(fabs(first_check - second_check), .05); std::vector shapes; shapes.resize(1); @@ -382,7 +388,7 @@ TYPED_TEST_P(CollisionDetectorTest, DiffSceneTester) second_check = (ros::WallTime::now() - before).toSec(); // the first check is going to take a while, as data must be constructed - EXPECT_LT(second_check, .1); + EXPECT_TIME_LT(second_check, .1); collision_detection::CollisionEnvPtr other_new_cenv = this->value_->allocateEnv(this->cenv_, this->cenv_->getWorld()); before = ros::WallTime::now(); @@ -392,7 +398,7 @@ TYPED_TEST_P(CollisionDetectorTest, DiffSceneTester) new_cenv->checkSelfCollision(req, res, robot_state); second_check = (ros::WallTime::now() - before).toSec(); - EXPECT_LT(fabs(first_check - second_check), .05); + EXPECT_TIME_LT(fabs(first_check - second_check), .05); } TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached) @@ -418,7 +424,7 @@ TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached) this->cenv_->checkRobotCollision(req, res, robot_state); double second_check = (ros::WallTime::now() - before).toSec(); - EXPECT_LT(second_check, .05); + EXPECT_TIME_LT(second_check, .05); collision_detection::CollisionEnv::ObjectConstPtr object = this->cenv_->getWorld()->getObject("kinect"); this->cenv_->getWorld()->removeObject("kinect"); @@ -457,8 +463,8 @@ TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached) this->cenv_->checkSelfCollision(req, res, robot_state2, *this->acm_); second_check = (ros::WallTime::now() - before).toSec(); - EXPECT_LT(first_check, .05); - EXPECT_LT(fabs(first_check - second_check), .1); + EXPECT_TIME_LT(first_check, .05); + EXPECT_TIME_LT(fabs(first_check - second_check), .1); } TYPED_TEST_P(CollisionDetectorTest, TestCollisionMapAdditionSpeed) @@ -474,7 +480,7 @@ TYPED_TEST_P(CollisionDetectorTest, TestCollisionMapAdditionSpeed) this->cenv_->getWorld()->addToObject("map", shapes, poses); double t = (ros::WallTime::now() - start).toSec(); // TODO (j-petit): investigate why bullet collision checking is considerably slower here - EXPECT_GE(5.0, t); + EXPECT_TIME_LT(t, 5.0); // this is not really a failure; it is just that slow; // looking into doing collision checking with a voxel grid. ROS_INFO_NAMED("collision_detection.bullet", "Adding boxes took %g", t); From a47e2c70ae1f7ad4fd91a89441f8f94fa490146c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 29 Oct 2021 15:19:14 +0200 Subject: [PATCH 130/229] Pass xacro_args to both, urdf and srdf loading --- .../src/widgets/start_screen_widget.cpp | 12 ++++-------- .../src/widgets/start_screen_widget.h | 2 +- 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp index 404acc8943..c9d38a1142 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp @@ -395,7 +395,7 @@ bool StartScreenWidget::loadExistingFiles() QApplication::processEvents(); // Load the SRDF - if (!loadSRDFFile(config_data_->srdf_path_)) + if (!loadSRDFFile(config_data_->srdf_path_, config_data_->xacro_args_)) return false; // error occured // Progress Indicator @@ -546,9 +546,7 @@ bool StartScreenWidget::loadNewFiles() // ****************************************************************************************** bool StartScreenWidget::loadURDFFile(const std::string& urdf_file_path, const std::string& xacro_args) { - const std::vector vec_xacro_args = { xacro_args }; - - if (!rdf_loader::RDFLoader::loadXmlFileToString(config_data_->urdf_string_, urdf_file_path, vec_xacro_args)) + if (!rdf_loader::RDFLoader::loadXmlFileToString(config_data_->urdf_string_, urdf_file_path, { xacro_args })) { QMessageBox::warning(this, "Error Loading Files", QString("URDF/COLLADA file not found: ").append(urdf_file_path.c_str())); @@ -592,12 +590,10 @@ bool StartScreenWidget::loadURDFFile(const std::string& urdf_file_path, const st // ****************************************************************************************** // Load SRDF File to Parameter Server // ****************************************************************************************** -bool StartScreenWidget::loadSRDFFile(const std::string& srdf_file_path) +bool StartScreenWidget::loadSRDFFile(const std::string& srdf_file_path, const std::string& xacro_args) { - const std::vector xacro_args; - std::string srdf_string; - if (!rdf_loader::RDFLoader::loadXmlFileToString(srdf_string, srdf_file_path, xacro_args)) + if (!rdf_loader::RDFLoader::loadXmlFileToString(srdf_string, srdf_file_path, { xacro_args })) { QMessageBox::warning(this, "Error Loading Files", QString("SRDF file not found: ").append(srdf_file_path.c_str())); return false; diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.h b/moveit_setup_assistant/src/widgets/start_screen_widget.h index b7c28ddef9..af5b1eb320 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.h +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.h @@ -146,7 +146,7 @@ private Q_SLOTS: bool loadURDFFile(const std::string& urdf_file_path, const std::string& xacro_args); /// Load SRDF File - bool loadSRDFFile(const std::string& srdf_file_path); + bool loadSRDFFile(const std::string& srdf_file_path, const std::string& xacro_args); /// Put SRDF File on Parameter Server bool setSRDFFile(const std::string& srdf_string); From a334d8b6ab3fbd772b63d63e947a076b030860d0 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 29 Oct 2021 17:17:41 +0200 Subject: [PATCH 131/229] Modernize loops --- .../src/widgets/setup_assistant_widget.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp index 98bd321f37..0c64555566 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp @@ -448,11 +448,9 @@ void SetupAssistantWidget::highlightGroup(const std::string& group_name) config_data_->getRobotModel()->getJointModelGroup(group_name); if (joint_model_group) { - const std::vector& link_models = joint_model_group->getLinkModels(); // Iterate through the links - for (std::vector::const_iterator link_it = link_models.begin(); - link_it < link_models.end(); ++link_it) - highlightLink((*link_it)->getName(), QColor(255, 0, 0)); + for (const moveit::core::LinkModel* lm : joint_model_group->getLinkModels()) + highlightLink(lm->getName(), QColor(255, 0, 0)); } } @@ -477,12 +475,12 @@ void SetupAssistantWidget::unhighlightAll() } // Iterate through the links - for (std::vector::const_iterator link_it = links.begin(); link_it < links.end(); ++link_it) + for (const std::string& link : links) { - if ((*link_it).empty()) + if (link.empty()) continue; - robot_state_display_->unsetLinkColor(*link_it); + robot_state_display_->unsetLinkColor(link); } } From 1249c2b183e557d502a50c135fa327f55f26fedf Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Tue, 28 Dec 2021 19:24:27 +0300 Subject: [PATCH 132/229] Move MoveItErrorCode class to moveit_core (#3009) ... reducing code duplication and facilitating re-use --- MIGRATION.md | 1 + .../include/moveit/utils/moveit_error_code.h | 76 ++++++++++++++++ .../test/chomp_moveit_test.cpp | 20 ++--- .../moveit/moveit_cpp/planning_component.h | 32 +------ .../move_group_interface.h | 72 +++++---------- .../src/move_group_interface.cpp | 87 ++++++++++--------- .../src/wrap_python_move_group.cpp | 20 ++--- .../test/move_group_interface_cpp_test.cpp | 6 +- .../test/move_group_pick_place_test.cpp | 4 +- .../src/motion_planning_frame_planning.cpp | 6 +- 10 files changed, 176 insertions(+), 148 deletions(-) create mode 100644 moveit_core/utils/include/moveit/utils/moveit_error_code.h diff --git a/MIGRATION.md b/MIGRATION.md index 6d275e5871..d59340fc77 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -29,6 +29,7 @@ API changes in MoveIt releases - All uses of `MOVEIT_CLASS_FORWARD` et. al. must now be followed by a semicolon for consistency (and to get -pedantic builds to pass for the codebase). - In case you start RViz in a namespace, the default topic for the trajectory visualization display now uses the relative instead of the absolute namespace (i.e. `/move_group/display_planned_path` instead of `/move_group/display_planned_path`). - `RobotState::attachBody()` now takes a unique_ptr instead of an owning raw pointer. +- Moved the class `MoveItErrorCode` from both `moveit_ros_planning` and `moveit_ros_planning_interface` to `moveit_core`. The class now is in namespace `moveit::core`, access via `moveit::planning_interface` or `moveit_cpp::PlanningComponent` is deprecated. ## ROS Melodic diff --git a/moveit_core/utils/include/moveit/utils/moveit_error_code.h b/moveit_core/utils/include/moveit/utils/moveit_error_code.h new file mode 100644 index 0000000000..5e9aa4bb11 --- /dev/null +++ b/moveit_core/utils/include/moveit/utils/moveit_error_code.h @@ -0,0 +1,76 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Inc. + * 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 the copyright holder 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. + *********************************************************************/ + +#pragma once + +#include + +namespace moveit +{ +namespace core +{ +/** + * @brief a wrapper around moveit_msgs::MoveItErrorCodes to make it easier to return an error code message from a function + */ +class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes +{ +public: + MoveItErrorCode() + { + val = 0; + } + MoveItErrorCode(int code) + { + val = code; + } + MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code) + { + val = code.val; + } + explicit operator bool() const + { + return val == moveit_msgs::MoveItErrorCodes::SUCCESS; + } + bool operator==(const int c) const + { + return val == c; + } + bool operator!=(const int c) const + { + return val != c; + } +}; + +} // namespace core +} // namespace moveit diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp index 7ec6538be9..7ea3e9ba65 100644 --- a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp +++ b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp @@ -58,9 +58,9 @@ TEST_F(CHOMPMoveitTest, jointSpaceGoodGoal) move_group_.setStartState(*(move_group_.getCurrentState())); move_group_.setJointValueTarget(std::vector({ 1.0, 1.0 })); - moveit::planning_interface::MoveItErrorCode error_code = move_group_.plan(my_plan_); + moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_); EXPECT_GT(my_plan_.trajectory_.joint_trajectory.points.size(), 0u); - EXPECT_EQ(error_code.val, moveit::planning_interface::MoveItErrorCode::SUCCESS); + EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::SUCCESS); } TEST_F(CHOMPMoveitTest, jointSpaceBadGoal) @@ -69,8 +69,8 @@ TEST_F(CHOMPMoveitTest, jointSpaceBadGoal) // joint2 is limited to [-PI/2, PI/2] move_group_.setJointValueTarget(std::vector({ 100.0, 2 * M_PI / 3.0 })); - moveit::planning_interface::MoveItErrorCode error_code = move_group_.plan(my_plan_); - EXPECT_EQ(error_code.val, moveit::planning_interface::MoveItErrorCode::INVALID_ROBOT_STATE); + moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_); + EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::INVALID_ROBOT_STATE); } TEST_F(CHOMPMoveitTest, cartesianGoal) @@ -83,17 +83,17 @@ TEST_F(CHOMPMoveitTest, cartesianGoal) target_pose1.position.z = 10000.; move_group_.setPoseTarget(target_pose1); - moveit::planning_interface::MoveItErrorCode error_code = move_group_.plan(my_plan_); + moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_); // CHOMP doesn't support Cartesian-space goals at the moment - EXPECT_EQ(error_code.val, moveit::planning_interface::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS); + EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS); } TEST_F(CHOMPMoveitTest, noStartState) { move_group_.setJointValueTarget(std::vector({ 0.2, 0.2 })); - moveit::planning_interface::MoveItErrorCode error_code = move_group_.plan(my_plan_); - EXPECT_EQ(error_code.val, moveit::planning_interface::MoveItErrorCode::SUCCESS); + moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_); + EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::SUCCESS); } TEST_F(CHOMPMoveitTest, collisionAtEndOfPath) @@ -101,8 +101,8 @@ TEST_F(CHOMPMoveitTest, collisionAtEndOfPath) move_group_.setStartState(*(move_group_.getCurrentState())); move_group_.setJointValueTarget(std::vector({ M_PI / 2.0, 0 })); - moveit::planning_interface::MoveItErrorCode error_code = move_group_.plan(my_plan_); - EXPECT_EQ(error_code.val, moveit::planning_interface::MoveItErrorCode::INVALID_MOTION_PLAN); + moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_); + EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN); } int main(int argc, char** argv) diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index 14873f9011..0b5469472d 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -40,6 +40,7 @@ #include #include #include +#include namespace moveit_cpp { @@ -50,34 +51,7 @@ class PlanningComponent public: MOVEIT_STRUCT_FORWARD(PlanSolution); - class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes - { - public: - MoveItErrorCode() - { - val = 0; - } - MoveItErrorCode(int code) - { - val = code; - } - MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code) - { - val = code.val; - } - explicit operator bool() const - { - return val == moveit_msgs::MoveItErrorCodes::SUCCESS; - } - bool operator==(const int code) const - { - return val == code; - } - bool operator!=(const int code) const - { - return val != code; - } - }; + using MoveItErrorCode [[deprecated("Use moveit::core::MoveItErrorCode")]] = moveit::core::MoveItErrorCode; /// The representation of a plan solution struct PlanSolution @@ -89,7 +63,7 @@ class PlanningComponent robot_trajectory::RobotTrajectoryPtr trajectory; /// Reason why the plan failed - MoveItErrorCode error_code; + moveit::core::MoveItErrorCode error_code; explicit operator bool() const { 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 8838bcc411..9ffb162e82 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 @@ -39,6 +39,7 @@ #include #include +#include #include #include #include @@ -60,34 +61,7 @@ namespace moveit /** \brief Simple interface to MoveIt components */ namespace planning_interface { -class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes -{ -public: - MoveItErrorCode() - { - val = 0; - } - MoveItErrorCode(int code) - { - val = code; - } - MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code) - { - val = code.val; - } - explicit operator bool() const - { - return val == moveit_msgs::MoveItErrorCodes::SUCCESS; - } - bool operator==(const int c) const - { - return val == c; - } - bool operator!=(const int c) const - { - return val != c; - } -}; +using MoveItErrorCode [[deprecated("Use moveit::core::MoveItErrorCode")]] = moveit::core::MoveItErrorCode; MOVEIT_CLASS_FORWARD(MoveGroupInterface); // Defines MoveGroupInterfacePtr, ConstPtr, WeakPtr... etc @@ -719,7 +693,7 @@ class MoveGroupInterface /** \brief Plan and execute a trajectory that takes the group of joints declared in the constructor to the specified target. This call is not blocking (does not wait for the execution of the trajectory to complete). */ - MoveItErrorCode asyncMove(); + moveit::core::MoveItErrorCode asyncMove(); /** \brief Get the move_group action client used by the \e MoveGroupInterface. The client can be used for querying the execution state of the trajectory and abort trajectory execution @@ -730,24 +704,24 @@ class MoveGroupInterface target. This call is always blocking (waits for the execution of the trajectory to complete) and requires an asynchronous spinner to be started.*/ - MoveItErrorCode move(); + moveit::core::MoveItErrorCode move(); /** \brief Compute a motion plan that takes the group declared in the constructor from the current state to the specified target. No execution is performed. The resulting plan is stored in \e plan*/ - MoveItErrorCode plan(Plan& plan); + moveit::core::MoveItErrorCode plan(Plan& plan); /** \brief Given a \e plan, execute it without waiting for completion. */ - MoveItErrorCode asyncExecute(const Plan& plan); + moveit::core::MoveItErrorCode asyncExecute(const Plan& plan); /** \brief Given a \e robot trajectory, execute it without waiting for completion. */ - MoveItErrorCode asyncExecute(const moveit_msgs::RobotTrajectory& trajectory); + moveit::core::MoveItErrorCode asyncExecute(const moveit_msgs::RobotTrajectory& trajectory); /** \brief Given a \e plan, execute it while waiting for completion. */ - MoveItErrorCode execute(const Plan& plan); + moveit::core::MoveItErrorCode execute(const Plan& plan); /** \brief Given a \e robot trajectory, execute it while waiting for completion. */ - MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory); + moveit::core::MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory); /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the @@ -824,19 +798,20 @@ class MoveGroupInterface /** \brief Pick up an object This applies a number of hard-coded default grasps */ - MoveItErrorCode pick(const std::string& object, bool plan_only = false) + moveit::core::MoveItErrorCode pick(const std::string& object, bool plan_only = false) { return pick(constructPickupGoal(object, std::vector(), plan_only)); } /** \brief Pick up an object given a grasp pose */ - MoveItErrorCode pick(const std::string& object, const moveit_msgs::Grasp& grasp, bool plan_only = false) + moveit::core::MoveItErrorCode pick(const std::string& object, const moveit_msgs::Grasp& grasp, bool plan_only = false) { return pick(constructPickupGoal(object, { grasp }, plan_only)); } /** \brief Pick up an object given possible grasp poses */ - MoveItErrorCode pick(const std::string& object, std::vector grasps, bool plan_only = false) + moveit::core::MoveItErrorCode pick(const std::string& object, std::vector grasps, + bool plan_only = false) { return pick(constructPickupGoal(object, std::move(grasps), plan_only)); } @@ -845,40 +820,41 @@ class MoveGroupInterface Use as follows: first create the goal with constructPickupGoal(), then set \e possible_grasps and any other desired variable in the goal, and finally pass it on to this function */ - MoveItErrorCode pick(const moveit_msgs::PickupGoal& goal); + moveit::core::MoveItErrorCode pick(const moveit_msgs::PickupGoal& goal); /** \brief Pick up an object calls the external moveit_msgs::GraspPlanning service "plan_grasps" to compute possible grasps */ - MoveItErrorCode planGraspsAndPick(const std::string& object = "", bool plan_only = false); + moveit::core::MoveItErrorCode planGraspsAndPick(const std::string& object = "", bool plan_only = false); /** \brief Pick up an object calls the external moveit_msgs::GraspPlanning service "plan_grasps" to compute possible grasps */ - MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false); + moveit::core::MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false); /** \brief Place an object somewhere safe in the world (a safe location will be detected) */ - MoveItErrorCode place(const std::string& object, bool plan_only = false) + moveit::core::MoveItErrorCode place(const std::string& object, bool plan_only = false) { return place(constructPlaceGoal(object, std::vector(), plan_only)); } /** \brief Place an object at one of the specified possible locations */ - MoveItErrorCode place(const std::string& object, std::vector locations, - bool plan_only = false) + moveit::core::MoveItErrorCode place(const std::string& object, std::vector locations, + bool plan_only = false) { return place(constructPlaceGoal(object, std::move(locations), plan_only)); } /** \brief Place an object at one of the specified possible locations */ - MoveItErrorCode place(const std::string& object, const std::vector& poses, - bool plan_only = false) + moveit::core::MoveItErrorCode place(const std::string& object, const std::vector& poses, + bool plan_only = false) { return place(constructPlaceGoal(object, posesToPlaceLocations(poses), plan_only)); } /** \brief Place an object at one of the specified possible location */ - MoveItErrorCode place(const std::string& object, const geometry_msgs::PoseStamped& pose, bool plan_only = false) + moveit::core::MoveItErrorCode place(const std::string& object, const geometry_msgs::PoseStamped& pose, + bool plan_only = false) { return place(constructPlaceGoal(object, posesToPlaceLocations({ pose }), plan_only)); } @@ -887,7 +863,7 @@ class MoveGroupInterface Use as follows: first create the goal with constructPlaceGoal(), then set \e place_locations and any other desired variable in the goal, and finally pass it on to this function */ - MoveItErrorCode place(const moveit_msgs::PlaceGoal& goal); + moveit::core::MoveItErrorCode place(const moveit_msgs::PlaceGoal& goal); /** \brief Given the name of an object in the planning scene, make the object attached to a link of the robot. If no link name is 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 7017f4bb54..197dd966df 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 @@ -680,17 +680,17 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return locations; } - MoveItErrorCode place(const moveit_msgs::PlaceGoal& goal) + moveit::core::MoveItErrorCode place(const moveit_msgs::PlaceGoal& goal) { if (!place_action_client_) { ROS_ERROR_STREAM_NAMED(LOGNAME, "place action client not found"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + return moveit::core::MoveItErrorCode::FAILURE; } if (!place_action_client_->isServerConnected()) { ROS_WARN_STREAM_NAMED(LOGNAME, "place action server not connected"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE); + return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE; } place_action_client_->sendGoal(goal); @@ -701,27 +701,27 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } if (place_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { - return MoveItErrorCode(place_action_client_->getResult()->error_code); + return place_action_client_->getResult()->error_code; } else { ROS_WARN_STREAM_NAMED(LOGNAME, "Fail: " << place_action_client_->getState().toString() << ": " << place_action_client_->getState().getText()); - return MoveItErrorCode(place_action_client_->getResult()->error_code); + return place_action_client_->getResult()->error_code; } } - MoveItErrorCode pick(const moveit_msgs::PickupGoal& goal) + moveit::core::MoveItErrorCode pick(const moveit_msgs::PickupGoal& goal) { if (!pick_action_client_) { ROS_ERROR_STREAM_NAMED(LOGNAME, "pick action client not found"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + return moveit::core::MoveItErrorCode::FAILURE; } if (!pick_action_client_->isServerConnected()) { ROS_WARN_STREAM_NAMED(LOGNAME, "pick action server not connected"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE); + return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE; } pick_action_client_->sendGoal(goal); @@ -731,17 +731,17 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } if (pick_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { - return MoveItErrorCode(pick_action_client_->getResult()->error_code); + return pick_action_client_->getResult()->error_code; } else { ROS_WARN_STREAM_NAMED(LOGNAME, "Fail: " << pick_action_client_->getState().toString() << ": " << pick_action_client_->getState().getText()); - return MoveItErrorCode(pick_action_client_->getResult()->error_code); + return pick_action_client_->getResult()->error_code; } } - MoveItErrorCode planGraspsAndPick(const std::string& object, bool plan_only = false) + moveit::core::MoveItErrorCode planGraspsAndPick(const std::string& object, bool plan_only = false) { if (object.empty()) { @@ -755,13 +755,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { ROS_ERROR_STREAM_NAMED(LOGNAME, "Asked for grasps for the object '" << object << "', but the object could not be found"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME); + return moveit::core::MoveItErrorCode::INVALID_OBJECT_NAME; } return planGraspsAndPick(objects[object], plan_only); } - MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false) + moveit::core::MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false) { if (!plan_grasps_service_.exists()) { @@ -769,7 +769,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl << GRASP_PLANNING_SERVICE_NAME << "' is not available." " This has to be implemented and started separately."); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE); + return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE; } moveit_msgs::GraspPlanning::Request request; @@ -784,23 +784,23 @@ class MoveGroupInterface::MoveGroupInterfaceImpl response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS) { ROS_ERROR_NAMED(LOGNAME, "Grasp planning failed. Unable to pick."); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + return moveit::core::MoveItErrorCode::FAILURE; } return pick(constructPickupGoal(object.id, std::move(response.grasps), plan_only)); } - MoveItErrorCode plan(Plan& plan) + moveit::core::MoveItErrorCode plan(Plan& plan) { if (!move_action_client_) { ROS_ERROR_STREAM_NAMED(LOGNAME, "move action client not found"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + return moveit::core::MoveItErrorCode::FAILURE; } if (!move_action_client_->isServerConnected()) { ROS_WARN_STREAM_NAMED(LOGNAME, "move action server not connected"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE); + return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE; } moveit_msgs::MoveGroupGoal goal; @@ -821,27 +821,27 @@ class MoveGroupInterface::MoveGroupInterfaceImpl plan.trajectory_ = move_action_client_->getResult()->planned_trajectory; plan.start_state_ = move_action_client_->getResult()->trajectory_start; plan.planning_time_ = move_action_client_->getResult()->planning_time; - return MoveItErrorCode(move_action_client_->getResult()->error_code); + return move_action_client_->getResult()->error_code; } else { ROS_WARN_STREAM_NAMED(LOGNAME, "Fail: " << move_action_client_->getState().toString() << ": " << move_action_client_->getState().getText()); - return MoveItErrorCode(move_action_client_->getResult()->error_code); + return move_action_client_->getResult()->error_code; } } - MoveItErrorCode move(bool wait) + moveit::core::MoveItErrorCode move(bool wait) { if (!move_action_client_) { ROS_ERROR_STREAM_NAMED(LOGNAME, "move action client not found"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + return moveit::core::MoveItErrorCode::FAILURE; } if (!move_action_client_->isServerConnected()) { ROS_WARN_STREAM_NAMED(LOGNAME, "move action server not connected"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE); + return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE; } moveit_msgs::MoveGroupGoal goal; @@ -856,7 +856,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl move_action_client_->sendGoal(goal); if (!wait) { - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS); + return moveit::core::MoveItErrorCode::SUCCESS; } if (!move_action_client_->waitForResult()) @@ -866,27 +866,27 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { - return MoveItErrorCode(move_action_client_->getResult()->error_code); + return move_action_client_->getResult()->error_code; } else { ROS_INFO_STREAM_NAMED(LOGNAME, move_action_client_->getState().toString() << ": " << move_action_client_->getState().getText()); - return MoveItErrorCode(move_action_client_->getResult()->error_code); + return move_action_client_->getResult()->error_code; } } - MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory, bool wait) + moveit::core::MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory, bool wait) { if (!execute_action_client_) { ROS_ERROR_STREAM_NAMED(LOGNAME, "execute action client not found"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + return moveit::core::MoveItErrorCode::FAILURE; } if (!execute_action_client_->isServerConnected()) { ROS_WARN_STREAM_NAMED(LOGNAME, "execute action server not connected"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE); + return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE; } moveit_msgs::ExecuteTrajectoryGoal goal; @@ -895,7 +895,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl execute_action_client_->sendGoal(goal); if (!wait) { - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS); + return moveit::core::MoveItErrorCode::SUCCESS; } if (!execute_action_client_->waitForResult()) @@ -905,13 +905,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (execute_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { - return MoveItErrorCode(execute_action_client_->getResult()->error_code); + return execute_action_client_->getResult()->error_code; } else { ROS_INFO_STREAM_NAMED(LOGNAME, execute_action_client_->getState().toString() << ": " << execute_action_client_->getState().getText()); - return MoveItErrorCode(execute_action_client_->getResult()->error_code); + return execute_action_client_->getResult()->error_code; } } @@ -1479,7 +1479,7 @@ void MoveGroupInterface::setMaxAccelerationScalingFactor(double max_acceleration impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor); } -MoveItErrorCode MoveGroupInterface::asyncMove() +moveit::core::MoveItErrorCode MoveGroupInterface::asyncMove() { return impl_->move(false); } @@ -1489,32 +1489,32 @@ actionlib::SimpleActionClient& MoveGroupInterface: return impl_->getMoveGroupClient(); } -MoveItErrorCode MoveGroupInterface::move() +moveit::core::MoveItErrorCode MoveGroupInterface::move() { return impl_->move(true); } -MoveItErrorCode MoveGroupInterface::asyncExecute(const Plan& plan) +moveit::core::MoveItErrorCode MoveGroupInterface::asyncExecute(const Plan& plan) { return impl_->execute(plan.trajectory_, false); } -MoveItErrorCode MoveGroupInterface::asyncExecute(const moveit_msgs::RobotTrajectory& trajectory) +moveit::core::MoveItErrorCode MoveGroupInterface::asyncExecute(const moveit_msgs::RobotTrajectory& trajectory) { return impl_->execute(trajectory, false); } -MoveItErrorCode MoveGroupInterface::execute(const Plan& plan) +moveit::core::MoveItErrorCode MoveGroupInterface::execute(const Plan& plan) { return impl_->execute(plan.trajectory_, true); } -MoveItErrorCode MoveGroupInterface::execute(const moveit_msgs::RobotTrajectory& trajectory) +moveit::core::MoveItErrorCode MoveGroupInterface::execute(const moveit_msgs::RobotTrajectory& trajectory) { return impl_->execute(trajectory, true); } -MoveItErrorCode MoveGroupInterface::plan(Plan& plan) +moveit::core::MoveItErrorCode MoveGroupInterface::plan(Plan& plan) { return impl_->plan(plan); } @@ -1539,22 +1539,23 @@ MoveGroupInterface::posesToPlaceLocations(const std::vectorposesToPlaceLocations(poses); } -MoveItErrorCode MoveGroupInterface::pick(const moveit_msgs::PickupGoal& goal) +moveit::core::MoveItErrorCode MoveGroupInterface::pick(const moveit_msgs::PickupGoal& goal) { return impl_->pick(goal); } -MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const std::string& object, bool plan_only) +moveit::core::MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const std::string& object, bool plan_only) { return impl_->planGraspsAndPick(object, plan_only); } -MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only) +moveit::core::MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const moveit_msgs::CollisionObject& object, + bool plan_only) { return impl_->planGraspsAndPick(object, plan_only); } -MoveItErrorCode MoveGroupInterface::place(const moveit_msgs::PlaceGoal& goal) +moveit::core::MoveItErrorCode MoveGroupInterface::place(const moveit_msgs::PlaceGoal& goal) { return impl_->place(goal); } diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp index 445dc80ba9..34cfdbee27 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp @@ -263,7 +263,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer msg.header.frame_id = getPoseReferenceFrame(); msg.header.stamp = ros::Time::now(); GILReleaser gr; - return place(object_name, msg, plan_only) == MoveItErrorCode::SUCCESS; + return place(object_name, msg, plan_only) == moveit::core::MoveItErrorCode::SUCCESS; } bool placePoses(const std::string& object_name, const bp::list& poses_list, bool plan_only = false) @@ -273,7 +273,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer for (int i = 0; i < l; ++i) py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(poses_list[i]), poses[i]); GILReleaser gr; - return place(object_name, poses, plan_only) == MoveItErrorCode::SUCCESS; + return place(object_name, poses, plan_only) == moveit::core::MoveItErrorCode::SUCCESS; } bool placeLocation(const std::string& object_name, const py_bindings_tools::ByteString& location_str, @@ -282,7 +282,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer std::vector locations(1); py_bindings_tools::deserializeMsg(location_str, locations[0]); GILReleaser gr; - return place(object_name, std::move(locations), plan_only) == MoveItErrorCode::SUCCESS; + return place(object_name, std::move(locations), plan_only) == moveit::core::MoveItErrorCode::SUCCESS; } bool placeLocations(const std::string& object_name, const bp::list& location_list, bool plan_only = false) @@ -292,13 +292,13 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer for (int i = 0; i < l; ++i) py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(location_list[i]), locations[i]); GILReleaser gr; - return place(object_name, std::move(locations), plan_only) == MoveItErrorCode::SUCCESS; + return place(object_name, std::move(locations), plan_only) == moveit::core::MoveItErrorCode::SUCCESS; } bool placeAnywhere(const std::string& object_name, bool plan_only = false) { GILReleaser gr; - return place(object_name, plan_only) == MoveItErrorCode::SUCCESS; + return place(object_name, plan_only) == moveit::core::MoveItErrorCode::SUCCESS; } void convertListToArrayOfPoses(const bp::list& poses, std::vector& msg) @@ -441,12 +441,12 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer bool movePython() { GILReleaser gr; - return move() == MoveItErrorCode::SUCCESS; + return move() == moveit::core::MoveItErrorCode::SUCCESS; } bool asyncMovePython() { - return asyncMove() == MoveItErrorCode::SUCCESS; + return asyncMove() == moveit::core::MoveItErrorCode::SUCCESS; } bool attachObjectPython(const std::string& object_name, const std::string& link_name, const bp::list& touch_links) @@ -459,14 +459,14 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer MoveGroupInterface::Plan plan; py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_); GILReleaser gr; - return execute(plan) == MoveItErrorCode::SUCCESS; + return execute(plan) == moveit::core::MoveItErrorCode::SUCCESS; } bool asyncExecutePython(const py_bindings_tools::ByteString& plan_str) { MoveGroupInterface::Plan plan; py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_); - return asyncExecute(plan) == MoveItErrorCode::SUCCESS; + return asyncExecute(plan) == moveit::core::MoveItErrorCode::SUCCESS; } bp::tuple planPython() @@ -668,7 +668,7 @@ static void wrap_move_group_interface() move_group_interface_class.def("move", &MoveGroupInterfaceWrapper::movePython); move_group_interface_class.def("execute", &MoveGroupInterfaceWrapper::executePython); move_group_interface_class.def("async_execute", &MoveGroupInterfaceWrapper::asyncExecutePython); - moveit::planning_interface::MoveItErrorCode (MoveGroupInterfaceWrapper::*pick_1)(const std::string&, bool) = + moveit::core::MoveItErrorCode (MoveGroupInterfaceWrapper::*pick_1)(const std::string&, bool) = &MoveGroupInterfaceWrapper::pick; move_group_interface_class.def("pick", pick_1); move_group_interface_class.def("pick", &MoveGroupInterfaceWrapper::pickGrasp); diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp index b481aa609b..ed673425ef 100644 --- a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp +++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp @@ -126,8 +126,8 @@ class MoveGroupTestFixture : public ::testing::Test { SCOPED_TRACE("planAndMove"); moveit::planning_interface::MoveGroupInterface::Plan my_plan; - ASSERT_EQ(move_group_->plan(my_plan), moveit::planning_interface::MoveItErrorCode::SUCCESS); - ASSERT_EQ(move_group_->move(), moveit::planning_interface::MoveItErrorCode::SUCCESS); + ASSERT_EQ(move_group_->plan(my_plan), moveit::core::MoveItErrorCode::SUCCESS); + ASSERT_EQ(move_group_->move(), moveit::core::MoveItErrorCode::SUCCESS); } void testEigenPose(const Eigen::Isometry3d& expected, const Eigen::Isometry3d& actual) @@ -318,7 +318,7 @@ TEST_F(MoveGroupTestFixture, CartPathTest) ASSERT_GE(EPSILON + move_group_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory), 1.0); // Execute trajectory - EXPECT_EQ(move_group_->execute(trajectory), moveit::planning_interface::MoveItErrorCode::SUCCESS); + EXPECT_EQ(move_group_->execute(trajectory), moveit::core::MoveItErrorCode::SUCCESS); // get the pose after the movement testPose(target_waypoint); diff --git a/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp b/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp index 4b2d906448..d0814f7037 100644 --- a/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp +++ b/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp @@ -219,7 +219,7 @@ TEST_F(PickPlaceTestFixture, PickPlaceTest) // Set support surface as table1. move_group_->setSupportSurfaceName("table1"); // Call pick to pick up the object using the grasps given - ASSERT_EQ(move_group_->pick("object", grasps), moveit::planning_interface::MoveItErrorCode::SUCCESS); + ASSERT_EQ(move_group_->pick("object", grasps), moveit::core::MoveItErrorCode::SUCCESS); // Ideally, you would create a vector of place locations to be attempted although in this example, we only create // a single place location. @@ -274,7 +274,7 @@ TEST_F(PickPlaceTestFixture, PickPlaceTest) // Set support surface as table2. move_group_->setSupportSurfaceName("table2"); // Call place to place the object using the place locations given. - ASSERT_EQ(move_group_->place("object", place_location), moveit::planning_interface::MoveItErrorCode::SUCCESS); + ASSERT_EQ(move_group_->place("object", place_location), moveit::core::MoveItErrorCode::SUCCESS); } int main(int argc, char** argv) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index c8767bef3a..272873b053 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -162,7 +162,7 @@ bool MotionPlanningFrame::computeCartesianPlan() bool MotionPlanningFrame::computeJointSpacePlan() { current_plan_ = std::make_shared(); - return move_group_->plan(*current_plan_) == moveit::planning_interface::MoveItErrorCode::SUCCESS; + return move_group_->plan(*current_plan_) == moveit::core::MoveItErrorCode::SUCCESS; } void MotionPlanningFrame::computePlanButtonClicked() @@ -199,7 +199,7 @@ void MotionPlanningFrame::computeExecuteButtonClicked() if (mgi && current_plan_) { ui_->stop_button->setEnabled(true); // enable stopping - bool success = mgi->execute(*current_plan_) == moveit::planning_interface::MoveItErrorCode::SUCCESS; + bool success = mgi->execute(*current_plan_) == moveit::core::MoveItErrorCode::SUCCESS; onFinishedExecution(success); } } @@ -221,7 +221,7 @@ void MotionPlanningFrame::computePlanAndExecuteButtonClicked() } else { - bool success = move_group_->move() == moveit::planning_interface::MoveItErrorCode::SUCCESS; + bool success = move_group_->move() == moveit::core::MoveItErrorCode::SUCCESS; onFinishedExecution(success); } ui_->plan_and_execute_button->setEnabled(true); From a956b2ae523c4618943d4bd9f0818b15320d35f6 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 30 Oct 2021 00:03:15 +0200 Subject: [PATCH 133/229] Adapt to API changes in srdfdom --- .../test_collision_common_panda.h | 8 +++-- ...t_bullet_continuous_collision_checking.cpp | 8 +++-- .../planning_scene/src/planning_scene.cpp | 8 +++-- .../src/collisions_updater.cpp | 2 +- .../src/tools/moveit_config_data.cpp | 36 +++++++++---------- .../widgets/configuration_files_widget.cpp | 2 +- .../src/widgets/default_collisions_widget.cpp | 11 +++--- 7 files changed, 41 insertions(+), 34 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h index ca840ac763..84ec2c9897 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h @@ -83,10 +83,12 @@ class CollisionDetectorPandaTest : public testing::Test const std::vector& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry(); acm_->setEntry(collision_links, collision_links, false); + // load collision defaults + for (const std::string& name : robot_model_->getSRDF()->getNoDefaultCollisionLinks()) + acm_->setDefaultEntry(name, collision_detection::AllowedCollision::NEVER); // allow collisions for pairs that have been disabled - const std::vector& dc = robot_model_->getSRDF()->getDisabledCollisionPairs(); - for (const srdf::Model::DisabledCollision& it : dc) - acm_->setEntry(it.link1_, it.link2_, true); + for (auto const& collision : robot_model_->getSRDF()->getCollisionPairs()) + acm_->setEntry(collision.link1_, collision.link2_, collision.disabled_); cenv_ = value_->allocateEnv(robot_model_); diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp index 0334fa531f..0d72ad7b6a 100644 --- a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp +++ b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp @@ -81,10 +81,12 @@ class BulletCollisionDetectionTester : public testing::Test const std::vector& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry(); acm_->setEntry(collision_links, collision_links, false); + // load collision defaults + for (const std::string& name : robot_model_->getSRDF()->getNoDefaultCollisionLinks()) + acm_->setDefaultEntry(name, collision_detection::AllowedCollision::NEVER); // allow collisions for pairs that have been disabled - const std::vector& dc = robot_model_->getSRDF()->getDisabledCollisionPairs(); - for (const srdf::Model::DisabledCollision& it : dc) - acm_->setEntry(it.link1_, it.link2_, true); + for (auto const& collision : robot_model_->getSRDF()->getCollisionPairs()) + acm_->setEntry(collision.link1_, collision.link2_, collision.disabled_); cenv_ = std::make_shared(robot_model_); diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 96db06e223..2daac721cb 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -156,10 +156,12 @@ void PlanningScene::initialize() const std::vector& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry(); acm_->setEntry(collision_links, collision_links, false); + // load collision defaults + for (const std::string& name : getRobotModel()->getSRDF()->getNoDefaultCollisionLinks()) + acm_->setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS); // allow collisions for pairs that have been disabled - const std::vector& dc = getRobotModel()->getSRDF()->getDisabledCollisionPairs(); - for (const srdf::Model::DisabledCollision& it : dc) - acm_->setEntry(it.link1_, it.link2_, true); + for (auto const& collision : getRobotModel()->getSRDF()->getCollisionPairs()) + acm_->setEntry(collision.link1_, collision.link2_, collision.disabled_); setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create()); } diff --git a/moveit_setup_assistant/src/collisions_updater.cpp b/moveit_setup_assistant/src/collisions_updater.cpp index 89b418e96e..b79f5cbd0e 100644 --- a/moveit_setup_assistant/src/collisions_updater.cpp +++ b/moveit_setup_assistant/src/collisions_updater.cpp @@ -97,7 +97,7 @@ bool setup(moveit_setup_assistant::MoveItConfigData& config_data, bool keep_old, } if (!keep_old) - config_data.srdf_->disabled_collisions_.clear(); + config_data.srdf_->collision_pairs_.clear(); return true; } diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 4535622c36..3f8d65e4dd 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -144,9 +144,9 @@ void MoveItConfigData::loadAllowedCollisionMatrix() allowed_collision_matrix_.clear(); // Update the allowed collision matrix, in case there has been a change - for (const auto& disabled_collision : srdf_->disabled_collisions_) + for (const auto& collision : srdf_->collision_pairs_) { - allowed_collision_matrix_.setEntry(disabled_collision.link1_, disabled_collision.link2_, true); + allowed_collision_matrix_.setEntry(collision.link1_, collision.link2_, collision.disabled_); } } @@ -1204,36 +1204,36 @@ bool MoveItConfigData::outputJointLimitsYAML(const std::string& file_path) // Set list of collision link pairs in SRDF; sorted; with optional filter // ****************************************************************************************** -class SortableDisabledCollision +class SortableCollisionPair { public: - SortableDisabledCollision(const srdf::Model::DisabledCollision& dc) - : dc_(dc), key_(dc.link1_ < dc.link2_ ? (dc.link1_ + "|" + dc.link2_) : (dc.link2_ + "|" + dc.link1_)) + SortableCollisionPair(const srdf::Model::CollisionPair& p) + : pair_(p), key_(p.link1_ < p.link2_ ? (p.link1_ + "|" + p.link2_) : (p.link2_ + "|" + p.link1_)) { } - operator const srdf::Model::DisabledCollision &() const + operator const srdf::Model::CollisionPair &() const { - return dc_; + return pair_; } - bool operator<(const SortableDisabledCollision& other) const + bool operator<(const SortableCollisionPair& other) const { return key_ < other.key_; } private: - const srdf::Model::DisabledCollision dc_; + const srdf::Model::CollisionPair pair_; const std::string key_; }; void MoveItConfigData::setCollisionLinkPairs(const moveit_setup_assistant::LinkPairMap& link_pairs, size_t skip_mask) { // Create temp disabled collision - srdf::Model::DisabledCollision dc; + srdf::Model::CollisionPair pair; - std::set disabled_collisions; - disabled_collisions.insert(srdf_->disabled_collisions_.begin(), srdf_->disabled_collisions_.end()); + std::set collision_pairs; + collision_pairs.insert(srdf_->collision_pairs_.begin(), srdf_->collision_pairs_.end()); - // copy the data in this class's LinkPairMap datastructure to srdf::Model::DisabledCollision format + // copy the data in this class's LinkPairMap datastructure to srdf::Model::CollisionPair format for (const std::pair, LinkPairData>& link_pair : link_pairs) { // Only copy those that are actually disabled @@ -1242,15 +1242,15 @@ void MoveItConfigData::setCollisionLinkPairs(const moveit_setup_assistant::LinkP if ((1 << link_pair.second.reason) & skip_mask) continue; - dc.link1_ = link_pair.first.first; - dc.link2_ = link_pair.first.second; - dc.reason_ = moveit_setup_assistant::disabledReasonToString(link_pair.second.reason); + pair.link1_ = link_pair.first.first; + pair.link2_ = link_pair.first.second; + pair.reason_ = moveit_setup_assistant::disabledReasonToString(link_pair.second.reason); - disabled_collisions.insert(SortableDisabledCollision(dc)); + collision_pairs.insert(SortableCollisionPair(pair)); } } - srdf_->disabled_collisions_.assign(disabled_collisions.begin(), disabled_collisions.end()); + srdf_->collision_pairs_.assign(collision_pairs.begin(), collision_pairs.end()); } // ****************************************************************************************** diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 2685d6d1d8..39f23c2e8d 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -641,7 +641,7 @@ bool ConfigurationFilesWidget::checkDependencies() } // Check that at least 1 link pair is disabled from collision checking - if (config_data_->srdf_->disabled_collisions_.empty()) + if (config_data_->srdf_->collision_pairs_.empty()) { dependencies << "No self-collisions have been disabled"; } diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp index cb548414e4..3d80e6efa7 100644 --- a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp +++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp @@ -677,12 +677,12 @@ void DefaultCollisionsWidget::checkedFilterChanged() void DefaultCollisionsWidget::linkPairsToSRDF() { // reset the data in the SRDF Writer class - config_data_->srdf_->disabled_collisions_.clear(); + config_data_->srdf_->collision_pairs_.clear(); // Create temp disabled collision - srdf::Model::DisabledCollision dc; + srdf::Model::CollisionPair dc; - // copy the data in this class's LinkPairMap datastructure to srdf::Model::DisabledCollision format + // copy the data in this class's LinkPairMap datastructure to srdf::Model::CollisionPair format for (moveit_setup_assistant::LinkPairMap::const_iterator pair_it = link_pairs_.begin(); pair_it != link_pairs_.end(); ++pair_it) { @@ -692,7 +692,8 @@ void DefaultCollisionsWidget::linkPairsToSRDF() dc.link1_ = pair_it->first.first; dc.link2_ = pair_it->first.second; dc.reason_ = moveit_setup_assistant::disabledReasonToString(pair_it->second.reason); - config_data_->srdf_->disabled_collisions_.push_back(dc); + dc.disabled_ = pair_it->second.disable_check; + config_data_->srdf_->collision_pairs_.push_back(dc); } } @@ -719,7 +720,7 @@ void DefaultCollisionsWidget::linkPairsFromSRDF() std::pair link_pair; // Loop through all disabled collisions in SRDF and update the comprehensive list that has already been created - for (const auto& disabled_collision : config_data_->srdf_->disabled_collisions_) + for (const auto& disabled_collision : config_data_->srdf_->collision_pairs_) { // Set the link names link_pair.first = disabled_collision.link1_; From 2c9d26b58543b7e1b8a769ab7423c3e75bb6e1c9 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 30 Oct 2021 00:05:24 +0200 Subject: [PATCH 134/229] Don't fill all ACM entries by default --- .../moveit/collision_detection/test_collision_common_panda.h | 3 --- .../test/test_bullet_continuous_collision_checking.cpp | 3 --- moveit_core/planning_scene/src/planning_scene.cpp | 3 --- 3 files changed, 9 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h index 84ec2c9897..9b0bf7097f 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h @@ -79,9 +79,6 @@ class CollisionDetectorPandaTest : public testing::Test robot_model_ok_ = static_cast(robot_model_); acm_ = std::make_shared(); - // Use default collision operations in the SRDF to setup the acm - const std::vector& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry(); - acm_->setEntry(collision_links, collision_links, false); // load collision defaults for (const std::string& name : robot_model_->getSRDF()->getNoDefaultCollisionLinks()) diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp index 0d72ad7b6a..06c6300f14 100644 --- a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp +++ b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp @@ -77,9 +77,6 @@ class BulletCollisionDetectionTester : public testing::Test robot_model_ok_ = static_cast(robot_model_); acm_ = std::make_shared(); - // Use default collision operations in the SRDF to setup the acm - const std::vector& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry(); - acm_->setEntry(collision_links, collision_links, false); // load collision defaults for (const std::string& name : robot_model_->getSRDF()->getNoDefaultCollisionLinks()) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 2daac721cb..6180b1d3ae 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -152,9 +152,6 @@ void PlanningScene::initialize() robot_state_->update(); acm_ = std::make_shared(); - // Use default collision operations in the SRDF to setup the acm - const std::vector& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry(); - acm_->setEntry(collision_links, collision_links, false); // load collision defaults for (const std::string& name : getRobotModel()->getSRDF()->getNoDefaultCollisionLinks()) From 9e268e79353ac639c5c55340d15a2b283a763821 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 30 Oct 2021 17:31:56 +0200 Subject: [PATCH 135/229] Improve formatting of comments --- .../collision_detection/collision_matrix.h | 53 ++++++++----------- 1 file changed, 23 insertions(+), 30 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h index 4b7a72910b..234ee5d7ff 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h @@ -94,37 +94,36 @@ class AllowedCollisionMatrix /** @brief Copy constructor */ AllowedCollisionMatrix(const AllowedCollisionMatrix& acm) = default; - /** @brief Get the type of the allowed collision between two elements. Return true if the entry is included in the - * collision matrix. - * Return false if the entry is not found. + /** @brief Get the type of the allowed collision between two elements. + * Return true if the entry is included in the collision matrix. Return false if the entry is not found. * @param name1 name of first element * @param name2 name of second element * @param allowed_collision_type The allowed collision type will be filled here */ bool getEntry(const std::string& name1, const std::string& name2, AllowedCollision::Type& allowed_collision_type) const; - /** @brief Get the allowed collision predicate between two elements. Return true if a predicate for entry is included - * in the collision matrix - * (if the type is AllowedCollision::CONDITIONAL). Return false if the entry is not found. + /** @brief Get the allowed collision predicate between two elements. + * Return true if a predicate for this entry is available in the collision matrix. + * Return false if the entry is not found. * @param name1 name of first element * @param name2 name of second element * @param fn A callback function that is used to decide if collisions are allowed between the two elements is filled * here */ bool getEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn) const; - /** @brief Check if the allowed collision matrix has an entry at all for an element. Returns true if the element is - * included. + /** @brief Check if the allowed collision matrix has an entry at all for an element. + * Returns true if the element is included. * @param name name of the element */ bool hasEntry(const std::string& name) const; - /** @brief Check if the allowed collision matrix has an entry for a pair of elements. Returns true if the pair is - * included. + /** @brief Check if the allowed collision matrix has an entry for a pair of elements. + * Returns true if the pair is included. * @param name1 name of first element * @param name2 name of second element*/ bool hasEntry(const std::string& name1, const std::string& name2) const; - /** @brief Remove an entry corresponding to a pair of elements. Nothing happens if the pair does not exist in the - * collision matrix. + /** @brief Remove an entry corresponding to a pair of elements. + * Nothing happens if the pair does not exist in the collision matrix. * @param name1 name of first element * @param name2 name of second element*/ void removeEntry(const std::string& name1, const std::string& name2); @@ -138,27 +137,24 @@ class AllowedCollisionMatrix * @param name2 name of second element * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an - * explicit collision - * computation is not necessary (AllowedCollision::ALWAYS).*/ + * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ void setEntry(const std::string& name1, const std::string& name2, bool allowed); - /** @brief Set an entry corresponding to a pair of elements. This sets the type of the entry to - * AllowedCollision::CONDITIONAL. + /** @brief Set an entry corresponding to a pair of elements. + * + * This sets the type of the entry to AllowedCollision::CONDITIONAL. * @param name1 name of first element * @param name2 name of second element - * @param fn A callback function that is used to decide if collisions are allowed between the two elements is - * expected here */ + * @param fn A callback function that is used to decide if collisions are allowed between the two elements */ void setEntry(const std::string& name1, const std::string& name2, const DecideContactFn& fn); - /** @brief Set the entries corresponding to a name. With each of the the known names in the collision matrix, form a - * pair using the name - * specified as argument to this function and set the entry as indicated by \e allowed. + /** @brief Set the entries corresponding to a name. + * With each of the the known names in the collision matrix, form a pair using the name + * specified as argument to this function and set the entry as indicated by \e allowed. * @param name the object name * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an - * explicit collision - * computation is not necessary (AllowedCollision::ALWAYS).*/ - void setEntry(const std::string& name, bool allowed); + * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ /** @brief Set multiple entries. Pairs of names are formed using \e name and \e other_names * @param name name of first element @@ -166,8 +162,7 @@ class AllowedCollisionMatrix * matrix entries will be set for all such pairs. * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an - * explicit collision - * computation is not necessary (AllowedCollision::ALWAYS).*/ + * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ void setEntry(const std::string& name, const std::vector& other_names, bool allowed); /** @brief Set an entry corresponding to all possible pairs between two sets of elements @@ -175,15 +170,13 @@ class AllowedCollisionMatrix * @param names2 Second set of names * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an - * explicit collision - * computation is not necessary (AllowedCollision::ALWAYS).*/ + * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ void setEntry(const std::vector& names1, const std::vector& names2, bool allowed); /** @brief Set an entry corresponding to all known pairs * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an - * explicit collision - * computation is not necessary (AllowedCollision::ALWAYS).*/ + * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ void setEntry(bool allowed); /** @brief Get all the names known to the collision matrix */ From 341e71d299fceb952c8834aabc001d82c486f6a6 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 30 Oct 2021 18:01:03 +0200 Subject: [PATCH 136/229] ACM: specific pair entries take precedence over defaults Reverts c72a8570d420a23a9fe4715705ed617f18836634 --- .../collision_detection/collision_matrix.h | 57 ++++++------------- .../src/collision_matrix.cpp | 28 ++++----- 2 files changed, 29 insertions(+), 56 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h index 234ee5d7ff..0e975d6b71 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h @@ -194,65 +194,42 @@ class AllowedCollisionMatrix return entries_.size(); } - /** @brief Set the default value for entries that include \e name. If such a default value is set, queries to - * getAllowedCollision() that include - * \e name will return this value instead, @b even if a pair that includes \e name was previously specified with - * setEntry(). + /** @brief Set the default value for entries that include \e name but are not set explicity with setEntry(). * @param name The name of the element for which to set the default value * @param allowed If false, indicates that collisions between \e name and any other element must be checked for and - * no collisions - * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between \e name and any other - * element are ok and - * an explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ + * no collisions will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between \e name and + * any other element are ok and an explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ void setDefaultEntry(const std::string& name, bool allowed); - /** @brief Set the default value for entries that include \e name to be AllowedCollision::CONDITIONAL and specify the - * allowed contact predicate to be \e fn. - * If this function is called, queries to getAllowedCollision() that include \e name will return this value instead, - * @b even if a pair that includes \e name - * was previously specified with setEntry(). + /** @brief Set the default value for entries that include \e name but are not set explicity with setEntry(). * @param name The name of the element for which to set the default value * @param fn A callback function that is used to decide if collisions are allowed between \e name and some other - * element is expected here. */ + * element is expected here. */ void setDefaultEntry(const std::string& name, const DecideContactFn& fn); - /** @brief Get the type of the allowed collision between to be considered by default for an element. Return true if a - * default value was - * found for the specified element. Return false otherwise. + /** @brief Get the type of the allowed collision to be considered by default for an element. + * Return true if a default value was found for the specified element. Return false otherwise. * @param name name of the element * @param allowed_collision The default allowed collision type will be filled here */ bool getDefaultEntry(const std::string& name, AllowedCollision::Type& allowed_collision) const; - /** @brief Get the type of the allowed collision between to be considered by default for an element. Return true if a - * default value was - * found for the specified element. Return false otherwise. + /** @brief Get the type of the allowed collision between to be considered by default for an element. + * Return true if a default value was found for the specified element. Return false otherwise. * @param name name of the element - * @param fn A callback function that is used to decide if collisions are allowed between the two elements is filled - * here. */ + * @param fn Return the callback function that is used to decide if collisions are allowed between the two elements. */ bool getDefaultEntry(const std::string& name, DecideContactFn& fn) const; - /** @brief Get the allowed collision predicate between two elements. Return true if a predicate for entry is included - * in the collision matrix - * (if the type is AllowedCollision::CONDITIONAL) or if one was computed from defaults. Return false if the entry is - * not found. - * Default values take precedence. If a default value is specified for either \e name1 or \e name2, that value is - * returned instead (if both - * elements have default values specified, both predicates must be satisfied). If no default values are specified, - * getEntry() is used to look - * for the pair (\e name1, \e name2). + /** @brief Get the allowed collision predicate between two elements. + * Return true if a predicate for entry is included in the collision matrix (if the type is AllowedCollision::CONDITIONAL) + * or if one was computed from defaults. Return false if the entry is not found. * @param name1 name of first element * @param name2 name of second element - * @param fn A callback function that is used to decide if collisions are allowed between the two elements is filled - * here */ + * @param fn Return the callback function that is used to decide if collisions are allowed between the two elements. */ bool getAllowedCollision(const std::string& name1, const std::string& name2, DecideContactFn& fn) const; - /** @brief Get the type of the allowed collision between two elements. Return true if the entry is included in the - * collision matrix or if - * specified defaults were found. Return false if the entry is not found. Default values take precedence. If a - * default value is specified - * for either \e name1 or \e name2, that value is returned instead (if both elements have default values specified, - * AllowedCollision::NEVER takes - * precedence). If no default values are specified, getEntry() is used to look for the pair (\e name1, \e name2). + /** @brief Get the type of the allowed collision between two elements. + * Return true if the entry is included in the collision matrix or if specified defaults were found. + * Return false if the entry is not found. * @param name1 name of first element * @param name2 name of second element * @param allowed_collision The allowed collision type will be filled here */ diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp index 369774ef18..093a457c17 100644 --- a/moveit_core/collision_detection/src/collision_matrix.cpp +++ b/moveit_core/collision_detection/src/collision_matrix.cpp @@ -255,14 +255,12 @@ static bool andDecideContact(const DecideContactFn& f1, const DecideContactFn& f bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2, DecideContactFn& fn) const { - DecideContactFn fn1, fn2; - bool found1 = getDefaultEntry(name1, fn1); - bool found2 = getDefaultEntry(name2, fn2); - - if (!found1 && !found2) - return getEntry(name1, name2, fn); - else + bool found = getEntry(name1, name2, fn); + if (!found) { + DecideContactFn fn1, fn2; + bool found1 = getDefaultEntry(name1, fn1); + bool found2 = getDefaultEntry(name2, fn2); if (found1 && !found2) fn = fn1; else if (!found1 && found2) @@ -271,21 +269,19 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const fn = std::bind(&andDecideContact, fn1, fn2, std::placeholders::_1); else return false; - return true; } + return true; } bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2, AllowedCollision::Type& allowed_collision) const { - AllowedCollision::Type t1, t2; - bool found1 = getDefaultEntry(name1, t1); - bool found2 = getDefaultEntry(name2, t2); - - if (!found1 && !found2) - return getEntry(name1, name2, allowed_collision); - else + bool found = getEntry(name1, name2, allowed_collision); + if (!found) { + AllowedCollision::Type t1, t2; + bool found1 = getDefaultEntry(name1, t1); + bool found2 = getDefaultEntry(name2, t2); if (found1 && !found2) allowed_collision = t1; else if (!found1 && found2) @@ -301,8 +297,8 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const } else return false; - return true; } + return true; } void AllowedCollisionMatrix::clear() From cb1ea39fbaf3212bcfa6cafb40dacefad3df6c41 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 30 Oct 2021 18:03:39 +0200 Subject: [PATCH 137/229] Add comment to prefer setDefaultEntry() over setEntry() ... because the former will consider future collision entries as well. --- .../include/moveit/collision_detection/collision_matrix.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h index 0e975d6b71..703f2c2dce 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h @@ -149,12 +149,14 @@ class AllowedCollisionMatrix void setEntry(const std::string& name1, const std::string& name2, const DecideContactFn& fn); /** @brief Set the entries corresponding to a name. - * With each of the the known names in the collision matrix, form a pair using the name + * With each of the *known names* in the collision matrix, form a pair using the name * specified as argument to this function and set the entry as indicated by \e allowed. + * As the set of known names might change in future, consider using setDefaultEntry() instead! * @param name the object name * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ + void setEntry(const std::string& name, bool allowed); /** @brief Set multiple entries. Pairs of names are formed using \e name and \e other_names * @param name name of first element From 9c7d8dde27184d5c67b08c5d032f84837e1c68e3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 31 Oct 2021 12:22:19 +0100 Subject: [PATCH 138/229] Optimization: Check for most common case first --- moveit_core/collision_detection/src/collision_matrix.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp index 093a457c17..e486f4cd58 100644 --- a/moveit_core/collision_detection/src/collision_matrix.cpp +++ b/moveit_core/collision_detection/src/collision_matrix.cpp @@ -282,7 +282,9 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const AllowedCollision::Type t1, t2; bool found1 = getDefaultEntry(name1, t1); bool found2 = getDefaultEntry(name2, t2); - if (found1 && !found2) + if (!found1 && !found2) + return false; + else if (found1 && !found2) allowed_collision = t1; else if (!found1 && found2) allowed_collision = t2; @@ -295,8 +297,6 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const else // ALWAYS is the only remaining case allowed_collision = AllowedCollision::ALWAYS; } - else - return false; } return true; } From e44e8e2913d3d1affb820d0c99ab8ce21d609a13 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 31 Oct 2021 21:20:14 +0100 Subject: [PATCH 139/229] Adapt message passing of AllowedCollisionMatrix - Serialize full current state (previously pairs with a default, but no entry were skipped) - Only initialize matrix entries that deviate from the default. --- .../collision_detection/collision_matrix.h | 3 + .../src/collision_matrix.cpp | 87 +++++++++++-------- 2 files changed, 53 insertions(+), 37 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h index 703f2c2dce..eabfd12c18 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h @@ -242,6 +242,9 @@ class AllowedCollisionMatrix void print(std::ostream& out) const; private: + bool getDefaultEntry(const std::string& name1, const std::string& name2, + AllowedCollision::Type& allowed_collision) const; + std::map > entries_; std::map > allowed_contacts_; diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp index e486f4cd58..4a4105f080 100644 --- a/moveit_core/collision_detection/src/collision_matrix.cpp +++ b/moveit_core/collision_detection/src/collision_matrix.cpp @@ -55,21 +55,33 @@ AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::AllowedCollisi { if (msg.entry_names.size() != msg.entry_values.size() || msg.default_entry_names.size() != msg.default_entry_values.size()) + { ROS_ERROR_NAMED("collision_detection", "The number of links does not match the number of entries " "in AllowedCollisionMatrix message"); - else + return; + } + for (std::size_t i = 0; i < msg.default_entry_names.size(); ++i) + setDefaultEntry(msg.default_entry_names[i], msg.default_entry_values[i]); + + for (std::size_t i = 0; i < msg.entry_names.size(); ++i) { - for (std::size_t i = 0; i < msg.entry_names.size(); ++i) - if (msg.entry_values[i].enabled.size() != msg.entry_names.size()) - ROS_ERROR_NAMED("collision_detection", - "Number of entries is incorrect for link '%s' in AllowedCollisionMatrix message", - msg.entry_names[i].c_str()); - else - for (std::size_t j = i + 1; j < msg.entry_values[i].enabled.size(); ++j) - setEntry(msg.entry_names[i], msg.entry_names[j], msg.entry_values[i].enabled[j]); + if (msg.entry_values[i].enabled.size() != msg.entry_names.size()) + { + ROS_ERROR_NAMED("collision_detection", + "Number of entries is incorrect for link '%s' in AllowedCollisionMatrix message", + msg.entry_names[i].c_str()); + return; + } + for (std::size_t j = i + 1; j < msg.entry_values[i].enabled.size(); ++j) + { + AllowedCollision::Type allowed_default, allowed_entry; + if (!getDefaultEntry(msg.entry_names[i], msg.entry_names[j], allowed_default)) + allowed_default = AllowedCollision::NEVER; + allowed_entry = msg.entry_values[i].enabled[j] ? AllowedCollision::ALWAYS : AllowedCollision::NEVER; - for (std::size_t i = 0; i < msg.default_entry_names.size(); ++i) - setDefaultEntry(msg.default_entry_names[i], msg.default_entry_values[i]); + if (allowed_entry != allowed_default) + setEntry(msg.entry_names[i], msg.entry_names[j], allowed_entry); + } } } @@ -273,34 +285,36 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const return true; } -bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2, - AllowedCollision::Type& allowed_collision) const +bool AllowedCollisionMatrix::getDefaultEntry(const std::string& name1, const std::string& name2, + AllowedCollision::Type& allowed_collision) const { - bool found = getEntry(name1, name2, allowed_collision); - if (!found) + AllowedCollision::Type t1, t2; + bool found1 = getDefaultEntry(name1, t1); + bool found2 = getDefaultEntry(name2, t2); + if (!found1 && !found2) + return false; + else if (found1 && !found2) + allowed_collision = t1; + else if (!found1 && found2) + allowed_collision = t2; + else if (found1 && found2) { - AllowedCollision::Type t1, t2; - bool found1 = getDefaultEntry(name1, t1); - bool found2 = getDefaultEntry(name2, t2); - if (!found1 && !found2) - return false; - else if (found1 && !found2) - allowed_collision = t1; - else if (!found1 && found2) - allowed_collision = t2; - else if (found1 && found2) - { - if (t1 == AllowedCollision::NEVER || t2 == AllowedCollision::NEVER) - allowed_collision = AllowedCollision::NEVER; - else if (t1 == AllowedCollision::CONDITIONAL || t2 == AllowedCollision::CONDITIONAL) - allowed_collision = AllowedCollision::CONDITIONAL; - else // ALWAYS is the only remaining case - allowed_collision = AllowedCollision::ALWAYS; - } + if (t1 == AllowedCollision::NEVER || t2 == AllowedCollision::NEVER) + allowed_collision = AllowedCollision::NEVER; + else if (t1 == AllowedCollision::CONDITIONAL || t2 == AllowedCollision::CONDITIONAL) + allowed_collision = AllowedCollision::CONDITIONAL; + else // ALWAYS is the only remaining case + allowed_collision = AllowedCollision::ALWAYS; } return true; } +bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2, + AllowedCollision::Type& allowed_collision) const +{ + return getEntry(name1, name2, allowed_collision) || getDefaultEntry(name1, name2, allowed_collision); +} + void AllowedCollisionMatrix::clear() { entries_.clear(); @@ -347,10 +361,9 @@ void AllowedCollisionMatrix::getMessage(moveit_msgs::AllowedCollisionMatrix& msg for (std::size_t j = i; j < msg.entry_names.size(); ++j) { - AllowedCollision::Type type; - bool found = getEntry(msg.entry_names[i], msg.entry_names[j], type); - if (found) - msg.entry_values[i].enabled[j] = msg.entry_values[j].enabled[i] = type == AllowedCollision::ALWAYS; + AllowedCollision::Type type = AllowedCollision::NEVER; + getAllowedCollision(msg.entry_names[i], msg.entry_names[j], type); + msg.entry_values[i].enabled[j] = msg.entry_values[j].enabled[i] = (type == AllowedCollision::ALWAYS); } } } From 66066b52a038b5221da4d0853322c6cd775a22ed Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 2 Nov 2021 11:56:02 +0100 Subject: [PATCH 140/229] ACM:print(): show default value --- .../collision_detection/src/collision_matrix.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp index 4a4105f080..367977991f 100644 --- a/moveit_core/collision_detection/src/collision_matrix.cpp +++ b/moveit_core/collision_detection/src/collision_matrix.cpp @@ -390,7 +390,7 @@ void AllowedCollisionMatrix::print(std::ostream& out) const // print indices along the top of the matrix for (std::size_t j = 0; j < number_digits; ++j) { - out << std::setw(spacing + number_digits + 4) << ""; + out << std::setw(spacing + number_digits + 8) << ""; for (std::size_t i = 0; i < names.size(); ++i) { std::stringstream ss; @@ -400,17 +400,25 @@ void AllowedCollisionMatrix::print(std::ostream& out) const out << std::endl; } + const char* indicator = "01?"; // ALWAYS / NEVER / CONDITIONAL for (std::size_t i = 0; i < names.size(); ++i) { out << std::setw(spacing) << names[i]; out << std::setw(number_digits + 1) << i; out << " | "; + // print default value + AllowedCollision::Type type; + if (getDefaultEntry(names[i], type)) + out << indicator[type]; + else + out << '-'; + out << " | "; + // print pairs for (std::size_t j = 0; j < names.size(); ++j) { - AllowedCollision::Type type; bool found = getAllowedCollision(names[i], names[j], type); if (found) - out << std::setw(3) << (type == AllowedCollision::ALWAYS ? '1' : (type == AllowedCollision::NEVER ? '0' : '?')); + out << std::setw(3) << indicator[type]; else out << std::setw(3) << '-'; } From 6679901a7e8d20315d33c3164b027747230ec7ff Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 22 Dec 2021 16:55:55 +0100 Subject: [PATCH 141/229] Adapt to API changes in srdfdom @v4hn requested splitting of collision_pairs into (re)enabled and disabled. --- moveit_core/planning_scene/src/planning_scene.cpp | 8 +++++--- moveit_setup_assistant/src/collisions_updater.cpp | 6 +++++- .../src/widgets/configuration_files_widget.cpp | 2 +- .../src/widgets/default_collisions_widget.cpp | 7 +++---- 4 files changed, 14 insertions(+), 9 deletions(-) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 6180b1d3ae..d209e89e75 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -156,9 +156,11 @@ void PlanningScene::initialize() // load collision defaults for (const std::string& name : getRobotModel()->getSRDF()->getNoDefaultCollisionLinks()) acm_->setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS); - // allow collisions for pairs that have been disabled - for (auto const& collision : getRobotModel()->getSRDF()->getCollisionPairs()) - acm_->setEntry(collision.link1_, collision.link2_, collision.disabled_); + // load collision pairs + for (auto const& collision : getRobotModel()->getSRDF()->getEnabledCollisionPairs()) + acm_->setEntry(collision.link1_, collision.link2_, false); + for (auto const& collision : getRobotModel()->getSRDF()->getDisabledCollisionPairs()) + acm_->setEntry(collision.link1_, collision.link2_, true); setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create()); } diff --git a/moveit_setup_assistant/src/collisions_updater.cpp b/moveit_setup_assistant/src/collisions_updater.cpp index b79f5cbd0e..48e2418cf7 100644 --- a/moveit_setup_assistant/src/collisions_updater.cpp +++ b/moveit_setup_assistant/src/collisions_updater.cpp @@ -97,7 +97,11 @@ bool setup(moveit_setup_assistant::MoveItConfigData& config_data, bool keep_old, } if (!keep_old) - config_data.srdf_->collision_pairs_.clear(); + { + config_data.srdf_->no_default_collision_links_.clear(); + config_data.srdf_->enabled_collision_pairs_.clear(); + config_data.srdf_->disabled_collision_pairs_.clear(); + } return true; } diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 39f23c2e8d..a334b80b2f 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -641,7 +641,7 @@ bool ConfigurationFilesWidget::checkDependencies() } // Check that at least 1 link pair is disabled from collision checking - if (config_data_->srdf_->collision_pairs_.empty()) + if (config_data_->srdf_->disabled_collision_pairs_.empty()) { dependencies << "No self-collisions have been disabled"; } diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp index 3d80e6efa7..938d54505f 100644 --- a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp +++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp @@ -677,7 +677,7 @@ void DefaultCollisionsWidget::checkedFilterChanged() void DefaultCollisionsWidget::linkPairsToSRDF() { // reset the data in the SRDF Writer class - config_data_->srdf_->collision_pairs_.clear(); + config_data_->srdf_->disabled_collision_pairs_.clear(); // Create temp disabled collision srdf::Model::CollisionPair dc; @@ -692,8 +692,7 @@ void DefaultCollisionsWidget::linkPairsToSRDF() dc.link1_ = pair_it->first.first; dc.link2_ = pair_it->first.second; dc.reason_ = moveit_setup_assistant::disabledReasonToString(pair_it->second.reason); - dc.disabled_ = pair_it->second.disable_check; - config_data_->srdf_->collision_pairs_.push_back(dc); + config_data_->srdf_->disabled_collision_pairs_.push_back(dc); } } @@ -720,7 +719,7 @@ void DefaultCollisionsWidget::linkPairsFromSRDF() std::pair link_pair; // Loop through all disabled collisions in SRDF and update the comprehensive list that has already been created - for (const auto& disabled_collision : config_data_->srdf_->collision_pairs_) + for (const auto& disabled_collision : config_data_->srdf_->disabled_collision_pairs_) { // Set the link names link_pair.first = disabled_collision.link1_; From 47a66152d5b3559d147b0cf57c4c8bcca50a3931 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 28 Dec 2021 11:24:09 +0100 Subject: [PATCH 142/229] Unify initialization of ACM from SRDF --- .../moveit/collision_detection/collision_matrix.h | 3 +++ .../test_collision_common_panda.h | 9 +-------- .../collision_detection/src/collision_matrix.cpp | 13 +++++++++++++ .../test_bullet_continuous_collision_checking.cpp | 9 +-------- moveit_core/planning_scene/src/planning_scene.cpp | 11 +---------- .../src/tools/moveit_config_data.cpp | 15 +++++++++------ 6 files changed, 28 insertions(+), 32 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h index eabfd12c18..b754a5bb48 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h @@ -88,6 +88,9 @@ class AllowedCollisionMatrix * will be ignored. */ AllowedCollisionMatrix(const std::vector& names, bool allowed = false); + /** @brief Construct from an SRDF representation */ + AllowedCollisionMatrix(const srdf::Model& srdf); + /** @brief Construct the structure from a message representation */ AllowedCollisionMatrix(const moveit_msgs::AllowedCollisionMatrix& msg); diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h index 9b0bf7097f..ac2c2ebf45 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h @@ -78,14 +78,7 @@ class CollisionDetectorPandaTest : public testing::Test robot_model_ = moveit::core::loadTestingRobotModel("panda"); robot_model_ok_ = static_cast(robot_model_); - acm_ = std::make_shared(); - - // load collision defaults - for (const std::string& name : robot_model_->getSRDF()->getNoDefaultCollisionLinks()) - acm_->setDefaultEntry(name, collision_detection::AllowedCollision::NEVER); - // allow collisions for pairs that have been disabled - for (auto const& collision : robot_model_->getSRDF()->getCollisionPairs()) - acm_->setEntry(collision.link1_, collision.link2_, collision.disabled_); + acm_ = std::make_shared(*robot_model_->getSRDF()); cenv_ = value_->allocateEnv(robot_model_); diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp index 367977991f..08b192b486 100644 --- a/moveit_core/collision_detection/src/collision_matrix.cpp +++ b/moveit_core/collision_detection/src/collision_matrix.cpp @@ -51,6 +51,19 @@ AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector& n setEntry(names[i], names[j], allowed); } +AllowedCollisionMatrix::AllowedCollisionMatrix(const srdf::Model& srdf) +{ + // load collision defaults + for (const std::string& name : srdf.getNoDefaultCollisionLinks()) + setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS); + // re-enable specific collision pairs + for (auto const& collision : srdf.getEnabledCollisionPairs()) + setEntry(collision.link1_, collision.link2_, false); + // *finally* disable selected collision pairs + for (auto const& collision : srdf.getDisabledCollisionPairs()) + setEntry(collision.link1_, collision.link2_, true); +} + AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::AllowedCollisionMatrix& msg) { if (msg.entry_names.size() != msg.entry_values.size() || diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp index 06c6300f14..ffda13ee80 100644 --- a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp +++ b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp @@ -76,14 +76,7 @@ class BulletCollisionDetectionTester : public testing::Test robot_model_ = moveit::core::loadTestingRobotModel("panda"); robot_model_ok_ = static_cast(robot_model_); - acm_ = std::make_shared(); - - // load collision defaults - for (const std::string& name : robot_model_->getSRDF()->getNoDefaultCollisionLinks()) - acm_->setDefaultEntry(name, collision_detection::AllowedCollision::NEVER); - // allow collisions for pairs that have been disabled - for (auto const& collision : robot_model_->getSRDF()->getCollisionPairs()) - acm_->setEntry(collision.link1_, collision.link2_, collision.disabled_); + acm_ = std::make_shared(*robot_model_->getSRDF()); cenv_ = std::make_shared(robot_model_); diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index d209e89e75..9715267682 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -151,16 +151,7 @@ void PlanningScene::initialize() robot_state_->setToDefaultValues(); robot_state_->update(); - acm_ = std::make_shared(); - - // load collision defaults - for (const std::string& name : getRobotModel()->getSRDF()->getNoDefaultCollisionLinks()) - acm_->setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS); - // load collision pairs - for (auto const& collision : getRobotModel()->getSRDF()->getEnabledCollisionPairs()) - acm_->setEntry(collision.link1_, collision.link2_, false); - for (auto const& collision : getRobotModel()->getSRDF()->getDisabledCollisionPairs()) - acm_->setEntry(collision.link1_, collision.link2_, true); + acm_ = std::make_shared(*getRobotModel()->getSRDF()); setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create()); } diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 3f8d65e4dd..a9f20cf945 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -140,14 +140,17 @@ planning_scene::PlanningScenePtr MoveItConfigData::getPlanningScene() // ****************************************************************************************** void MoveItConfigData::loadAllowedCollisionMatrix() { - // Clear the allowed collision matrix allowed_collision_matrix_.clear(); - // Update the allowed collision matrix, in case there has been a change - for (const auto& collision : srdf_->collision_pairs_) - { - allowed_collision_matrix_.setEntry(collision.link1_, collision.link2_, collision.disabled_); - } + // load collision defaults + for (const std::string& name : srdf_->no_default_collision_links_) + allowed_collision_matrix_.setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS); + // re-enable specific collision pairs + for (auto const& collision : srdf_->enabled_collision_pairs_) + allowed_collision_matrix_.setEntry(collision.link1_, collision.link2_, false); + // *finally* disable selected collision pairs + for (auto const& collision : srdf_->disabled_collision_pairs_) + allowed_collision_matrix_.setEntry(collision.link1_, collision.link2_, true); } // ****************************************************************************************** From 2a6641392208f1ac28b4420d6c1323e4713c6c56 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 28 Dec 2021 12:07:30 +0100 Subject: [PATCH 143/229] Move MoveItConfigData::setCollisionLinkPairs to collisions_updater.cpp This method is only used there to update disabled collision entries. --- .../tools/moveit_config_data.h | 7 --- .../src/collisions_updater.cpp | 48 ++++++++++++++++- .../src/tools/moveit_config_data.cpp | 53 ------------------- 3 files changed, 47 insertions(+), 61 deletions(-) diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h index 7ce6f0caae..050ec293bf 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h @@ -330,13 +330,6 @@ class MoveItConfigData */ std::string getGazeboCompatibleURDF(); - /** - * \brief Set list of collision link pairs in SRDF; sorted; with optional filter - * \param link_pairs list of collision link pairs - * \param skip_mask mask of shifted moveit_setup_assistant::DisabledReason values that will be skipped - */ - void setCollisionLinkPairs(const moveit_setup_assistant::LinkPairMap& link_pairs, size_t skip_mask = 0); - /** * \brief Decide the best two joints to be used for the projection evaluator * \param planning_group name of group to use diff --git a/moveit_setup_assistant/src/collisions_updater.cpp b/moveit_setup_assistant/src/collisions_updater.cpp index 48e2418cf7..3e6f36ad52 100644 --- a/moveit_setup_assistant/src/collisions_updater.cpp +++ b/moveit_setup_assistant/src/collisions_updater.cpp @@ -115,6 +115,52 @@ moveit_setup_assistant::LinkPairMap compute(moveit_setup_assistant::MoveItConfig trials > 0, trials, min_collision_fraction, verbose); } +// less operation for two CollisionPairs +struct CollisionPairLess +{ + bool operator()(const srdf::Model::CollisionPair& left, const srdf::Model::CollisionPair& right) const + { + return left.link1_ < right.link1_ && left.link2_ < right.link2_; + } +}; + +// Update collision pairs +void updateCollisionLinkPairs(std::vector& target_pairs, + const moveit_setup_assistant::LinkPairMap& source_pairs, size_t skip_mask) +{ + // remove duplicates + std::set filtered; + for (auto& p : target_pairs) + { + if (p.link1_ >= p.link2_) + std::swap(p.link1_, p.link2_); // unify link1, link2 sorting + filtered.insert(p); + } + + // copy the data in this class's LinkPairMap datastructure to srdf::Model::CollisionPair format + for (const auto& link_pair : source_pairs) + { + // Only copy those that are actually disabled + if (!link_pair.second.disable_check) + continue; + + // Filter out pairs matching the skip_mask + if ((1 << link_pair.second.reason) & skip_mask) + continue; + + srdf::Model::CollisionPair pair; + pair.link1_ = link_pair.first.first; + pair.link2_ = link_pair.first.second; + if (pair.link1_ >= pair.link2_) + std::swap(pair.link1_, pair.link2_); + pair.reason_ = moveit_setup_assistant::disabledReasonToString(link_pair.second.reason); + + filtered.insert(pair); + } + + target_pairs.assign(filtered.begin(), filtered.end()); +} + int main(int argc, char* argv[]) { std::string config_pkg_path; @@ -205,7 +251,7 @@ int main(int argc, char* argv[]) if (!include_always) skip_mask |= (1 << moveit_setup_assistant::ALWAYS); - config_data.setCollisionLinkPairs(link_pairs, skip_mask); + updateCollisionLinkPairs(config_data.srdf_->disabled_collision_pairs_, link_pairs, skip_mask); config_data.srdf_->writeSRDF(output_path.empty() ? config_data.srdf_path_ : output_path); diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index a9f20cf945..77208dc5bb 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -1203,59 +1203,6 @@ bool MoveItConfigData::outputJointLimitsYAML(const std::string& file_path) return true; // file created successfully } -// ****************************************************************************************** -// Set list of collision link pairs in SRDF; sorted; with optional filter -// ****************************************************************************************** - -class SortableCollisionPair -{ -public: - SortableCollisionPair(const srdf::Model::CollisionPair& p) - : pair_(p), key_(p.link1_ < p.link2_ ? (p.link1_ + "|" + p.link2_) : (p.link2_ + "|" + p.link1_)) - { - } - operator const srdf::Model::CollisionPair &() const - { - return pair_; - } - bool operator<(const SortableCollisionPair& other) const - { - return key_ < other.key_; - } - -private: - const srdf::Model::CollisionPair pair_; - const std::string key_; -}; - -void MoveItConfigData::setCollisionLinkPairs(const moveit_setup_assistant::LinkPairMap& link_pairs, size_t skip_mask) -{ - // Create temp disabled collision - srdf::Model::CollisionPair pair; - - std::set collision_pairs; - collision_pairs.insert(srdf_->collision_pairs_.begin(), srdf_->collision_pairs_.end()); - - // copy the data in this class's LinkPairMap datastructure to srdf::Model::CollisionPair format - for (const std::pair, LinkPairData>& link_pair : link_pairs) - { - // Only copy those that are actually disabled - if (link_pair.second.disable_check) - { - if ((1 << link_pair.second.reason) & skip_mask) - continue; - - pair.link1_ = link_pair.first.first; - pair.link2_ = link_pair.first.second; - pair.reason_ = moveit_setup_assistant::disabledReasonToString(link_pair.second.reason); - - collision_pairs.insert(SortableCollisionPair(pair)); - } - } - - srdf_->collision_pairs_.assign(collision_pairs.begin(), collision_pairs.end()); -} - // ****************************************************************************************** // Decide the best two joints to be used for the projection evaluator // ****************************************************************************************** From 890a80e31a85336c4df734d9a33b51eb034cb0fa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Wed, 29 Dec 2021 15:38:23 +0100 Subject: [PATCH 144/229] Disable slow robot_interaction tests in DEBUG mode (#3014) These tests are known to run for a very long time in debug builds. So let's disable them in this case. If you still insist to run them, you can do so via `locked_robot_state_test --gtest_also_run_disabled_tests` --- .../test/locked_robot_state_test.cpp | 32 ++++++++++++------- 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp b/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp index 3e5b155768..f3448bb38b 100644 --- a/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp +++ b/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp @@ -551,62 +551,70 @@ TEST(LockedRobotState, set1) runThreads(1, 1, 0); } -TEST(LockedRobotState, set2) +// skip all more complex locking checks when optimizations are disabled +// they can easily outrun 20 minutes with Debug builds +#ifdef NDEBUG +#define OPT_TEST(F, N) TEST(F, N) +#else +#define OPT_TEST(F, N) TEST(F, DISABLED_##N) +#endif + +OPT_TEST(LockedRobotState, set2) { runThreads(1, 2, 0); } -TEST(LockedRobotState, set3) +OPT_TEST(LockedRobotState, set3) { runThreads(1, 3, 0); } -TEST(LockedRobotState, mod1) +OPT_TEST(LockedRobotState, mod1) { runThreads(1, 0, 1); } -TEST(LockedRobotState, mod2) +OPT_TEST(LockedRobotState, mod2) { runThreads(1, 0, 1); } -TEST(LockedRobotState, mod3) +OPT_TEST(LockedRobotState, mod3) { runThreads(1, 0, 1); } -TEST(LockedRobotState, set1mod1) +OPT_TEST(LockedRobotState, set1mod1) { runThreads(1, 1, 1); } -TEST(LockedRobotState, set2mod1) +OPT_TEST(LockedRobotState, set2mod1) { runThreads(1, 2, 1); } -TEST(LockedRobotState, set1mod2) +OPT_TEST(LockedRobotState, set1mod2) { runThreads(1, 1, 2); } -TEST(LockedRobotState, set3mod1) +OPT_TEST(LockedRobotState, set3mod1) { runThreads(1, 3, 1); } -TEST(LockedRobotState, set1mod3) +OPT_TEST(LockedRobotState, set1mod3) { runThreads(1, 1, 3); } -TEST(LockedRobotState, set3mod3) +OPT_TEST(LockedRobotState, set3mod3) { runThreads(1, 3, 3); } -TEST(LockedRobotState, set3mod3c3) +OPT_TEST(LockedRobotState, set3mod3c3) { runThreads(3, 3, 3); } From 155eefa46e30d44caac9a7d6ec2ebae6eda88ffb Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 31 Dec 2021 02:07:03 +0100 Subject: [PATCH 145/229] 1.1.7 --- moveit/CHANGELOG.rst | 3 +++ moveit/package.xml | 2 +- moveit_commander/CHANGELOG.rst | 3 +++ moveit_commander/package.xml | 2 +- moveit_core/CHANGELOG.rst | 15 +++++++++++++++ moveit_core/package.xml | 2 +- moveit_kinematics/CHANGELOG.rst | 6 ++++++ moveit_kinematics/package.xml | 2 +- .../chomp/chomp_interface/CHANGELOG.rst | 5 +++++ moveit_planners/chomp/chomp_interface/package.xml | 2 +- .../chomp/chomp_motion_planner/CHANGELOG.rst | 3 +++ .../chomp/chomp_motion_planner/package.xml | 2 +- .../chomp/chomp_optimizer_adapter/CHANGELOG.rst | 3 +++ .../chomp/chomp_optimizer_adapter/package.xml | 2 +- moveit_planners/moveit_planners/CHANGELOG.rst | 5 +++++ moveit_planners/moveit_planners/package.xml | 2 +- moveit_planners/ompl/CHANGELOG.rst | 6 ++++++ moveit_planners/ompl/package.xml | 2 +- .../pilz_industrial_motion_planner/CHANGELOG.rst | 6 ++++++ .../pilz_industrial_motion_planner/package.xml | 2 +- .../CHANGELOG.rst | 5 +++++ .../package.xml | 2 +- .../moveit_fake_controller_manager/CHANGELOG.rst | 5 +++++ .../moveit_fake_controller_manager/package.xml | 2 +- moveit_plugins/moveit_plugins/CHANGELOG.rst | 3 +++ moveit_plugins/moveit_plugins/package.xml | 2 +- .../moveit_ros_control_interface/CHANGELOG.rst | 3 +++ .../moveit_ros_control_interface/package.xml | 2 +- .../CHANGELOG.rst | 6 ++++++ .../moveit_simple_controller_manager/package.xml | 2 +- moveit_ros/benchmarks/CHANGELOG.rst | 6 ++++++ moveit_ros/benchmarks/package.xml | 2 +- moveit_ros/manipulation/CHANGELOG.rst | 5 +++++ moveit_ros/manipulation/package.xml | 2 +- moveit_ros/move_group/CHANGELOG.rst | 5 +++++ moveit_ros/move_group/package.xml | 2 +- moveit_ros/moveit_ros/CHANGELOG.rst | 3 +++ moveit_ros/moveit_ros/package.xml | 2 +- moveit_ros/moveit_servo/CHANGELOG.rst | 3 +++ moveit_ros/moveit_servo/package.xml | 2 +- moveit_ros/occupancy_map_monitor/CHANGELOG.rst | 5 +++++ moveit_ros/occupancy_map_monitor/package.xml | 2 +- moveit_ros/perception/CHANGELOG.rst | 6 ++++++ moveit_ros/perception/package.xml | 2 +- moveit_ros/planning/CHANGELOG.rst | 9 +++++++++ moveit_ros/planning/package.xml | 2 +- moveit_ros/planning_interface/CHANGELOG.rst | 7 +++++++ moveit_ros/planning_interface/package.xml | 2 +- moveit_ros/robot_interaction/CHANGELOG.rst | 6 ++++++ moveit_ros/robot_interaction/package.xml | 2 +- moveit_ros/visualization/CHANGELOG.rst | 13 +++++++++++++ moveit_ros/visualization/package.xml | 2 +- moveit_ros/warehouse/CHANGELOG.rst | 5 +++++ moveit_ros/warehouse/package.xml | 2 +- moveit_runtime/CHANGELOG.rst | 3 +++ moveit_runtime/package.xml | 2 +- moveit_setup_assistant/CHANGELOG.rst | 8 ++++++++ moveit_setup_assistant/package.xml | 2 +- 58 files changed, 190 insertions(+), 29 deletions(-) diff --git a/moveit/CHANGELOG.rst b/moveit/CHANGELOG.rst index ba31fd2f92..c227ce07f1 100644 --- a/moveit/CHANGELOG.rst +++ b/moveit/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ + 1.1.6 (2021-11-06) ------------------ diff --git a/moveit/package.xml b/moveit/package.xml index 9c6ee7c1b7..c168d1f847 100644 --- a/moveit/package.xml +++ b/moveit/package.xml @@ -1,7 +1,7 @@ moveit - 1.1.6 + 1.1.7 Meta package that contains all essential package of MoveIt. Until Summer 2016 MoveIt had been developed over multiple repositories, where developers' usability and maintenance effort was non-trivial. See the detailed discussion for the merge of several repositories. Dave Coleman Michael Ferguson diff --git a/moveit_commander/CHANGELOG.rst b/moveit_commander/CHANGELOG.rst index 05021c05db..7c02f1e54d 100644 --- a/moveit_commander/CHANGELOG.rst +++ b/moveit_commander/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_commander ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ + 1.1.6 (2021-11-06) ------------------ * Use relative imports (`#2912 `_) diff --git a/moveit_commander/package.xml b/moveit_commander/package.xml index 68268eb3be..4970b27b70 100644 --- a/moveit_commander/package.xml +++ b/moveit_commander/package.xml @@ -1,7 +1,7 @@ moveit_commander - 1.1.6 + 1.1.7 Python interfaces to MoveIt Ioan Sucan diff --git a/moveit_core/CHANGELOG.rst b/moveit_core/CHANGELOG.rst index f381b2f6ee..e79d02a577 100644 --- a/moveit_core/CHANGELOG.rst +++ b/moveit_core/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package moveit_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ +* Move ``MoveItErrorCode`` class to ``moveit_core`` (`#3009 `_) +* Disable (flaky) timing tests in ``DEBUG`` mode (`#3012 `_) +* ``RobotState::attachBody``: Migrate to unique_ptr argument (`#3011 `_) +* Add API stress tests for ``TOTG``, fix undefined behavior (`#2957 `_) +* Do not assert on printTransform with non-isometry (`#3005 `_) +* Provide ``MOVEIT_VERSION_CHECK`` macro (`#2997 `_) +* Quietly use backward_cpp/ros if available (`#2988 `_) +* Allow restricting collision pairs to a group (`#2987 `_) +* Add backwards compatibility for old scene serialization format (`#2986 `_) +* Switch to ``std::bind`` (`#2967 `_) +* Add waypoint duration to the trajectory deep copy unit test (`#2961 `_) +* Contributors: AndyZe, Henning Kayser, Jafar Abdi, Jochen Sprickerhof, Michael Görner, Robert Haschke, Simon Schmeisser, Wolfgang Merkt, pvanlaar + 1.1.6 (2021-11-06) ------------------ * Silent warning about invalid ``virtual_joint`` in Gazebo setups diff --git a/moveit_core/package.xml b/moveit_core/package.xml index 5e983e1c0f..a931da247a 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -1,7 +1,7 @@ moveit_core - 1.1.6 + 1.1.7 Core libraries used by MoveIt Ioan Sucan Sachin Chitta diff --git a/moveit_kinematics/CHANGELOG.rst b/moveit_kinematics/CHANGELOG.rst index 042f1dc1bb..dc2d697b8f 100644 --- a/moveit_kinematics/CHANGELOG.rst +++ b/moveit_kinematics/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_kinematics ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ +* ``round_collada_numbers.py``: python 2/3 compatibility (`#2983 `_) +* Switch to ``std::bind`` (`#2967 `_) +* Contributors: Jochen Sprickerhof, Tomislav Bazina + 1.1.6 (2021-11-06) ------------------ * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index 9ecae6dcf1..5584069d43 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -1,7 +1,7 @@ moveit_kinematics - 1.1.6 + 1.1.7 Package for all inverse kinematics solvers in MoveIt Dave Coleman diff --git a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst index 7ce1926fe2..7d22b68618 100644 --- a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package chomp_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ +* Move ``MoveItErrorCode`` class to ``moveit_core`` (`#3009 `_) +* Contributors: Jafar Abdi + 1.1.6 (2021-11-06) ------------------ * Use test_environment.launch in unittests (`#2949 `_) diff --git a/moveit_planners/chomp/chomp_interface/package.xml b/moveit_planners/chomp/chomp_interface/package.xml index a540ef8a77..a0c96bc8bf 100644 --- a/moveit_planners/chomp/chomp_interface/package.xml +++ b/moveit_planners/chomp/chomp_interface/package.xml @@ -2,7 +2,7 @@ moveit_planners_chomp The interface for using CHOMP within MoveIt - 1.1.6 + 1.1.7 Gil Jones Chittaranjan Srinivas Swaminathan diff --git a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst index a0e95d0ff1..12ae16ab37 100644 --- a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package chomp_motion_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ + 1.1.6 (2021-11-06) ------------------ * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` diff --git a/moveit_planners/chomp/chomp_motion_planner/package.xml b/moveit_planners/chomp/chomp_motion_planner/package.xml index 3ddc837c28..6970ae123d 100644 --- a/moveit_planners/chomp/chomp_motion_planner/package.xml +++ b/moveit_planners/chomp/chomp_motion_planner/package.xml @@ -2,7 +2,7 @@ chomp_motion_planner chomp_motion_planner - 1.1.6 + 1.1.7 Gil Jones Mrinal Kalakrishnan diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst index f157e261ab..1812ac2e32 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_chomp_optimizer_adapter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ + 1.1.6 (2021-11-06) ------------------ * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml index 964630594c..51f53726e7 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml +++ b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml @@ -3,7 +3,7 @@ moveit_chomp_optimizer_adapter MoveIt planning request adapter utilizing chomp for solution optimization - 1.1.6 + 1.1.7 Raghavender Sahdev Raghavender Sahdev MoveIt Release Team diff --git a/moveit_planners/moveit_planners/CHANGELOG.rst b/moveit_planners/moveit_planners/CHANGELOG.rst index 88bc0f818b..9aecb4ba65 100644 --- a/moveit_planners/moveit_planners/CHANGELOG.rst +++ b/moveit_planners/moveit_planners/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_planners ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ +* Add pilz planner to ``moveit_planners`` dependency +* Contributors: v4hn + 1.1.6 (2021-11-06) ------------------ diff --git a/moveit_planners/moveit_planners/package.xml b/moveit_planners/moveit_planners/package.xml index 84d8c332db..9391ca11b7 100644 --- a/moveit_planners/moveit_planners/package.xml +++ b/moveit_planners/moveit_planners/package.xml @@ -1,6 +1,6 @@ moveit_planners - 1.1.6 + 1.1.7 Metapacakge that installs all available planners for MoveIt Ioan Sucan Sachin Chitta diff --git a/moveit_planners/ompl/CHANGELOG.rst b/moveit_planners/ompl/CHANGELOG.rst index 74380e604a..81a6e61d70 100644 --- a/moveit_planners/ompl/CHANGELOG.rst +++ b/moveit_planners/ompl/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_planners_ompl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ +* Use termination condition for simplification step (`#2981 `_) +* Switch to ``std::bind`` (`#2967 `_) +* Contributors: Jochen Sprickerhof, Simon Schmeisser + 1.1.6 (2021-11-06) ------------------ * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` diff --git a/moveit_planners/ompl/package.xml b/moveit_planners/ompl/package.xml index cdd71debcc..f5ea3680b4 100644 --- a/moveit_planners/ompl/package.xml +++ b/moveit_planners/ompl/package.xml @@ -1,6 +1,6 @@ moveit_planners_ompl - 1.1.6 + 1.1.7 MoveIt interface to OMPL Ioan Sucan diff --git a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst index 6db7860dbc..3074492399 100644 --- a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst +++ b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package pilz_industrial_motion_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ +* Pilz planner: improve reporting of invalid start joints (`#3000 `_) +* Switch to ``std::bind`` (`#2967 `_) +* Contributors: Jochen Sprickerhof, Robert Haschke, v4hn + 1.1.6 (2021-11-06) ------------------ * Fix calculation of subframe offset (`#2890 `_) diff --git a/moveit_planners/pilz_industrial_motion_planner/package.xml b/moveit_planners/pilz_industrial_motion_planner/package.xml index 8c1b056703..5f41b9b65a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/package.xml +++ b/moveit_planners/pilz_industrial_motion_planner/package.xml @@ -1,7 +1,7 @@ pilz_industrial_motion_planner - 1.1.6 + 1.1.7 MoveIt plugin to generate industrial trajectories PTP, LIN, CIRC and sequences thereof. Alexander Gutenkunst diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst index 3a52f57b77..584145a42c 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pilz_industrial_motion_planner_testutils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ +* Switch to ``std::bind`` (`#2967 `_) +* Contributors: Jochen Sprickerhof + 1.1.6 (2021-11-06) ------------------ * clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 `_) diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml index 425b86d9b5..d004a501ed 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml @@ -1,7 +1,7 @@ pilz_industrial_motion_planner_testutils - 1.1.6 + 1.1.7 Helper scripts and functionality to test industrial motion generation Alexander Gutenkunst diff --git a/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst index bd397a6b35..24b55d5018 100644 --- a/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst +++ b/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_fake_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ +* Switch to ``std::bind`` (`#2967 `_) +* Contributors: Jochen Sprickerhof + 1.1.6 (2021-11-06) ------------------ * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` diff --git a/moveit_plugins/moveit_fake_controller_manager/package.xml b/moveit_plugins/moveit_fake_controller_manager/package.xml index 90fb3a88ab..a8ce23a50f 100644 --- a/moveit_plugins/moveit_fake_controller_manager/package.xml +++ b/moveit_plugins/moveit_fake_controller_manager/package.xml @@ -1,6 +1,6 @@ moveit_fake_controller_manager - 1.1.6 + 1.1.7 A fake controller manager plugin for MoveIt. Ioan Sucan diff --git a/moveit_plugins/moveit_plugins/CHANGELOG.rst b/moveit_plugins/moveit_plugins/CHANGELOG.rst index 1f4b2c84ee..f4b6307e38 100644 --- a/moveit_plugins/moveit_plugins/CHANGELOG.rst +++ b/moveit_plugins/moveit_plugins/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ + 1.1.6 (2021-11-06) ------------------ diff --git a/moveit_plugins/moveit_plugins/package.xml b/moveit_plugins/moveit_plugins/package.xml index 322d113cbd..0ca1854d4a 100644 --- a/moveit_plugins/moveit_plugins/package.xml +++ b/moveit_plugins/moveit_plugins/package.xml @@ -1,6 +1,6 @@ moveit_plugins - 1.1.6 + 1.1.7 Metapackage for MoveIt plugins. Michael Ferguson diff --git a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst index 8fa38fd676..6f2019fe8b 100644 --- a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst +++ b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_ros_control_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ + 1.1.6 (2021-11-06) ------------------ * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` diff --git a/moveit_plugins/moveit_ros_control_interface/package.xml b/moveit_plugins/moveit_ros_control_interface/package.xml index d8f3ad47bc..3b928cbe84 100644 --- a/moveit_plugins/moveit_ros_control_interface/package.xml +++ b/moveit_plugins/moveit_ros_control_interface/package.xml @@ -1,7 +1,7 @@ moveit_ros_control_interface - 1.1.6 + 1.1.7 ros_control controller manager interface for MoveIt Mathias Lüdtke diff --git a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst index f6dff94a2f..054a0d18b3 100644 --- a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst +++ b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_simple_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ +* ``simple_controller_manager``: add ``max_effort`` parameter to ``GripperCommand`` action (`#2984 `_) +* Switch to ``std::bind`` (`#2967 `_) +* Contributors: Jochen Sprickerhof, Rick Staa + 1.1.6 (2021-11-06) ------------------ * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` diff --git a/moveit_plugins/moveit_simple_controller_manager/package.xml b/moveit_plugins/moveit_simple_controller_manager/package.xml index 86f1fa2e3b..45d89919bf 100644 --- a/moveit_plugins/moveit_simple_controller_manager/package.xml +++ b/moveit_plugins/moveit_simple_controller_manager/package.xml @@ -1,6 +1,6 @@ moveit_simple_controller_manager - 1.1.6 + 1.1.7 A generic, simple controller manager plugin for MoveIt. Michael Ferguson diff --git a/moveit_ros/benchmarks/CHANGELOG.rst b/moveit_ros/benchmarks/CHANGELOG.rst index 60ef11f3cf..4b074ba918 100644 --- a/moveit_ros/benchmarks/CHANGELOG.rst +++ b/moveit_ros/benchmarks/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_ros_benchmarks ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ +* Provide ``MOVEIT_VERSION_CHECK`` macro (`#2997 `_) +* Switch to ``std::bind`` (`#2967 `_) +* Contributors: Jochen Sprickerhof, Robert Haschke + 1.1.6 (2021-11-06) ------------------ * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` diff --git a/moveit_ros/benchmarks/package.xml b/moveit_ros/benchmarks/package.xml index 8073661820..de34c71a84 100644 --- a/moveit_ros/benchmarks/package.xml +++ b/moveit_ros/benchmarks/package.xml @@ -1,6 +1,6 @@ moveit_ros_benchmarks - 1.1.6 + 1.1.7 Enhanced tools for benchmarks in MoveIt diff --git a/moveit_ros/manipulation/CHANGELOG.rst b/moveit_ros/manipulation/CHANGELOG.rst index 412b0702c4..99498e8c33 100644 --- a/moveit_ros/manipulation/CHANGELOG.rst +++ b/moveit_ros/manipulation/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_manipulation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ +* Switch to ``std::bind`` (`#2967 `_) +* Contributors: Jochen Sprickerhof + 1.1.6 (2021-11-06) ------------------ * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` diff --git a/moveit_ros/manipulation/package.xml b/moveit_ros/manipulation/package.xml index 30792de339..ae6ab2c1d0 100644 --- a/moveit_ros/manipulation/package.xml +++ b/moveit_ros/manipulation/package.xml @@ -1,6 +1,6 @@ moveit_ros_manipulation - 1.1.6 + 1.1.7 Components of MoveIt used for manipulation Ioan Sucan diff --git a/moveit_ros/move_group/CHANGELOG.rst b/moveit_ros/move_group/CHANGELOG.rst index 2fd523bd35..1636a92805 100644 --- a/moveit_ros/move_group/CHANGELOG.rst +++ b/moveit_ros/move_group/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_move_group ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ +* Switch to ``std::bind`` (`#2967 `_) +* Contributors: Jochen Sprickerhof + 1.1.6 (2021-11-06) ------------------ * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` diff --git a/moveit_ros/move_group/package.xml b/moveit_ros/move_group/package.xml index b8a1bca95e..8d710c25c1 100644 --- a/moveit_ros/move_group/package.xml +++ b/moveit_ros/move_group/package.xml @@ -1,6 +1,6 @@ moveit_ros_move_group - 1.1.6 + 1.1.7 The move_group node for MoveIt Ioan Sucan Sachin Chitta diff --git a/moveit_ros/moveit_ros/CHANGELOG.rst b/moveit_ros/moveit_ros/CHANGELOG.rst index 0fc302e0a1..15d87a877c 100644 --- a/moveit_ros/moveit_ros/CHANGELOG.rst +++ b/moveit_ros/moveit_ros/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ + 1.1.6 (2021-11-06) ------------------ diff --git a/moveit_ros/moveit_ros/package.xml b/moveit_ros/moveit_ros/package.xml index f33313f1a4..5f4c52cfcd 100644 --- a/moveit_ros/moveit_ros/package.xml +++ b/moveit_ros/moveit_ros/package.xml @@ -1,6 +1,6 @@ moveit_ros - 1.1.6 + 1.1.7 Components of MoveIt that use ROS Ioan Sucan Sachin Chitta diff --git a/moveit_ros/moveit_servo/CHANGELOG.rst b/moveit_ros/moveit_servo/CHANGELOG.rst index 48af78cadf..15d6a564f1 100644 --- a/moveit_ros/moveit_servo/CHANGELOG.rst +++ b/moveit_ros/moveit_servo/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_servo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ + 1.1.6 (2021-11-06) ------------------ * Backport position limit enforcement from MoveIt2 (`#2898 `_) diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml index bc27fc32de..baf4fcbd39 100644 --- a/moveit_ros/moveit_servo/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -1,7 +1,7 @@ moveit_servo - 1.1.6 + 1.1.7 Provides real-time manipulator Cartesian and joint servoing. diff --git a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst index 8783014b85..e69d4cabc8 100644 --- a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst +++ b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_occupancy_map_monitor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ +* Switch to ``std::bind`` (`#2967 `_) +* Contributors: Jochen Sprickerhof + 1.1.6 (2021-11-06) ------------------ * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` diff --git a/moveit_ros/occupancy_map_monitor/package.xml b/moveit_ros/occupancy_map_monitor/package.xml index 2daf9b3638..ef2dc0f6d1 100644 --- a/moveit_ros/occupancy_map_monitor/package.xml +++ b/moveit_ros/occupancy_map_monitor/package.xml @@ -1,7 +1,7 @@ moveit_ros_occupancy_map_monitor - 1.1.6 + 1.1.7 Components of MoveIt connecting to occupancy map Ioan Sucan diff --git a/moveit_ros/perception/CHANGELOG.rst b/moveit_ros/perception/CHANGELOG.rst index 2a07ed4e95..8f908dfd1c 100644 --- a/moveit_ros/perception/CHANGELOG.rst +++ b/moveit_ros/perception/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_ros_perception ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ +* Switch to ``std::bind`` (`#2967 `_) +* Add ns for depth image & pointcloud octomap updaters (`#2916 `_) +* Contributors: Jochen Sprickerhof, Tim Redick + 1.1.6 (2021-11-06) ------------------ * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` diff --git a/moveit_ros/perception/package.xml b/moveit_ros/perception/package.xml index 7509d0f741..66ecf21bf0 100644 --- a/moveit_ros/perception/package.xml +++ b/moveit_ros/perception/package.xml @@ -1,6 +1,6 @@ moveit_ros_perception - 1.1.6 + 1.1.7 Components of MoveIt connecting to perception Ioan Sucan diff --git a/moveit_ros/planning/CHANGELOG.rst b/moveit_ros/planning/CHANGELOG.rst index 5d62ec94d7..a0a7957d4b 100644 --- a/moveit_ros/planning/CHANGELOG.rst +++ b/moveit_ros/planning/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package moveit_ros_planning ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ +* Move ``MoveItErrorCode`` class to ``moveit_core`` (`#3009 `_) +* Switch to ``std::bind`` (`#2967 `_) +* ``MoveitCpp``: Added ability to set path constraints for ``PlanningComponent`` (`#2959 `_) +* ``RDFLoader``: clear buffer before reading content (`#2963 `_) +* Reset markers on ``display_contacts`` topic for a new planning attempt (`#2944 `_) +* Contributors: Colin Kohler, Jafar Abdi, Jochen Sprickerhof, Rick Staa, Robert Haschke + 1.1.6 (2021-11-06) ------------------ * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` diff --git a/moveit_ros/planning/package.xml b/moveit_ros/planning/package.xml index aaae5c0805..9e054bf46a 100644 --- a/moveit_ros/planning/package.xml +++ b/moveit_ros/planning/package.xml @@ -1,7 +1,7 @@ moveit_ros_planning - 1.1.6 + 1.1.7 Planning components of MoveIt that use ROS Ioan Sucan Sachin Chitta diff --git a/moveit_ros/planning_interface/CHANGELOG.rst b/moveit_ros/planning_interface/CHANGELOG.rst index 18f34c1b38..6188648952 100644 --- a/moveit_ros/planning_interface/CHANGELOG.rst +++ b/moveit_ros/planning_interface/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit_ros_planning_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ +* Move ``MoveItErrorCode`` class to ``moveit_core`` (`#3009 `_) +* Fix uninitialized ``RobotState`` in ``MoveGroupInterface`` (`#3008 `_) +* Switch to ``std::bind`` (`#2967 `_) +* Contributors: Captain Yoshi, Jafar Abdi, Jochen Sprickerhof + 1.1.6 (2021-11-06) ------------------ * Use test_environment.launch in unittests (`#2949 `_) diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml index 058d0cf408..e3e7af3c6e 100644 --- a/moveit_ros/planning_interface/package.xml +++ b/moveit_ros/planning_interface/package.xml @@ -1,7 +1,7 @@ moveit_ros_planning_interface - 1.1.6 + 1.1.7 Components of MoveIt that offer simpler interfaces to planning and execution Ioan Sucan diff --git a/moveit_ros/robot_interaction/CHANGELOG.rst b/moveit_ros/robot_interaction/CHANGELOG.rst index 5e4d171fdf..e4783aeade 100644 --- a/moveit_ros/robot_interaction/CHANGELOG.rst +++ b/moveit_ros/robot_interaction/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_ros_robot_interaction ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ +* Add marker for subgroups even if no endeffector is defined for them (`#2977 `_) +* Switch to ``std::bind`` (`#2967 `_) +* Contributors: Jochen Sprickerhof, Michael Görner + 1.1.6 (2021-11-06) ------------------ * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` diff --git a/moveit_ros/robot_interaction/package.xml b/moveit_ros/robot_interaction/package.xml index a6f592ef81..0ed25abf0a 100644 --- a/moveit_ros/robot_interaction/package.xml +++ b/moveit_ros/robot_interaction/package.xml @@ -1,6 +1,6 @@ moveit_ros_robot_interaction - 1.1.6 + 1.1.7 Components of MoveIt that offer interaction via interactive markers Ioan Sucan diff --git a/moveit_ros/visualization/CHANGELOG.rst b/moveit_ros/visualization/CHANGELOG.rst index 4d1935f035..3a62029f4a 100644 --- a/moveit_ros/visualization/CHANGELOG.rst +++ b/moveit_ros/visualization/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package moveit_ros_visualization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ +* Move ``MoveItErrorCode`` class to ``moveit_core`` (`#3009 `_) +* ``RobotState::attachBody``: Migrate to ``unique_ptr`` argument (`#3011 `_) +* Fix "ClassLoader: SEVERE WARNING" on reset of MPD (`#2925 `_) +* Switch to ``std::bind`` (`#2967 `_) +* Various fixes to MotionPlanning display (`#2944 `_) + + * Avoid flickering of the progress bar + * Joints widget: avoid flickering of the nullspace slider +* Modernize: std::make_shared +* Contributors: Jafar Abdi, JafarAbdi, Jochen Sprickerhof, Robert Haschke, pvanlaar + 1.1.6 (2021-11-06) ------------------ * Re-initialize params, subscribers, and topics when the ``MoveGroupNS`` has changed (`#2922 `_) diff --git a/moveit_ros/visualization/package.xml b/moveit_ros/visualization/package.xml index f4933f51b4..4c795e7a2c 100644 --- a/moveit_ros/visualization/package.xml +++ b/moveit_ros/visualization/package.xml @@ -1,6 +1,6 @@ moveit_ros_visualization - 1.1.6 + 1.1.7 Components of MoveIt that offer visualization Ioan Sucan diff --git a/moveit_ros/warehouse/CHANGELOG.rst b/moveit_ros/warehouse/CHANGELOG.rst index 03a4958608..301e9bbf8a 100644 --- a/moveit_ros/warehouse/CHANGELOG.rst +++ b/moveit_ros/warehouse/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_warehouse ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ +* Switch to ``std::bind`` (`#2967 `_) +* Contributors: Jochen Sprickerhof + 1.1.6 (2021-11-06) ------------------ * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` diff --git a/moveit_ros/warehouse/package.xml b/moveit_ros/warehouse/package.xml index 0e56a433cd..6d797a33e3 100644 --- a/moveit_ros/warehouse/package.xml +++ b/moveit_ros/warehouse/package.xml @@ -1,6 +1,6 @@ moveit_ros_warehouse - 1.1.6 + 1.1.7 Components of MoveIt connecting to MongoDB Ioan Sucan diff --git a/moveit_runtime/CHANGELOG.rst b/moveit_runtime/CHANGELOG.rst index 87365e3bc5..37f472be78 100644 --- a/moveit_runtime/CHANGELOG.rst +++ b/moveit_runtime/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_runtime ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ + 1.1.6 (2021-11-06) ------------------ diff --git a/moveit_runtime/package.xml b/moveit_runtime/package.xml index 0416e7fcd2..3408d3a568 100644 --- a/moveit_runtime/package.xml +++ b/moveit_runtime/package.xml @@ -1,7 +1,7 @@ moveit_runtime - 1.1.6 + 1.1.7 moveit_runtime meta package contains MoveIt packages that are essential for its runtime (e.g. running MoveIt on robots). Isaac I. Y. Saito diff --git a/moveit_setup_assistant/CHANGELOG.rst b/moveit_setup_assistant/CHANGELOG.rst index 9486988fc9..b522b008f2 100644 --- a/moveit_setup_assistant/CHANGELOG.rst +++ b/moveit_setup_assistant/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package moveit_setup_assistant ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.7 (2021-12-31) +------------------ +* Pass xacro_args to both, urdf and srdf loading (`#3013 `_) +* Switch to ``std::bind`` (`#2967 `_) +* Update timestamp of .setup_assistant file when writing files (`#2964 `_) +* Upload controller_list for simple controller manager (`#2954 `_) +* Contributors: Jochen Sprickerhof, Rick Staa, Robert Haschke + 1.1.6 (2021-11-06) ------------------ * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core`` diff --git a/moveit_setup_assistant/package.xml b/moveit_setup_assistant/package.xml index 456bb0559a..dee33a6a1a 100644 --- a/moveit_setup_assistant/package.xml +++ b/moveit_setup_assistant/package.xml @@ -1,6 +1,6 @@ moveit_setup_assistant - 1.1.6 + 1.1.7 Generates a configuration package that makes it easy to use MoveIt From 8ede06817671e5cb8800ebbdee9f8e2c19eec98c Mon Sep 17 00:00:00 2001 From: andreas-botbuilt <94128674+andreas-botbuilt@users.noreply.github.com> Date: Fri, 31 Dec 2021 01:43:40 -0500 Subject: [PATCH 146/229] collision_distance_field: Fix undefined behavior vector insertion (#942) --- .../src/collision_env_distance_field.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp index 02fdc5a2bf..9516d46c4d 100644 --- a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp @@ -942,8 +942,8 @@ DistanceFieldCacheEntryPtr CollisionEnvDistanceField::generateDistanceFieldCache for (collision_detection::PosedBodyPointDecompositionVectorPtr& non_group_attached_body_decomposition : non_group_attached_body_decompositions) { - all_points.insert(all_points.end(), non_group_attached_body_decomposition->getCollisionPoints().begin(), - non_group_attached_body_decomposition->getCollisionPoints().end()); + const EigenSTL::vector_Vector3d collision_points = non_group_attached_body_decomposition->getCollisionPoints(); + all_points.insert(all_points.end(), collision_points.begin(), collision_points.end()); } dfce->distance_field_->addPointsToField(all_points); From 2a71c9c936a2cdf3fe7a8cbd76a5ab01b2c67318 Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Fri, 31 Dec 2021 14:56:33 +0300 Subject: [PATCH 147/229] collision_distance_field: Fix undefined behavior vector insertion (#3017) Co-authored-by: andreas-botbuilt <94128674+andreas-botbuilt@users.noreply.github.com> --- .../src/collision_env_distance_field.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp index 5e2c34bfbe..135451c8c3 100644 --- a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp @@ -958,8 +958,8 @@ DistanceFieldCacheEntryPtr CollisionEnvDistanceField::generateDistanceFieldCache for (collision_detection::PosedBodyPointDecompositionVectorPtr& non_group_attached_body_decomposition : non_group_attached_body_decompositions) { - all_points.insert(all_points.end(), non_group_attached_body_decomposition->getCollisionPoints().begin(), - non_group_attached_body_decomposition->getCollisionPoints().end()); + const EigenSTL::vector_Vector3d collision_points = non_group_attached_body_decomposition->getCollisionPoints(); + all_points.insert(all_points.end(), collision_points.begin(), collision_points.end()); } dfce->distance_field_->addPointsToField(all_points); From 71d6e36b222e897f651fe02d88d231843f32f372 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Sat, 1 Jan 2022 16:17:27 -0700 Subject: [PATCH 148/229] Depend on the source of our own released repos (#949) --- moveit2.repos | 16 ++-------------- 1 file changed, 2 insertions(+), 14 deletions(-) diff --git a/moveit2.repos b/moveit2.repos index 073a40bfcd..903e175fdb 100644 --- a/moveit2.repos +++ b/moveit2.repos @@ -2,20 +2,8 @@ repositories: geometric_shapes: type: git url: https://github.com/ros-planning/geometric_shapes - version: ros2 + version: 2.1.2 moveit_msgs: type: git url: https://github.com/ros-planning/moveit_msgs - version: ros2 - moveit_resources: - type: git - url: https://github.com/ros-planning/moveit_resources - version: ros2 - warehouse_ros: - type: git - url: https://github.com/ros-planning/warehouse_ros - version: ros2 - warehouse_ros_mongo: - type: git - url: https://github.com/ros-planning/warehouse_ros_mongo - version: ros2 + version: 2.2.0 From 6141e3ae147c58f56f7c455ed93c96c02ac20462 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 2 Jan 2022 08:19:15 -0600 Subject: [PATCH 149/229] Put hybrid planning actions under a common namespace (#932) * Put hybrid planning actions under a common namespace * Use ~ * New pkg for common resources. Does not work. * Use inline rather than anonymous namespace Co-authored-by: Jafar Abdi * Revert "Use inline rather than anonymous namespace" This reverts commit 29a7d279776be21f4666c7e0fadeaa6b7ef8debf. * Revert "New pkg for common resources. Does not work." This reverts commit 68a173baee4b7f8b2c1f74285f96c8e3892c5909. * Some progress toward loading common parameters Co-authored-by: Jafar Abdi --- .../src/global_planner_component.cpp | 12 +++++-- .../src/hybrid_planning_manager.cpp | 31 ++++++++++++++++--- .../local_planner/local_planner_component.h | 2 ++ .../src/local_planner_component.cpp | 2 +- .../config/common_hybrid_planning_params.yaml | 6 ++++ .../test/hybrid_planning_demo_node.cpp | 16 ++++++++-- .../test/launch/hybrid_planning_common.py | 16 ++++++++-- 7 files changed, 73 insertions(+), 12 deletions(-) create mode 100644 moveit_ros/hybrid_planning/test/config/common_hybrid_planning_params.yaml diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp b/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp index e4e7ae6826..7cf42f4d85 100644 --- a/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp @@ -43,7 +43,7 @@ namespace { const rclcpp::Logger LOGGER = rclcpp::get_logger("global_planner_component"); -} +} // namespace namespace moveit::hybrid_planning { @@ -62,8 +62,16 @@ GlobalPlannerComponent::GlobalPlannerComponent(const rclcpp::NodeOptions& option bool GlobalPlannerComponent::initializeGlobalPlanner() { // Initialize global planning request action server + std::string global_planning_action_name = ""; + node_->declare_parameter("global_planning_action_name", ""); + node_->get_parameter("global_planning_action_name", global_planning_action_name); + if (global_planning_action_name.empty()) + { + RCLCPP_ERROR(LOGGER, "global_planning_action_name was not defined"); + return false; + } global_planning_request_server_ = rclcpp_action::create_server( - node_, "global_planning_action", + node_, global_planning_action_name, [](const rclcpp_action::GoalUUID& /*unused*/, std::shared_ptr /*unused*/) { RCLCPP_INFO(LOGGER, "Received global planning goal request"); diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp index f7c207cdf5..55ee267f84 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp @@ -38,7 +38,7 @@ namespace { const rclcpp::Logger LOGGER = rclcpp::get_logger("hybrid_planning_manager"); -} +} // namespace namespace moveit::hybrid_planning { @@ -105,8 +105,15 @@ bool HybridPlanningManager::initialize() } // Initialize local planning action client + std::string local_planning_action_name = this->declare_parameter("local_planning_action_name", ""); + this->get_parameter("local_planning_action_name", local_planning_action_name); + if (local_planning_action_name.empty()) + { + RCLCPP_ERROR(LOGGER, "local_planning_action_name parameter was not defined"); + return false; + } local_planner_action_client_ = - rclcpp_action::create_client(this, "local_planning_action"); + rclcpp_action::create_client(this, local_planning_action_name); if (!local_planner_action_client_->wait_for_action_server(2s)) { RCLCPP_ERROR(LOGGER, "Local planner action server not available after waiting"); @@ -114,8 +121,15 @@ bool HybridPlanningManager::initialize() } // Initialize global planning action client - global_planner_action_client_ = - rclcpp_action::create_client(this, "global_planning_action"); + std::string global_planning_action_name = this->declare_parameter("global_planning_action_name", ""); + this->get_parameter("global_planning_action_name", global_planning_action_name); + if (global_planning_action_name.empty()) + { + RCLCPP_ERROR(LOGGER, "global_planning_action_name parameter was not defined"); + return false; + } + global_planner_action_client_ = rclcpp_action::create_client( + this, "/test/hybrid_planning/global_planning_action" /*global_planning_action_name*/); if (!global_planner_action_client_->wait_for_action_server(2s)) { RCLCPP_ERROR(LOGGER, "Global planner action server not available after waiting"); @@ -123,9 +137,16 @@ bool HybridPlanningManager::initialize() } // Initialize hybrid planning action server + std::string hybrid_planning_action_name = this->declare_parameter("hybrid_planning_action_name", ""); + this->get_parameter("hybrid_planning_action_name", hybrid_planning_action_name); + if (hybrid_planning_action_name.empty()) + { + RCLCPP_ERROR(LOGGER, "hybrid_planning_action_name parameter was not defined"); + return false; + } hybrid_planning_request_server_ = rclcpp_action::create_server( this->get_node_base_interface(), this->get_node_clock_interface(), this->get_node_logging_interface(), - this->get_node_waitables_interface(), "run_hybrid_planning", + this->get_node_waitables_interface(), hybrid_planning_action_name, [](const rclcpp_action::GoalUUID& /*unused*/, std::shared_ptr /*unused*/) { RCLCPP_INFO(LOGGER, "Received goal request"); diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h index aec567b8a9..47d08d850d 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h @@ -111,6 +111,7 @@ class LocalPlannerComponent node); declareOrGetParam("local_constraint_solver_plugin_name", local_constraint_solver_plugin_name, undefined, node); + declareOrGetParam("local_planning_action_name", local_planning_action_name, undefined, node); declareOrGetParam("local_planning_frequency", local_planning_frequency, 1.0, node); declareOrGetParam("global_solution_topic", global_solution_topic, undefined, node); declareOrGetParam("local_solution_topic", local_solution_topic, undefined, node); @@ -125,6 +126,7 @@ class LocalPlannerComponent std::string publish_planning_scene_topic; std::string trajectory_operator_plugin_name; std::string local_constraint_solver_plugin_name; + std::string local_planning_action_name; std::string global_solution_topic; std::string local_solution_topic; std::string local_solution_topic_type; diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp index bf507a363e..b5ab3ac1d9 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp @@ -153,7 +153,7 @@ bool LocalPlannerComponent::initialize() // Initialize local planning request action server using namespace std::placeholders; local_planning_request_server_ = rclcpp_action::create_server( - node_, "local_planning_action", + node_, config_.local_planning_action_name, [](const rclcpp_action::GoalUUID& /*unused*/, std::shared_ptr /*unused*/) { RCLCPP_INFO(LOGGER, "Received local planning goal request"); diff --git a/moveit_ros/hybrid_planning/test/config/common_hybrid_planning_params.yaml b/moveit_ros/hybrid_planning/test/config/common_hybrid_planning_params.yaml new file mode 100644 index 0000000000..1825256505 --- /dev/null +++ b/moveit_ros/hybrid_planning/test/config/common_hybrid_planning_params.yaml @@ -0,0 +1,6 @@ +# Parameters that are shared across several component nodes + +# A namespace can be added if multiple hybrid planners are launched +global_planning_action_name: "/test/hybrid_planning/global_planning_action" +local_planning_action_name: "/test/hybrid_planning/local_planning_action" +hybrid_planning_action_name: "/test/hybrid_planning/run_hybrid_planning" diff --git a/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp b/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp index db66d4106d..8a4fa59cd8 100644 --- a/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp +++ b/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp @@ -56,7 +56,7 @@ using namespace std::chrono_literals; namespace { const rclcpp::Logger LOGGER = rclcpp::get_logger("test_hybrid_planning_client"); -} +} // namespace class HybridPlanningDemo { @@ -64,7 +64,19 @@ class HybridPlanningDemo HybridPlanningDemo(const rclcpp::Node::SharedPtr& node) { node_ = node; - hp_action_client_ = rclcpp_action::create_client(node_, "run_hybrid_planning"); + + std::string hybrid_planning_action_name = ""; + if (node_->has_parameter("hybrid_planning_action_name")) + { + node_->get_parameter("hybrid_planning_action_name", hybrid_planning_action_name); + } + else + { + RCLCPP_ERROR(LOGGER, "hybrid_planning_action_name parameter was not defined"); + std::exit(EXIT_FAILURE); + } + hp_action_client_ = + rclcpp_action::create_client(node_, hybrid_planning_action_name); robot_state_publisher_ = node_->create_publisher("display_robot_state", 1); collision_object_1_.header.frame_id = "panda_link0"; diff --git a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py index c9eb8b0939..e9b71a7638 100644 --- a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py +++ b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py @@ -77,6 +77,9 @@ def generate_common_hybrid_launch_description(): } # Any parameters that are unique to your plugins go here + common_hybrid_planning_param = load_yaml( + "moveit_hybrid_planning", "config/common_hybrid_planning_params.yaml" + ) global_planner_param = load_yaml( "moveit_hybrid_planning", "config/global_planner.yaml" ) @@ -99,6 +102,7 @@ def generate_common_hybrid_launch_description(): plugin="moveit::hybrid_planning::GlobalPlannerComponent", name="global_planner", parameters=[ + common_hybrid_planning_param, global_planner_param, robot_description, robot_description_semantic, @@ -112,6 +116,7 @@ def generate_common_hybrid_launch_description(): plugin="moveit::hybrid_planning::LocalPlannerComponent", name="local_planner", parameters=[ + common_hybrid_planning_param, local_planner_param, robot_description, robot_description_semantic, @@ -122,7 +127,10 @@ def generate_common_hybrid_launch_description(): package="moveit_hybrid_planning", plugin="moveit::hybrid_planning::HybridPlanningManager", name="hybrid_planning_manager", - parameters=[hybrid_planning_manager_param], + parameters=[ + common_hybrid_planning_param, + hybrid_planning_manager_param, + ], ), ], output="screen", @@ -196,7 +204,11 @@ def generate_common_hybrid_launch_description(): executable="hybrid_planning_demo_node", name="hybrid_planning_demo_node", output="screen", - parameters=[robot_description, robot_description_semantic], + parameters=[ + robot_description, + robot_description_semantic, + common_hybrid_planning_param, + ], ) launched_nodes = [ From a9cc2bb6544d449c162268637300d038bc063a55 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 2 Jan 2022 12:36:10 -0600 Subject: [PATCH 150/229] Add jerk to the robot model (#683) * Add jerk to the robot model * Add joint limit parsing to a unit test * Add jerk to computeVariableBoundsMsg and <<, too --- .../include/moveit/robot_model/joint_model.h | 7 ++++++ moveit_core/robot_model/src/joint_model.cpp | 13 ++++++++++ moveit_core/robot_model/test/test.cpp | 12 ++++++++++ .../robot_model_loader/CMakeLists.txt | 1 + .../src/robot_model_loader.cpp | 24 +++++++++++++++++++ 5 files changed, 57 insertions(+) diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h index 4e5117a1f5..69fd8e502e 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h @@ -63,6 +63,9 @@ struct VariableBounds , min_acceleration_(0.0) , max_acceleration_(0.0) , acceleration_bounded_(false) + , min_jerk_(0.0) + , max_jerk_(0.0) + , jerk_bounded_(false) { } @@ -77,6 +80,10 @@ struct VariableBounds double min_acceleration_; double max_acceleration_; bool acceleration_bounded_; + + double min_jerk_; + double max_jerk_; + bool jerk_bounded_; }; class LinkModel; diff --git a/moveit_core/robot_model/src/joint_model.cpp b/moveit_core/robot_model/src/joint_model.cpp index e7224ab281..c3b9fea999 100644 --- a/moveit_core/robot_model/src/joint_model.cpp +++ b/moveit_core/robot_model/src/joint_model.cpp @@ -157,6 +157,12 @@ void JointModel::setVariableBounds(const std::vectorgetLinkIndex(), static_cast(i)); } moveit::tools::Profiler::Status(); + + // This joint has effort and velocity limits defined in the URDF. Nothing else. + const std::string joint_name = "fl_caster_rotation_joint"; + const auto& joint = robot_model_->getJointModel(joint_name); + const auto& bounds = joint->getVariableBounds(joint->getName()); + + EXPECT_TRUE(bounds.velocity_bounded_); + EXPECT_EQ(bounds.max_velocity_, 10.0); + + EXPECT_FALSE(bounds.position_bounded_); + EXPECT_FALSE(bounds.acceleration_bounded_); + EXPECT_FALSE(bounds.jerk_bounded_); } TEST(SiblingAssociateLinks, SimpleYRobot) diff --git a/moveit_ros/planning/robot_model_loader/CMakeLists.txt b/moveit_ros/planning/robot_model_loader/CMakeLists.txt index 7ea0234fd2..45b65f64be 100644 --- a/moveit_ros/planning/robot_model_loader/CMakeLists.txt +++ b/moveit_ros/planning/robot_model_loader/CMakeLists.txt @@ -11,6 +11,7 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} urdf Boost moveit_core + moveit_msgs ) target_link_libraries(${MOVEIT_LIB_NAME} moveit_rdf_loader diff --git a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp index 5956d559ea..147090af8c 100644 --- a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp +++ b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp @@ -180,6 +180,15 @@ void RobotModelLoader::configure(const Options& opt) if (node_->get_parameter(param_name, has_acc_limits)) joint_limit[joint_id].has_acceleration_limits = has_acc_limits; + param_name = prefix + "has_jerk_limits"; + if (!node_->has_parameter(param_name)) + { + node_->declare_parameter(param_name, rclcpp::ParameterType::PARAMETER_BOOL); + } + bool has_jerk_limits = false; + if (node_->get_parameter(param_name, has_jerk_limits)) + joint_limit[joint_id].has_jerk_limits = has_jerk_limits; + if (has_vel_limits) { param_name = prefix + "max_velocity"; @@ -209,6 +218,21 @@ void RobotModelLoader::configure(const Options& opt) joint_limit[joint_id].joint_name.c_str()); } } + + if (has_jerk_limits) + { + param_name = prefix + "max_jerk"; + if (!node_->has_parameter(param_name)) + { + node_->declare_parameter(param_name, rclcpp::ParameterType::PARAMETER_DOUBLE); + } + + if (!node_->get_parameter(param_name, joint_limit[joint_id].max_jerk)) + { + RCLCPP_ERROR(LOGGER, "Specified a jerk limit for joint: %s but did not set a max jerk", + joint_limit[joint_id].joint_name.c_str()); + } + } } catch (const rclcpp::ParameterTypeException& e) { From ab230fb6adcf16317f268915e686c03dfb3c8ca6 Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Tue, 4 Jan 2022 00:02:46 +0300 Subject: [PATCH 151/229] Add rosdistro version to upstream ws cache prefix (#952) --- .github/workflows/ci.yaml | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 699fa11c98..d58731fc89 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -18,11 +18,15 @@ jobs: env: - IMAGE: rolling-ci CCOV: true + ROS_DISTRO: rolling - IMAGE: rolling-ci-testing IKFAST_TEST: true + ROS_DISTRO: rolling - IMAGE: galactic-ci CLANG_TIDY: pedantic + ROS_DISTRO: galactic - IMAGE: galactic-ci-testing + ROS_DISTRO: galactic env: CXXFLAGS: "-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-deprecated-copy" DOCKER_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.env.IMAGE }} @@ -66,6 +70,11 @@ jobs: sudo rm -rf /usr/local df -h - uses: actions/checkout@v2 + - name: Get latest release date for rosdistro + id: rosdistro_release_date + uses: JafarAbdi/latest-rosdistro-release-date-action@main + with: + rosdistro: ${{ matrix.env.ROS_DISTRO }} - name: Cache upstream workspace uses: pat-s/always-upload-cache@v2.1.5 with: @@ -73,7 +82,7 @@ jobs: key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }} restore-keys: ${{ env.CACHE_PREFIX }} env: - CACHE_PREFIX: upstream_ws-${{ matrix.env.IMAGE }}-${{ hashFiles('moveit2*.repos', '.github/workflows/ci.yaml') }} + CACHE_PREFIX: ${{ steps.rosdistro_release_date.outputs.date }}-upstream_ws-${{ matrix.env.IMAGE }}-${{ hashFiles('moveit2*.repos', '.github/workflows/ci.yaml') }} # The target directory cache doesn't include the source directory because # that comes from the checkout. See "prepare target_ws for cache" task below - name: Cache target workspace From ac6b7d6be82fc272a1e65c557e227e5a4a2447c8 Mon Sep 17 00:00:00 2001 From: Jeroen Date: Tue, 4 Jan 2022 23:16:33 +0100 Subject: [PATCH 152/229] Fix deprecation warning in moveit_cpp (#3019) Fixup for #3009. --- moveit_ros/planning/moveit_cpp/src/planning_component.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index 515e186394..bdb4e6bbf0 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -111,7 +111,7 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet if (!joint_model_group_) { ROS_ERROR_NAMED(LOGNAME, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str()); - last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME); + last_plan_solution_->error_code = moveit::core::MoveItErrorCode::INVALID_GROUP_NAME; return *last_plan_solution_; } @@ -148,7 +148,7 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet if (current_goal_constraints_.empty()) { ROS_ERROR_NAMED(LOGNAME, "No goal constraints set for planning request"); - last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + last_plan_solution_->error_code = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS; return *last_plan_solution_; } req.goal_constraints = current_goal_constraints_; @@ -161,7 +161,7 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet if (planning_pipeline_names_.find(parameters.planning_pipeline) == planning_pipeline_names_.end()) { ROS_ERROR_NAMED(LOGNAME, "No planning pipeline available for name '%s'", parameters.planning_pipeline.c_str()); - last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + last_plan_solution_->error_code = moveit::core::MoveItErrorCode::FAILURE; return *last_plan_solution_; } const planning_pipeline::PlanningPipelinePtr pipeline = From 5a1a2f138068b55f65cfef838bbb7947d95fa579 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 5 Jan 2022 03:45:52 -0600 Subject: [PATCH 153/229] hybrid_planning: Fix global_planner action name (#960) --- .../src/hybrid_planning_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp index 55ee267f84..30d65b90cd 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp @@ -128,8 +128,8 @@ bool HybridPlanningManager::initialize() RCLCPP_ERROR(LOGGER, "global_planning_action_name parameter was not defined"); return false; } - global_planner_action_client_ = rclcpp_action::create_client( - this, "/test/hybrid_planning/global_planning_action" /*global_planning_action_name*/); + global_planner_action_client_ = + rclcpp_action::create_client(this, global_planning_action_name); if (!global_planner_action_client_->wait_for_action_server(2s)) { RCLCPP_ERROR(LOGGER, "Global planner action server not available after waiting"); From 334da75c3c91e39dbde81df2436dc3fe08285134 Mon Sep 17 00:00:00 2001 From: Stephanie Eng Date: Thu, 6 Jan 2022 10:47:35 -0500 Subject: [PATCH 154/229] Replace NULL with nullptr (#961) * Fixes #841 --- .../collision_detection/collision_env.h | 2 +- .../collision_common.h | 4 ++-- .../src/default_constraint_samplers.cpp | 2 +- .../propagation_distance_field.h | 2 +- .../moveit/distance_field/voxel_grid.h | 2 +- .../kinematic_constraint.h | 4 ++-- .../moveit/planning_scene/planning_scene.h | 16 +++++++------- .../planning_scene/src/planning_scene.cpp | 8 +++---- .../include/moveit/robot_model/joint_model.h | 4 ++-- .../moveit/robot_model/joint_model_group.h | 2 +- .../include/moveit/robot_model/link_model.h | 4 ++-- .../include/moveit/robot_model/robot_model.h | 12 +++++----- .../include/moveit/robot_state/robot_state.h | 2 +- .../src/kinematics_constraint_aware.cpp | 6 ++--- .../templates/ikfast.h | 22 +++++++++---------- .../src/chomp_optimizer.cpp | 4 ++-- .../sbpl_interface/dummy_environment.h | 2 +- .../sbpl_interface/environment_chain3d.h | 2 +- .../environment_chain3d_types.h | 6 ++--- .../src/environment_chain3d.cpp | 10 ++++----- .../include/ikfast.h | 22 +++++++++---------- .../src/prbt_manipulator_ikfast_solver.cpp | 6 ++--- .../manipulation/pick_place/src/place.cpp | 2 +- .../src/occupancy_map_monitor.cpp | 2 +- .../src/kinematics_plugin_loader.cpp | 2 +- .../moveit/py_bindings_tools/serialize_msg.h | 2 +- .../robot_interaction/locked_robot_state.h | 2 +- .../src/widgets/planning_groups_widget.cpp | 2 +- .../src/widgets/ros_controllers_widget.cpp | 2 +- 29 files changed, 79 insertions(+), 79 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h index 9581cdfc51..c846274021 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h @@ -233,7 +233,7 @@ class CollisionEnv /** set the world to use. * This can be expensive unless the new and old world are empty. - * Passing NULL will result in a new empty world being created. */ + * Passing nullptr will result in a new empty world being created. */ virtual void setWorld(const WorldPtr& world); /** access the world geometry */ diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h index 15403391df..ee5a00beab 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h @@ -162,13 +162,13 @@ struct CollisionData /** \brief If the collision request includes a group name, this set contains the pointers to the link models that * are considered for collision. * - * If the pointer is NULL, all collisions are considered. */ + * If the pointer is nullptr, all collisions are considered. */ const std::set* active_components_only_; /** \brief The user-specified response location. */ CollisionResult* res_; - /** \brief The user-specified collision matrix (may be NULL). */ + /** \brief The user-specified collision matrix (may be nullptr). */ const AllowedCollisionMatrix* acm_; /** \brief Flag indicating whether collision checking is complete. */ diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index e6d17cc542..4dac6535f8 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -61,7 +61,7 @@ bool JointConstraintSampler::configure(const std::vector::VoxelGrid(double size_x, double size_y, double size_z, double reso } template -VoxelGrid::VoxelGrid() : data_(NULL) +VoxelGrid::VoxelGrid() : data_(nullptr) { for (int i = DIM_X; i <= DIM_Z; ++i) { diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h index 5c2a9bd6d9..9edc00fc43 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h @@ -253,7 +253,7 @@ class JointConstraint : public KinematicConstraint /** * \brief Get the joint model for which this constraint operates * - * @return The relevant joint model if enabled, and otherwise NULL + * @return The relevant joint model if enabled, and otherwise nullptr */ const moveit::core::JointModel* getJointModel() const { @@ -576,7 +576,7 @@ class PositionConstraint : public KinematicConstraint void print(std::ostream& out = std::cout) const override; /** - * \brief Returns the associated link model, or NULL if not enabled + * \brief Returns the associated link model, or nullptr if not enabled * * * @return The link model diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index 6093f646bf..5b3c06099a 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -968,7 +968,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : private boost::noncopyable, MOVEIT_STRUCT_FORWARD(CollisionDetector); - /* Construct a new CollisionDector from allocator, copy-construct environments from parent_detector if not null */ + /* Construct a new CollisionDector from allocator, copy-construct environments from parent_detector if not nullptr */ void allocateCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator, const CollisionDetectorPtr& parent_detector); @@ -976,7 +976,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : private boost::noncopyable, struct CollisionDetector { collision_detection::CollisionDetectorAllocatorPtr alloc_; - collision_detection::CollisionEnvPtr cenv_; // never NULL + collision_detection::CollisionEnvPtr cenv_; // never nullptr collision_detection::CollisionEnvConstPtr cenv_const_; collision_detection::CollisionEnvPtr cenv_unpadded_; @@ -1000,24 +1000,24 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : private boost::noncopyable, moveit::core::RobotModelConstPtr robot_model_; // Never null (may point to same model as parent) - moveit::core::RobotStatePtr robot_state_; // if NULL use parent's + moveit::core::RobotStatePtr robot_state_; // if nullptr use parent's // Called when changes are made to attached bodies moveit::core::AttachedBodyCallback current_state_attached_body_callback_; // This variable is not necessarily used by child planning scenes // This Transforms class is actually a SceneTransforms class - moveit::core::TransformsPtr scene_transforms_; // if NULL use parent's + moveit::core::TransformsPtr scene_transforms_; // if nullptr use parent's - collision_detection::WorldPtr world_; // never NULL, never shared with parent/child + collision_detection::WorldPtr world_; // never nullptr, never shared with parent/child collision_detection::WorldConstPtr world_const_; // copy of world_ - collision_detection::WorldDiffPtr world_diff_; // NULL unless this is a diff scene + collision_detection::WorldDiffPtr world_diff_; // nullptr unless this is a diff scene collision_detection::World::ObserverCallbackFn current_world_object_update_callback_; collision_detection::World::ObserverHandle current_world_object_update_observer_handle_; - CollisionDetectorPtr collision_detector_; // Never NULL. + CollisionDetectorPtr collision_detector_; // Never nullptr. - collision_detection::AllowedCollisionMatrixPtr acm_; // if NULL use parent's + collision_detection::AllowedCollisionMatrixPtr acm_; // if nullptr use parent's StateFeasibilityFn state_feasibility_; MotionFeasibilityFn motion_feasibility_; diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index c2e6a1a25e..ec1848afae 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -128,10 +128,10 @@ PlanningScene::PlanningScene(const urdf::ModelInterfaceSharedPtr& urdf_model, : world_(world), world_const_(world) { if (!urdf_model) - throw moveit::ConstructException("The URDF model cannot be NULL"); + throw moveit::ConstructException("The URDF model cannot be nullptr"); if (!srdf_model) - throw moveit::ConstructException("The SRDF model cannot be NULL"); + throw moveit::ConstructException("The SRDF model cannot be nullptr"); robot_model_ = createRobotModel(urdf_model, srdf_model); if (!robot_model_) @@ -169,7 +169,7 @@ void PlanningScene::initialize() allocateCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create()); } -/* return NULL on failure */ +/* return nullptr on failure */ moveit::core::RobotModelPtr PlanningScene::createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model) { @@ -183,7 +183,7 @@ moveit::core::RobotModelPtr PlanningScene::createRobotModel(const urdf::ModelInt PlanningScene::PlanningScene(const PlanningSceneConstPtr& parent) : parent_(parent) { if (!parent_) - throw moveit::ConstructException("NULL parent pointer for planning scene"); + throw moveit::ConstructException("nullptr parent pointer for planning scene"); if (!parent_->getName().empty()) name_ = parent_->getName() + "+"; diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h index 69fd8e502e..9467c72548 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h @@ -152,7 +152,7 @@ class JointModel /** \brief Get the link that this joint connects to. The robot is assumed to start with a joint, so the root - joint will return a NULL pointer here. */ + joint will return nullptr here. */ const LinkModel* getParentLinkModel() const { return parent_link_model_; @@ -492,7 +492,7 @@ class JointModel /** \brief The link after this joint */ const LinkModel* child_link_model_; - /** \brief The joint this one mimics (NULL for joints that do not mimic) */ + /** \brief The joint this one mimics (nullptr for joints that do not mimic) */ const JointModel* mimic_; /** \brief The offset to the mimic joint */ 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 655d749553..49bfbf1b1a 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 @@ -510,7 +510,7 @@ class JointModelGroup /** * \brief Get one end effector tip, throwing an error if there ends up being more in the joint model group * This is a useful helper function because most planning groups (almost all) only have one tip - * \return pointer to LinkModel, or NULL on failure + * \return pointer to LinkModel, or nullptr on failure */ const moveit::core::LinkModel* getOnlyOneEndEffectorTip() const; diff --git a/moveit_core/robot_model/include/moveit/robot_model/link_model.h b/moveit_core/robot_model/include/moveit/robot_model/link_model.h index 3bd2dfbe00..eee845316e 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/link_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/link_model.h @@ -112,7 +112,7 @@ class LinkModel void setParentJointModel(const JointModel* joint); /** \brief Get the link model whose child this link is (through some joint). There may not always be a parent link - * (NULL is returned for the root link) */ + * (nullptr is returned for the root link) */ const LinkModel* getParentLinkModel() const { return parent_link_model_; @@ -234,7 +234,7 @@ class LinkModel /** \brief JointModel that connects this link to the parent link */ const JointModel* parent_joint_model_; - /** \brief The parent link model (NULL for the root link) */ + /** \brief The parent link model (nullptr for the root link) */ const LinkModel* parent_link_model_; /** \brief List of directly descending joints (each connects to a child link) */ diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h index d23050ea89..75b40bfdab 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h @@ -135,13 +135,13 @@ class RobotModel /** \brief Check if a joint exists. Return true if it does. */ bool hasJointModel(const std::string& name) const; - /** \brief Get a joint by its name. Output error and return NULL when the joint is missing. */ + /** \brief Get a joint by its name. Output error and return nullptr when the joint is missing. */ const JointModel* getJointModel(const std::string& joint) const; - /** \brief Get a joint by its index. Output error and return NULL when the link is missing. */ + /** \brief Get a joint by its index. Output error and return nullptr when the link is missing. */ const JointModel* getJointModel(int index) const; - /** \brief Get a joint by its name. Output error and return NULL when the joint is missing. */ + /** \brief Get a joint by its name. Output error and return nullptr when the joint is missing. */ JointModel* getJointModel(const std::string& joint); /** \brief Get the array of joints, in the order they appear @@ -244,13 +244,13 @@ class RobotModel * If this is followed by a call to getLinkModel(), better use the latter with the has_link argument */ bool hasLinkModel(const std::string& name) const; - /** \brief Get a link by its name. Output error and return NULL when the link is missing. */ + /** \brief Get a link by its name. Output error and return nullptr when the link is missing. */ const LinkModel* getLinkModel(const std::string& link, bool* has_link = nullptr) const; - /** \brief Get a link by its index. Output error and return NULL when the link is missing. */ + /** \brief Get a link by its index. Output error and return nullptr when the link is missing. */ const LinkModel* getLinkModel(int index) const; - /** \brief Get a link by its name. Output error and return NULL when the link is missing. */ + /** \brief Get a link by its name. Output error and return nullptr when the link is missing. */ LinkModel* getLinkModel(const std::string& link, bool* has_link = nullptr); /** \brief Get the latest link upwards the kinematic tree, which is only connected via fixed joints diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index 485cc61e89..4f31171a80 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -1660,7 +1660,7 @@ class RobotState /** \brief Clear all attached bodies. This calls delete on the AttachedBody instances, if needed. */ void clearAttachedBodies(); - /** \brief Get the attached body named \e name. Return NULL if not found. */ + /** \brief Get the attached body named \e name. Return nullptr if not found. */ const AttachedBody* getAttachedBody(const std::string& name) const; /** \brief Check if an attached body named \e id exists in this state */ diff --git a/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp b/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp index e24bdebf40..605b1dfa95 100644 --- a/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp +++ b/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp @@ -55,7 +55,7 @@ KinematicsConstraintAware::KinematicsConstraintAware(const moveit::core::RobotMo if (!kinematic_model->hasJointModelGroup(group_name)) { ROS_ERROR_NAMED("kinematics_constraint_aware", "The group %s does not exist", group_name.c_str()); - joint_model_group_ = NULL; + joint_model_group_ = nullptr; return; } kinematic_model_ = kinematic_model; @@ -90,13 +90,13 @@ KinematicsConstraintAware::KinematicsConstraintAware(const moveit::core::RobotMo } else { - joint_model_group_ = NULL; + joint_model_group_ = nullptr; return; } } else { - joint_model_group_ = NULL; + joint_model_group_ = nullptr; ROS_INFO_NAMED("kinematics_constraint_aware", "No solver allocated for group %s", group_name.c_str()); } has_sub_groups_ = true; diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast.h b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast.h index 84995fa1d3..508400aa67 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast.h +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast.h @@ -86,7 +86,7 @@ class IkSolutionBase virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const { solution.resize(GetDOF()); - GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); + GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : nullptr); } /// \brief Gets the indices of the configuration space that have to be preset before a full solution can be returned @@ -131,15 +131,15 @@ class IkFastFunctions { public: IkFastFunctions() - : _ComputeIk(NULL) - , _ComputeFk(NULL) - , _GetNumFreeParameters(NULL) - , _GetFreeParameters(NULL) - , _GetNumJoints(NULL) - , _GetIkRealSize(NULL) - , _GetIkFastVersion(NULL) - , _GetIkType(NULL) - , _GetKinematicsHash(NULL) + : _ComputeIk(nullptr) + , _ComputeFk(nullptr) + , _GetNumFreeParameters(nullptr) + , _GetFreeParameters(nullptr) + , _GetNumJoints(nullptr) + , _GetIkRealSize(nullptr) + , _GetIkFastVersion(nullptr) + , _GetIkType(nullptr) + , _GetKinematicsHash(nullptr) { } virtual ~IkFastFunctions() @@ -202,7 +202,7 @@ class IkSolution : public IkSolutionBase virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const { solution.resize(GetDOF()); - GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); + GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : nullptr); } virtual const std::vector& GetFree() const diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp index ce929ba577..416d1df90b 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp @@ -273,12 +273,12 @@ void ChompOptimizer::registerParents(const moveit::core::JointModel* model) { if (model->getParentLinkModel() == nullptr) { - RCLCPP_ERROR(LOGGER, "Model %s not root but has NULL link model parent", model->getName().c_str()); + RCLCPP_ERROR(LOGGER, "Model %s not root but has nullptr link model parent", model->getName().c_str()); return; } else if (model->getParentLinkModel()->getParentJointModel() == nullptr) { - RCLCPP_ERROR(LOGGER, "Model %s not root but has NULL joint model parent", model->getName().c_str()); + RCLCPP_ERROR(LOGGER, "Model %s not root but has nullptr joint model parent", model->getName().c_str()); return; } parent_model = model->getParentLinkModel()->getParentJointModel(); diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/dummy_environment.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/dummy_environment.h index b2da4714d4..013ac31878 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/dummy_environment.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/dummy_environment.h @@ -79,7 +79,7 @@ class DummyEnvironment : public DiscreteSpaceInformation virtual int SizeofCreatedEnv(){}; /** \brief prints the state variables for a state with stateID */ - virtual void PrintState(int stateID, bool bVerbose, FILE* fOut = NULL){}; + virtual void PrintState(int stateID, bool bVerbose, FILE* fOut = nullptr){}; /** \brief prints environment config file */ virtual void PrintEnv_Config(FILE* fOut){}; diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d.h index 744f84d077..06904e92d1 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d.h @@ -181,7 +181,7 @@ class EnvironmentChain3D : public DiscreteSpaceInformation * @param prints out a little extra information * @param the file pointer to print to (stdout by default) */ - virtual void PrintState(int stateID, bool bVerbose, FILE* fOut = NULL); + virtual void PrintState(int stateID, bool bVerbose, FILE* fOut = nullptr); /** @brief Not defined. */ virtual void PrintEnv_Config(FILE* fOut); diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d_types.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d_types.h index 8f69a8d921..387d70af77 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d_types.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d_types.h @@ -88,8 +88,8 @@ struct EnvChain3DPlanningData { EnvChain3DPlanningData(std::vector& state_ID_to_index_mapping) : state_ID_to_index_mapping_(state_ID_to_index_mapping) - , goal_hash_entry_(NULL) - , start_hash_entry_(NULL) + , goal_hash_entry_(nullptr) + , start_hash_entry_(nullptr) , hash_table_size_(HASH_TABLE_SIZE) { coord_to_state_ID_table_.resize(hash_table_size_); @@ -149,7 +149,7 @@ struct EnvChain3DPlanningData return coord_to_state_ID_table_[bin][i]; } } - return NULL; + return nullptr; } bool convertFromStateIDsToAngles(const std::vector& state_ids, diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp index 3dd78b4004..c7753b3c0c 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp @@ -49,7 +49,7 @@ namespace sbpl_interface { EnvironmentChain3D::EnvironmentChain3D(const planning_scene::PlanningSceneConstPtr& planning_scene) : planning_scene_(planning_scene) - , bfs_(NULL) + , bfs_(nullptr) , state_(planning_scene->getCurrentState()) , planning_data_(StateID2IndexMapping) , goal_constraint_set_(planning_scene->getRobotModel(), planning_scene->getTransforms()) @@ -63,7 +63,7 @@ EnvironmentChain3D::EnvironmentChain3D(const planning_scene::PlanningSceneConstP EnvironmentChain3D::~EnvironmentChain3D() { - if (bfs_ != NULL) + if (bfs_ != nullptr) { delete bfs_; } @@ -116,9 +116,9 @@ int EnvironmentChain3D::SizeofCreatedEnv() return planning_data_.state_ID_to_coord_table_.size(); } -void EnvironmentChain3D::PrintState(int stateID, bool bVerbose, FILE* fOut /*=NULL*/) +void EnvironmentChain3D::PrintState(int stateID, bool bVerbose, FILE* fOut /*=nullptr*/) { - // if(fOut == NULL) + // if(fOut == nullptr) // fOut = stdout; // EnvChain3DHashEntry* HashEntry = EnvChain.StateID2CoordTable[stateID]; @@ -287,7 +287,7 @@ void EnvironmentChain3D::GetSuccs(int source_state_ID, std::vector* succ_id planning_parameters_.joint_motion_primitive_distance_); } - EnvChain3DHashEntry* succ_hash_entry = NULL; + EnvChain3DHashEntry* succ_hash_entry = nullptr; // bool can_get_closer = false; // for(unsigned int j = 0; j < joint_motion_wrappers_.size(); ++j) { // if(joint_motion_wrappers_[j]->canGetCloser(succ_joint_angles[j], diff --git a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/include/ikfast.h b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/include/ikfast.h index e672c974af..0a4f7e70e9 100644 --- a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/include/ikfast.h +++ b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/include/ikfast.h @@ -83,7 +83,7 @@ class IkSolutionBase virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const { solution.resize(GetDOF()); - GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); + GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : nullptr); } /// \brief Gets the indices of the configuration space that have to be preset before a full solution can be returned @@ -128,15 +128,15 @@ class IkFastFunctions { public: IkFastFunctions() - : _ComputeIk(NULL) - , _ComputeFk(NULL) - , _GetNumFreeParameters(NULL) - , _GetFreeParameters(NULL) - , _GetNumJoints(NULL) - , _GetIkRealSize(NULL) - , _GetIkFastVersion(NULL) - , _GetIkType(NULL) - , _GetKinematicsHash(NULL) + : _ComputeIk(nullptr) + , _ComputeFk(nullptr) + , _GetNumFreeParameters(nullptr) + , _GetFreeParameters(nullptr) + , _GetNumJoints(nullptr) + , _GetIkRealSize(nullptr) + , _GetIkFastVersion(nullptr) + , _GetIkType(nullptr) + , _GetKinematicsHash(nullptr) { } virtual ~IkFastFunctions() @@ -199,7 +199,7 @@ class IkSolution : public IkSolutionBase virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const { solution.resize(GetDOF()); - GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); + GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : nullptr); } virtual const std::vector& GetFree() const diff --git a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_solver.cpp b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_solver.cpp index 0841c91a7f..8359d75287 100644 --- a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_solver.cpp +++ b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_solver.cpp @@ -356,7 +356,7 @@ eetrans[2]=((0.2604)+(((0.35)*x1))+((x29*(((((-0.084)*x24))+((x15*x2))))))+((x11 } IKFAST_API int GetNumFreeParameters() { return 0; } -IKFAST_API int* GetFreeParameters() { return NULL; } +IKFAST_API int* GetFreeParameters() { return nullptr; } IKFAST_API int GetNumJoints() { return 6; } IKFAST_API int GetIkRealSize() { return sizeof(IkReal); } @@ -20093,7 +20093,7 @@ int main(int argc, char** argv) eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]); for(std::size_t i = 0; i < vfree.size(); ++i) vfree[i] = atof(argv[13+i]); - bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions); + bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : nullptr, solutions); if( !bSuccess ) { fprintf(stderr,"Failed to get ik solution\n"); @@ -20106,7 +20106,7 @@ int main(int argc, char** argv) const IkSolutionBase& sol = solutions.GetSolution(i); printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size()); std::vector vsolfree(sol.GetFree().size()); - sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL); + sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:nullptr); for( std::size_t j = 0; j < solvalues.size(); ++j) printf("%.15f, ", solvalues[j]); printf("\n"); diff --git a/moveit_ros/manipulation/pick_place/src/place.cpp b/moveit_ros/manipulation/pick_place/src/place.cpp index f7e404aff4..0852bf2aab 100644 --- a/moveit_ros/manipulation/pick_place/src/place.cpp +++ b/moveit_ros/manipulation/pick_place/src/place.cpp @@ -181,7 +181,7 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME; return false; } - // set the end effector (this was initialized to NULL above) + // set the end effector (this was initialized to nullptr above) eef = planning_scene->getRobotModel()->getEndEffector(eef_name); } } diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp index 4d921d5b58..846a807450 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp @@ -171,7 +171,7 @@ void OccupancyMapMonitor::addUpdater(const OccupancyMapUpdaterPtr& updater) updater->setTransformCacheCallback(transform_cache_callback_); } else - RCLCPP_ERROR(LOGGER, "NULL updater was specified"); + RCLCPP_ERROR(LOGGER, "nullptr updater was specified"); } void OccupancyMapMonitor::publishDebugInformation(bool flag) diff --git a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp index f718f3f460..7f10364519 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp +++ b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp @@ -148,7 +148,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl } if (!jmg) { - RCLCPP_ERROR(LOGGER, "Specified group is NULL. Cannot allocate kinematics solver."); + RCLCPP_ERROR(LOGGER, "Specified group is nullptr. Cannot allocate kinematics solver."); return result; } const std::vector& links = jmg->getLinkModels(); diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h index 7f72363231..61514fdb60 100644 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h +++ b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h @@ -87,7 +87,7 @@ class ByteString : public boost::python::object { static_assert(sizeof(uint8_t) == sizeof(char), "ros/python buffer layout mismatch"); char* buf = PyBytes_AsString(ptr()); - // buf == NULL on error + // buf == nullptr on error if (!buf) { throw std::runtime_error("Underlying python object is not a Bytes/String instance"); diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h index ebd2f72de1..ff165ae0e0 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h @@ -103,7 +103,7 @@ class LockedRobotState private: // The state maintained by this class. - // When a modify function is being called this is NULL. + // When a modify function is being called this is nullptr. // PROTECTED BY state_lock_ moveit::core::RobotStatePtr state_; }; diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp index a590ffbe8b..54f2d5011e 100644 --- a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp +++ b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp @@ -810,7 +810,7 @@ void PlanningGroupsWidget::addGroup() adding_new_group_ = true; // Load the data - loadGroupScreen(nullptr); // NULL indicates this is a new group, not an existing one + loadGroupScreen(nullptr); // nullptr indicates this is a new group, not an existing one // Switch to screen changeScreen(GROUP); diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp index 8cef7999a4..777754a478 100644 --- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp +++ b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp @@ -398,7 +398,7 @@ void ROSControllersWidget::addController() adding_new_controller_ = true; // Load the data - loadControllerScreen(nullptr); // NULL indicates this is a new controller, not an existing one + loadControllerScreen(nullptr); // nullptr indicates this is a new controller, not an existing one changeScreen(2); // 1 is index of controller edit } From 53f9affaeb10cd077f9e5444f884d1c9b3ef6871 Mon Sep 17 00:00:00 2001 From: Wolf Vollprecht Date: Thu, 6 Jan 2022 16:58:44 +0100 Subject: [PATCH 155/229] Fix usage of boost placeholder (#958) --- .../src/move_group_sequence_action.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp index bd10650652..4620ca83f6 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp @@ -163,8 +163,8 @@ void MoveGroupSequenceAction::executeSequenceCallbackPlanAndExecute( opt.replan_delay_ = goal->planning_options.replan_delay; opt.before_execution_callback_ = boost::bind(&MoveGroupSequenceAction::startMoveExecutionCallback, this); - opt.plan_callback_ = - boost::bind(&MoveGroupSequenceAction::planUsingSequenceManager, this, boost::cref(goal->request), _1); + opt.plan_callback_ = boost::bind(&MoveGroupSequenceAction::planUsingSequenceManager, this, boost::cref(goal->request), + boost::placeholders::_1); if (goal->planning_options.look_around && context_->plan_with_sensing_) { From 30139c155ea6ccdf349c9f04dca6d09fd61a519e Mon Sep 17 00:00:00 2001 From: Martin Oehler Date: Thu, 6 Jan 2022 18:33:59 +0100 Subject: [PATCH 156/229] Fix wrong transform in distance fields' determineCollisionSpheres() (#3022) --- .../src/collision_distance_field_types.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp index 2f80e15e38..6bdec239f4 100644 --- a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp +++ b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp @@ -52,7 +52,7 @@ collision_detection::determineCollisionSpheres(const bodies::Body* body, Eigen:: body->computeBoundingCylinder(cyl); unsigned int num_points = ceil(cyl.length / (cyl.radius / 2.0)); double spacing = cyl.length / ((num_points * 1.0) - 1.0); - relative_transform = body->getPose().inverse() * cyl.pose; + relative_transform = cyl.pose; for (unsigned int i = 1; i < num_points - 1; i++) { From 4f97486e211f498cddde48b61e24d49fd73d9b31 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 6 Jan 2022 20:19:34 +0100 Subject: [PATCH 157/229] Mergify configuration for Galactic backport (#965) --- .github/mergify.yml | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/.github/mergify.yml b/.github/mergify.yml index 39e8328edb..67953b1b3a 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -8,6 +8,15 @@ pull_request_rules: branches: - foxy + - name: backport to galactic at reviewers discretion + conditions: + - base=main + - "label=backport-galactic" + actions: + backport: + branches: + - galactic + - name: ask to resolve conflict conditions: - conflict From 834ebb9e976432509d8dc89d69a8abd90d312b25 Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Thu, 6 Jan 2022 23:27:04 +0300 Subject: [PATCH 158/229] Make TimeParameterization classes polymorphic (#3021) --- .../iterative_spline_parameterization.h | 6 +++--- .../iterative_time_parameterization.h | 6 +++--- .../time_optimal_trajectory_generation.h | 6 +++--- .../time_parameterization.h | 18 ++++++++++++++++++ .../src/time_optimal_trajectory_generation.cpp | 4 ---- 5 files changed, 27 insertions(+), 13 deletions(-) create mode 100644 moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h index 92f14befdb..139989e8ab 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h @@ -38,6 +38,7 @@ #pragma once #include +#include namespace trajectory_processing { @@ -67,14 +68,13 @@ namespace trajectory_processing /// velocity and acceleration limits, will result in a longer trajectory. /// If this is a problem, try retuning (increasing) the limits. /// -class IterativeSplineParameterization +class IterativeSplineParameterization : public TimeParameterization { public: IterativeSplineParameterization(bool add_points = true); - ~IterativeSplineParameterization() = default; bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, - const double max_acceleration_scaling_factor = 1.0) const; + const double max_acceleration_scaling_factor = 1.0) const override; private: bool add_points_; /// @brief If true, add two points to trajectory (first and last segments). diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h index b142f987a5..9863badda5 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h @@ -37,19 +37,19 @@ #pragma once #include +#include namespace trajectory_processing { /// \brief This class modifies the timestamps of a trajectory to respect /// velocity and acceleration constraints. -class IterativeParabolicTimeParameterization +class IterativeParabolicTimeParameterization : public TimeParameterization { public: IterativeParabolicTimeParameterization(unsigned int max_iterations = 100, double max_time_change_per_it = .01); - ~IterativeParabolicTimeParameterization() = default; bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, - const double max_acceleration_scaling_factor = 1.0) const; + const double max_acceleration_scaling_factor = 1.0) const override; private: unsigned int max_iterations_; /// @brief maximum number of iterations to find solution diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h index 0ee4d13f41..cbbc6cd739 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h @@ -41,6 +41,7 @@ #include #include #include +#include namespace trajectory_processing { @@ -159,15 +160,14 @@ class Trajectory mutable std::list::const_iterator cached_trajectory_segment_; }; -class TimeOptimalTrajectoryGeneration +class TimeOptimalTrajectoryGeneration : public TimeParameterization { public: TimeOptimalTrajectoryGeneration(const double path_tolerance = 0.1, const double resample_dt = 0.1, const double min_angle_change = 0.001); - ~TimeOptimalTrajectoryGeneration(); bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, - const double max_acceleration_scaling_factor = 1.0) const; + const double max_acceleration_scaling_factor = 1.0) const override; private: const double path_tolerance_; diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h new file mode 100644 index 0000000000..3a64960e54 --- /dev/null +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h @@ -0,0 +1,18 @@ +#pragma once + +#include + +namespace trajectory_processing +{ +/** + * @brief Base class for trajectory parameterization algorithms + */ +class TimeParameterization +{ +public: + virtual ~TimeParameterization() = default; + virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, + const double max_velocity_scaling_factor = 1.0, + const double max_acceleration_scaling_factor = 1.0) const = 0; +}; +} // namespace trajectory_processing 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 15aff331cd..016f0c4cfb 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -861,10 +861,6 @@ TimeOptimalTrajectoryGeneration::TimeOptimalTrajectoryGeneration(const double pa { } -TimeOptimalTrajectoryGeneration::~TimeOptimalTrajectoryGeneration() -{ -} - bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor, const double max_acceleration_scaling_factor) const From ea86018d20c2e558033d0f231c192d0f7626e03e Mon Sep 17 00:00:00 2001 From: Rick Staa Date: Fri, 7 Jan 2022 10:54:13 +0100 Subject: [PATCH 159/229] MSA: Add STOMP + OMPL-CHOMP configs (#2955) - Add stomp planner to MSA - Add OMPL-CHOMP planner to MSA - Remove obsolete CHOMP parameters - Update CHOMP config parameters to match code defaults - Create CHOMP config via template (instead of code) Co-authored-by: Robert Haschke --- .../chomp_interface/src/chomp_interface.cpp | 6 - .../chomp_interface/test/chomp_planning.yaml | 6 - .../chomp_motion_planner/chomp_parameters.h | 6 - .../src/chomp_parameters.cpp | 6 - .../tools/moveit_config_data.h | 2 +- .../src/tools/moveit_config_data.cpp | 144 +++++++++++++++--- .../widgets/configuration_files_widget.cpp | 35 ++++- .../config/chomp_planning.yaml | 18 +++ .../ompl-chomp_planning_pipeline.launch.xml | 19 +++ .../launch/stomp_planning_pipeline.launch.xml | 25 +++ 10 files changed, 220 insertions(+), 47 deletions(-) create mode 100644 moveit_setup_assistant/templates/moveit_config_pkg_template/config/chomp_planning.yaml create mode 100644 moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl-chomp_planning_pipeline.launch.xml create mode 100644 moveit_setup_assistant/templates/moveit_config_pkg_template/launch/stomp_planning_pipeline.launch.xml diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp index 42f5f9072c..53a278ed0f 100644 --- a/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp +++ b/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp @@ -52,14 +52,9 @@ void CHOMPInterface::loadParams() nh_.param("obstacle_cost_weight", params_.obstacle_cost_weight_, 1.0); nh_.param("learning_rate", params_.learning_rate_, 0.01); - // nh_.param("add_randomness", params_.add_randomness_, false); nh_.param("smoothness_cost_velocity", params_.smoothness_cost_velocity_, 0.0); nh_.param("smoothness_cost_acceleration", params_.smoothness_cost_acceleration_, 1.0); nh_.param("smoothness_cost_jerk", params_.smoothness_cost_jerk_, 0.0); - // nh_.param("hmc_discretization", params_.hmc_discretization_, 0.01); - // nh_.param("hmc_stochasticity", params_.hmc_stochasticity_, 0.01); - // nh_.param("hmc_annealing_factor", params_.hmc_annealing_factor_, 0.99); - // nh_.param("use_hamiltonian_monte_carlo", params_.use_hamiltonian_monte_carlo_, false); nh_.param("ridge_factor", params_.ridge_factor_, 0.0); nh_.param("use_pseudo_inverse", params_.use_pseudo_inverse_, false); nh_.param("pseudo_inverse_ridge_factor", params_.pseudo_inverse_ridge_factor_, 1e-4); @@ -70,7 +65,6 @@ void CHOMPInterface::loadParams() ROS_WARN("The param 'collision_clearence' has been renamed to 'collision_clearance', please update your config!"); nh_.param("collision_clearance", params_.min_clearance_, 0.2); nh_.param("collision_threshold", params_.collision_threshold_, 0.07); - // nh_.param("random_jump_amount", params_.random_jump_amount_, 1.0); nh_.param("use_stochastic_descent", params_.use_stochastic_descent_, true); { params_.trajectory_initialization_method_ = "quintic-spline"; diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_planning.yaml b/moveit_planners/chomp/chomp_interface/test/chomp_planning.yaml index d6681c1519..b6b2a82f11 100644 --- a/moveit_planners/chomp/chomp_interface/test/chomp_planning.yaml +++ b/moveit_planners/chomp/chomp_interface/test/chomp_planning.yaml @@ -5,14 +5,9 @@ smoothness_cost_weight: 0.1 obstacle_cost_weight: 1.0 learning_rate: 0.01 animate_path: true -add_randomness: false smoothness_cost_velocity: 0.0 smoothness_cost_acceleration: 1.0 smoothness_cost_jerk: 0.0 -hmc_discretization: 0.01 -hmc_stochasticity: 0.01 -hmc_annealing_factor: 0.99 -use_hamiltonian_monte_carlo: false ridge_factor: 0.0 use_pseudo_inverse: false pseudo_inverse_ridge_factor: 1e-4 @@ -21,5 +16,4 @@ animate_endeffector_segment: "link3" joint_update_limit: 0.1 collision_clearance: 0.2 collision_threshold: 0.07 -random_jump_amount: 1.0 use_stochastic_descent: true diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h index f39dbf55f0..31b61b9c42 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h @@ -78,14 +78,9 @@ class ChompParameters double smoothness_cost_velocity_; /// variables associated with the cost in velocity double smoothness_cost_acceleration_; /// variables associated with the cost in acceleration double smoothness_cost_jerk_; /// variables associated with the cost in jerk - // bool add_randomness_; - // bool use_hamiltonian_monte_carlo_; bool use_stochastic_descent_; /// set this to true/false if you want to use stochastic descent while optimizing the /// cost. - // double hmc_stochasticity_; - // double hmc_discretization_; - // double hmc_annealing_factor_; double ridge_factor_; /// the noise added to the diagnal of the total quadratic cost matrix in the objective function bool use_pseudo_inverse_; /// enable pseudo inverse calculations or not. double pseudo_inverse_ridge_factor_; /// set the ridge factor if pseudo inverse is enabled @@ -94,7 +89,6 @@ class ChompParameters double min_clearance_; /// the minimum distance that needs to be maintained to avoid obstacles double collision_threshold_; /// the collision threshold cost that needs to be mainted to avoid collisions bool filter_mode_; - // double random_jump_amount_; static const std::vector VALID_INITIALIZATION_METHODS; std::string trajectory_initialization_method_; /// trajectory initialization method to be specified diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp index f3d4829d04..1df4c54a12 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp @@ -47,14 +47,9 @@ ChompParameters::ChompParameters() obstacle_cost_weight_ = 1.0; learning_rate_ = 0.01; - // add_randomness_ = false; smoothness_cost_velocity_ = 0.0; smoothness_cost_acceleration_ = 1.0; smoothness_cost_jerk_ = 0.0; - // hmc_discretization_ = 0.01; - // hmc_stochasticity_ = 0.01; - // hmc_annealing_factor_ = 0.99; - // use_hamiltonian_monte_carlo_ = false; ridge_factor_ = 0.0; use_pseudo_inverse_ = false; pseudo_inverse_ridge_factor_ = 1e-4; @@ -62,7 +57,6 @@ ChompParameters::ChompParameters() joint_update_limit_ = 0.1; min_clearance_ = 0.2; collision_threshold_ = 0.07; - // random_jump_amount_ = 1.0; use_stochastic_descent_ = true; filter_mode_ = false; trajectory_initialization_method_ = std::string("quintic-spline"); diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h index 7ce6f0caae..b99bafb50f 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h @@ -309,7 +309,7 @@ class MoveItConfigData std::map getInitialJoints() const; bool outputSetupAssistantFile(const std::string& file_path); bool outputOMPLPlanningYAML(const std::string& file_path); - bool outputCHOMPPlanningYAML(const std::string& file_path); + bool outputSTOMPPlanningYAML(const std::string& file_path); bool outputKinematicsYAML(const std::string& file_path); bool outputJointLimitsYAML(const std::string& file_path); bool outputFakeControllersYAML(const std::string& file_path); diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 4535622c36..69f64b2922 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -286,31 +286,133 @@ bool MoveItConfigData::outputOMPLPlanningYAML(const std::string& file_path) } // ****************************************************************************************** -// Output CHOMP Planning config files +// Output STOMP Planning config files // ****************************************************************************************** -bool MoveItConfigData::outputCHOMPPlanningYAML(const std::string& file_path) +bool MoveItConfigData::outputSTOMPPlanningYAML(const std::string& file_path) { YAML::Emitter emitter; + emitter << YAML::BeginMap; + + // Add STOMP default for every group + for (srdf::Model::Group& group : srdf_->groups_) + { + emitter << YAML::Key << "stomp/" + group.name_; + emitter << YAML::BeginMap; + emitter << YAML::Key << "group_name"; + emitter << YAML::Value << group.name_; + + emitter << YAML::Key << "optimization"; + emitter << YAML::BeginMap; + emitter << YAML::Key << "num_timesteps"; + emitter << YAML::Value << "60"; + emitter << YAML::Key << "num_iterations"; + emitter << YAML::Value << "40"; + emitter << YAML::Key << "num_iterations_after_valid"; + emitter << YAML::Value << "0"; + emitter << YAML::Key << "num_rollouts"; + emitter << YAML::Value << "30"; + emitter << YAML::Key << "max_rollouts"; + emitter << YAML::Value << "30"; + emitter << YAML::Key << "initialization_method"; + emitter << YAML::Value << "1"; + emitter << YAML::Comment("[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST]"); + emitter << YAML::Key << "control_cost_weight"; + emitter << YAML::Value << "0.0"; + emitter << YAML::EndMap; + + emitter << YAML::Key << "task"; + emitter << YAML::BeginMap; + + emitter << YAML::Key << "noise_generator"; + emitter << YAML::BeginSeq; + emitter << YAML::BeginMap; + emitter << YAML::Key << "class"; + emitter << YAML::Value << "stomp_moveit/NormalDistributionSampling"; + emitter << YAML::Key << "stddev"; + emitter << YAML::Flow; + const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_); + const std::vector& joint_models = joint_model_group->getActiveJointModels(); + std::vector stddev(joint_models.size(), 0.05); + emitter << YAML::Value << stddev; + emitter << YAML::EndMap; + emitter << YAML::EndSeq; + + emitter << YAML::Key << "cost_functions"; + emitter << YAML::BeginSeq; + emitter << YAML::BeginMap; + emitter << YAML::Key << "class"; + emitter << YAML::Value << "stomp_moveit/CollisionCheck"; + emitter << YAML::Key << "collision_penalty"; + emitter << YAML::Value << "1.0"; + emitter << YAML::Key << "cost_weight"; + emitter << YAML::Value << "1.0"; + emitter << YAML::Key << "kernel_window_percentage"; + emitter << YAML::Value << "0.2"; + emitter << YAML::Key << "longest_valid_joint_move"; + emitter << YAML::Value << "0.05"; + emitter << YAML::EndMap; + emitter << YAML::EndSeq; + + emitter << YAML::Key << "noisy_filters"; + emitter << YAML::BeginSeq; + emitter << YAML::BeginMap; + emitter << YAML::Key << "class"; + emitter << YAML::Value << "stomp_moveit/JointLimits"; + emitter << YAML::Key << "lock_start"; + emitter << YAML::Value << "True"; + emitter << YAML::Key << "lock_goal"; + emitter << YAML::Value << "True"; + emitter << YAML::EndMap; + emitter << YAML::BeginMap; + emitter << YAML::Key << "class"; + emitter << YAML::Value << "stomp_moveit/MultiTrajectoryVisualization"; + emitter << YAML::Key << "line_width"; + emitter << YAML::Value << "0.02"; + emitter << YAML::Key << "rgb"; + emitter << YAML::Flow; + std::vector noisy_filters_rgb{ 255, 255, 0 }; + emitter << YAML::Value << noisy_filters_rgb; + emitter << YAML::Key << "marker_array_topic"; + emitter << YAML::Value << "stomp_trajectories"; + emitter << YAML::Key << "marker_namespace"; + emitter << YAML::Value << "noisy"; + emitter << YAML::EndMap; + emitter << YAML::EndSeq; + + emitter << YAML::Key << "update_filters"; + emitter << YAML::BeginSeq; + emitter << YAML::BeginMap; + emitter << YAML::Key << "class"; + emitter << YAML::Value << "stomp_moveit/PolynomialSmoother"; + emitter << YAML::Key << "poly_order"; + emitter << YAML::Value << "6"; + emitter << YAML::EndMap; + emitter << YAML::BeginMap; + emitter << YAML::Key << "class"; + emitter << YAML::Value << "stomp_moveit/TrajectoryVisualization"; + emitter << YAML::Key << "line_width"; + emitter << YAML::Value << "0.05"; + emitter << YAML::Key << "rgb"; + emitter << YAML::Flow; + std::vector update_filters_rgb{ 0, 191, 255 }; + emitter << YAML::Value << update_filters_rgb; + emitter << YAML::Key << "error_rgb"; + emitter << YAML::Flow; + std::vector update_filters_error_rgb{ 255, 0, 0 }; + emitter << YAML::Value << update_filters_error_rgb; + emitter << YAML::Key << "publish_intermediate"; + emitter << YAML::Value << "True"; + emitter << YAML::Key << "marker_topic"; + emitter << YAML::Value << "stomp_trajectory"; + emitter << YAML::Key << "marker_namespace"; + emitter << YAML::Value << "optimized"; + emitter << YAML::EndMap; + emitter << YAML::EndSeq; + + emitter << YAML::EndMap; + emitter << YAML::EndMap; + } - emitter << YAML::Value << YAML::BeginMap; - emitter << YAML::Key << "planning_time_limit" << YAML::Value << "10.0"; - emitter << YAML::Key << "max_iterations" << YAML::Value << "200"; - emitter << YAML::Key << "max_iterations_after_collision_free" << YAML::Value << "5"; - emitter << YAML::Key << "smoothness_cost_weight" << YAML::Value << "0.1"; - emitter << YAML::Key << "obstacle_cost_weight" << YAML::Value << "1.0"; - emitter << YAML::Key << "learning_rate" << YAML::Value << "0.01"; - emitter << YAML::Key << "smoothness_cost_velocity" << YAML::Value << "0.0"; - emitter << YAML::Key << "smoothness_cost_acceleration" << YAML::Value << "1.0"; - emitter << YAML::Key << "smoothness_cost_jerk" << YAML::Value << "0.0"; - emitter << YAML::Key << "ridge_factor" << YAML::Value << "0.01"; - emitter << YAML::Key << "use_pseudo_inverse" << YAML::Value << "false"; - emitter << YAML::Key << "pseudo_inverse_ridge_factor" << YAML::Value << "1e-4"; - emitter << YAML::Key << "joint_update_limit" << YAML::Value << "0.1"; - emitter << YAML::Key << "collision_clearance" << YAML::Value << "0.2"; - emitter << YAML::Key << "collision_threshold" << YAML::Value << "0.07"; - emitter << YAML::Key << "use_stochastic_descent" << YAML::Value << "true"; - emitter << YAML::Key << "enable_failure_recovery" << YAML::Value << "true"; - emitter << YAML::Key << "max_recovery_attempts" << YAML::Value << "5"; emitter << YAML::EndMap; std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc); diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 2685d6d1d8..2d12593bdc 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -287,8 +287,16 @@ bool ConfigurationFilesWidget::loadGenFiles() // chomp_planning.yaml -------------------------------------------------------------------------------------- file.file_name_ = "chomp_planning.yaml"; file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); + template_path = config_data_->appendPaths(config_data_->template_package_path_, file.rel_path_); file.description_ = "Specifies which chomp planning plugin parameters to be used for the CHOMP planner"; - file.gen_func_ = std::bind(&MoveItConfigData::outputCHOMPPlanningYAML, config_data_, std::placeholders::_1); + file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + gen_files_.push_back(file); + + // stomp_planning.yaml -------------------------------------------------------------------------------------- + file.file_name_ = "stomp_planning.yaml"; + file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); + file.description_ = "Specifies which stomp planning plugin parameters to be used for the STOMP planner"; + file.gen_func_ = boost::bind(&MoveItConfigData::outputSTOMPPlanningYAML, config_data_, _1); file.write_on_changes = MoveItConfigData::GROUPS; // need to double check if this is actually correct! gen_files_.push_back(file); @@ -447,6 +455,31 @@ bool ConfigurationFilesWidget::loadGenFiles() file.write_on_changes = 0; gen_files_.push_back(file); + // stomp_planning_pipeline.launch + // -------------------------------------------------------------------------------------- + file.file_name_ = "stomp_planning_pipeline.launch.xml"; + file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); + template_path = config_data_->appendPaths(template_launch_path, file.file_name_); + file.description_ = "Intended to be included in other launch files that require the STOMP planning plugin. Defines " + "the proper plugin name on the parameter server and a default selection of planning request " + "adapters."; + file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.write_on_changes = 0; + gen_files_.push_back(file); + + // ompl-chomp_planning_pipeline.launch.xml + // -------------------------------------------------------------------------------------- + file.file_name_ = "ompl-chomp_planning_pipeline.launch.xml"; + file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); + template_path = config_data_->appendPaths(template_launch_path, file.file_name_); + file.description_ = + "Intended to be included in other launch files that require the OMPL-CHOMP planning plugins. Defines " + "the proper plugin name on the parameter server and a default selection of planning request " + "adapters."; + file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.write_on_changes = 0; + gen_files_.push_back(file); + // planning_pipeline.launch -------------------------------------------------------------------------------------- file.file_name_ = "planning_pipeline.launch.xml"; file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/config/chomp_planning.yaml b/moveit_setup_assistant/templates/moveit_config_pkg_template/config/chomp_planning.yaml new file mode 100644 index 0000000000..eb9c91225b --- /dev/null +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.0 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearance: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: false +max_recovery_attempts: 5 diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl-chomp_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl-chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000000..b078992417 --- /dev/null +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl-chomp_planning_pipeline.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/stomp_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/stomp_planning_pipeline.launch.xml new file mode 100644 index 0000000000..c1de09f874 --- /dev/null +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/stomp_planning_pipeline.launch.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + From 21e2eedb342f8bf80770ac687c822ff5436d2c12 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 7 Jan 2022 16:14:48 +0100 Subject: [PATCH 160/229] Only target Rolling in docker and pre-release jobs (#970) --- .github/workflows/docker.yaml | 8 ++++---- .github/workflows/prerelease.yaml | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/workflows/docker.yaml b/.github/workflows/docker.yaml index ab11bf58ed..1fb1495eba 100644 --- a/.github/workflows/docker.yaml +++ b/.github/workflows/docker.yaml @@ -14,7 +14,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling, galactic] + ROS_DISTRO: [rolling] runs-on: ubuntu-latest permissions: packages: write @@ -66,7 +66,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling, galactic] + ROS_DISTRO: [rolling] runs-on: ubuntu-latest permissions: packages: write @@ -119,7 +119,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling, galactic] + ROS_DISTRO: [rolling] runs-on: ubuntu-latest permissions: packages: write @@ -172,7 +172,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling, galactic] + ROS_DISTRO: [rolling] runs-on: ubuntu-latest permissions: packages: write diff --git a/.github/workflows/prerelease.yaml b/.github/workflows/prerelease.yaml index 8b860855cf..ccfd27bdfb 100644 --- a/.github/workflows/prerelease.yaml +++ b/.github/workflows/prerelease.yaml @@ -11,7 +11,7 @@ jobs: strategy: fail-fast: false matrix: - distro: [foxy, galactic, rolling] + distro: [rolling] env: ROS_DISTRO: ${{ matrix.distro }} From 79addb30a703f566f3d81b01dc4d34c0f8cce209 Mon Sep 17 00:00:00 2001 From: jschleicher Date: Fri, 7 Jan 2022 18:14:25 +0100 Subject: [PATCH 161/229] Remove some Maintainers from Pilz Planner (#971) --- moveit_planners/pilz_industrial_motion_planner/package.xml | 3 --- .../pilz_industrial_motion_planner_testutils/package.xml | 3 --- 2 files changed, 6 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/package.xml b/moveit_planners/pilz_industrial_motion_planner/package.xml index f26f924500..c2f2f2dbaa 100644 --- a/moveit_planners/pilz_industrial_motion_planner/package.xml +++ b/moveit_planners/pilz_industrial_motion_planner/package.xml @@ -5,11 +5,8 @@ 2.3.2 MoveIt plugin to generate industrial trajectories PTP, LIN, CIRC and sequences thereof. - Alexander Gutenkunst Christian Henkel Immanuel Martini - Joachim Schleicher - Hagen Slusarek BSD diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml index e50bab1de9..e86e7800f5 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml @@ -5,11 +5,8 @@ 2.3.2 Helper scripts and functionality to test industrial motion generation - Alexander Gutenkunst Christian Henkel Immanuel Martini - Joachim Schleicher - Hagen Slusarek BSD From 4f8945035053afcb7888c69e91cea38b119f88ff Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 7 Jan 2022 14:39:47 -0600 Subject: [PATCH 162/229] Servo: re-order velocity limit check & minor cleanup (#956) --- .../config/panda_simulated_config.yaml | 6 +- .../panda_simulated_config_pose_tracking.yaml | 6 +- .../include/moveit_servo/enforce_limits.hpp | 14 +-- .../include/moveit_servo/servo_calcs.h | 6 +- .../moveit_servo/src/enforce_limits.cpp | 26 +++-- moveit_ros/moveit_servo/src/servo_calcs.cpp | 55 +++-------- .../moveit_servo/src/servo_parameters.cpp | 15 ++- .../test/enforce_limits_tests.cpp | 99 +++++++++---------- 8 files changed, 99 insertions(+), 128 deletions(-) diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml index f7763fc536..1771d10611 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml @@ -7,9 +7,9 @@ use_gazebo: false # Whether the robot is started in a Gazebo simulation environm command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: # Scale parameters are only used if command_in_type=="unitless" - linear: 0.4 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. - rotational: 0.8 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. - # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. + linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. joint: 0.5 ## Properties of outgoing commands diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml index a82a1bc374..cdbe53f85f 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml +++ b/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml @@ -7,9 +7,9 @@ use_gazebo: false # Whether the robot is started in a Gazebo simulation environm command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: # Scale parameters are only used if command_in_type=="unitless" - linear: 0.6 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. - rotational: 0.3 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. - # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. + linear: 0.6 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.3 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. joint: 0.01 ## Properties of outgoing commands diff --git a/moveit_ros/moveit_servo/include/moveit_servo/enforce_limits.hpp b/moveit_ros/moveit_servo/include/moveit_servo/enforce_limits.hpp index 9291cd16ff..b549e70467 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/enforce_limits.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/enforce_limits.hpp @@ -46,15 +46,11 @@ namespace moveit_servo { /** - * @brief Scale the delta theta to match joint velocity/acceleration limits - * - * @param[in] joint_model_group The joint model group - * @param[in] publish_period The publish period of servo (used for calculating joint velocity) - * @param[in] delta_theta The delta theta input - * - * @return The delta theta output + * @brief Decrease robot position change and velocity, if needed, to satisfy joint velocity limits + * @param joint_model_group Active joint group. Used to retrieve limits. + * @param joint_state The command that will go to the robot. */ -Eigen::ArrayXd enforceVelocityLimits(const moveit::core::JointModelGroup* joint_model_group, double publish_period, - const Eigen::ArrayXd& delta_theta); +void enforceVelocityLimits(const moveit::core::JointModelGroup* joint_model_group, const double publish_period, + sensor_msgs::msg::JointState& joint_state); } // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index 6fe96015a5..b2413e6f99 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -148,11 +148,13 @@ class ServoCalcs /** \brief If incoming velocity commands are from a unitless joystick, scale them to physical units. * Also, multiply by timestep to calculate a position change. + * @return a vector of position deltas */ Eigen::VectorXd scaleCartesianCommand(const geometry_msgs::msg::TwistStamped& command); /** \brief If incoming velocity commands are from a unitless joystick, scale them to physical units. * Also, multiply by timestep to calculate a position change. + * @return a vector of position deltas */ Eigen::VectorXd scaleJointCommand(const control_msgs::msg::JointJog& command); @@ -201,8 +203,7 @@ class ServoCalcs * @param previous_vel Eigen vector of previous velocities being updated * @return Returns false if there is a problem, true otherwise */ - bool applyJointUpdate(const Eigen::ArrayXd& delta_theta, sensor_msgs::msg::JointState& joint_state, - Eigen::ArrayXd& previous_vel); + bool applyJointUpdate(const Eigen::ArrayXd& delta_theta, sensor_msgs::msg::JointState& joint_state); /** \brief Gazebo simulations have very strict message timestamp requirements. * Satisfy Gazebo by stuffing multiple messages into one. @@ -334,7 +335,6 @@ class ServoCalcs // Use ArrayXd type to enable more coefficient-wise operations Eigen::ArrayXd delta_theta_; - Eigen::ArrayXd prev_joint_velocity_; const int gazebo_redundant_message_count_ = 30; diff --git a/moveit_ros/moveit_servo/src/enforce_limits.cpp b/moveit_ros/moveit_servo/src/enforce_limits.cpp index 6c9fd55aa1..afc0538180 100644 --- a/moveit_ros/moveit_servo/src/enforce_limits.cpp +++ b/moveit_ros/moveit_servo/src/enforce_limits.cpp @@ -47,7 +47,7 @@ namespace moveit_servo { namespace { -double getVelocityScalingFactor(const moveit::core::JointModelGroup* joint_model_group, const Eigen::ArrayXd& velocity) +double getVelocityScalingFactor(const moveit::core::JointModelGroup* joint_model_group, const Eigen::VectorXd& velocity) { std::size_t joint_delta_index{ 0 }; double velocity_scaling_factor{ 1.0 }; @@ -69,17 +69,27 @@ double getVelocityScalingFactor(const moveit::core::JointModelGroup* joint_model } // namespace -Eigen::ArrayXd enforceVelocityLimits(const moveit::core::JointModelGroup* joint_model_group, double publish_period, - const Eigen::ArrayXd& delta_theta) +void enforceVelocityLimits(const moveit::core::JointModelGroup* joint_model_group, const double publish_period, + sensor_msgs::msg::JointState& joint_state) { - // Convert to joint angle velocities for checking and applying joint specific velocity limits. - Eigen::ArrayXd velocity = delta_theta / publish_period; - // Get the velocity scaling factor + Eigen::VectorXd velocity = + Eigen::Map(joint_state.velocity.data(), joint_state.velocity.size()); double velocity_scaling_factor = getVelocityScalingFactor(joint_model_group, velocity); - // Scale the resulting detas to avoid violating limits. - return velocity_scaling_factor * velocity * publish_period; + // Take a smaller step if the velocity scaling factor is less than 1 + if (velocity_scaling_factor < 1) + { + Eigen::VectorXd velocity_residuals = (1 - velocity_scaling_factor) * velocity; + Eigen::VectorXd positions = + Eigen::Map(joint_state.position.data(), joint_state.position.size()); + positions -= velocity_residuals * publish_period; + + velocity *= velocity_scaling_factor; + // Back to sensor_msgs type + joint_state.velocity = std::vector(velocity.data(), velocity.data() + velocity.size()); + joint_state.position = std::vector(positions.data(), positions.data() + positions.size()); + } } } // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index 03127a1a1e..2527f201e4 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -119,7 +119,6 @@ ServoCalcs::ServoCalcs(rclcpp::Node::SharedPtr node, RCLCPP_ERROR_STREAM(LOGGER, "Invalid move group name: `" << parameters_->move_group_name << "`"); throw std::runtime_error("Invalid move group name"); } - prev_joint_velocity_ = Eigen::ArrayXd::Zero(joint_model_group_->getActiveJointModels().size()); // Subscribe to command topics using std::placeholders::_1; @@ -149,10 +148,6 @@ ServoCalcs::ServoCalcs(rclcpp::Node::SharedPtr node, node_->create_subscription("~/collision_velocity_scale", rclcpp::SystemDefaultsQoS(), std::bind(&ServoCalcs::collisionVelocityScaleCB, this, _1)); - // Publish to collision_check for worst stop time - worst_case_stop_time_pub_ = - node_->create_publisher("~/worst_case_stop_time", rclcpp::SystemDefaultsQoS()); - // Publish freshly-calculated joints to the robot. // Put the outgoing msg in the right format (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). if (parameters_->command_out_type == "trajectory_msgs/JointTrajectory") @@ -200,25 +195,6 @@ ServoCalcs::ServoCalcs(rclcpp::Node::SharedPtr node, std::exit(EXIT_FAILURE); } - // Load the smoothing plugin - try - { - smoother_ = smoothing_loader_.createSharedInstance(parameters_->smoothing_filter_plugin_name); - } - catch (pluginlib::PluginlibException& ex) - { - RCLCPP_ERROR(LOGGER, "Exception while loading the smoothing plugin '%s': '%s'", - parameters_->smoothing_filter_plugin_name.c_str(), ex.what()); - std::exit(EXIT_FAILURE); - } - - // Initialize the smoothing plugin - if (!smoother_->initialize(node_, planning_scene_monitor_->getRobotModel(), num_joints_)) - { - RCLCPP_ERROR(LOGGER, "Smoothing plugin could not be initialized"); - std::exit(EXIT_FAILURE); - } - // A matrix of all zeros is used to check whether matrices have been initialized Eigen::Matrix3d empty_matrix; empty_matrix.setZero(); @@ -616,12 +592,16 @@ bool ServoCalcs::internalServoUpdate(Eigen::ArrayXd& delta_theta, trajectory_msgs::msg::JointTrajectory& joint_trajectory, const ServoType servo_type) { + // The order of operations here is: + // 1. apply velocity scaling for collisions (in the position domain) + // 2. low-pass filter the position command in applyJointUpdate() + // 3. calculate velocities in applyJointUpdate() + // 4. apply velocity limits + // 5. apply position limits. This is a higher priority than velocity limits, so check it last. + // Set internal joint state from original internal_joint_state_ = original_joint_state_; - // Enforce SRDF Velocity, Acceleration limits - delta_theta = enforceVelocityLimits(joint_model_group_, parameters_->publish_period, delta_theta); - // Apply collision scaling double collision_scale = collision_velocity_scale_; if (collision_scale > 0 && collision_scale < 1) @@ -638,13 +618,16 @@ bool ServoCalcs::internalServoUpdate(Eigen::ArrayXd& delta_theta, } delta_theta *= collision_scale; - // Loop through joints and update them, calculate velocities, and filter - if (!applyJointUpdate(delta_theta, internal_joint_state_, prev_joint_velocity_)) + // Loop thru joints and update them, calculate velocities, and filter + if (!applyJointUpdate(delta_theta, internal_joint_state_)) return false; // Mark the lowpass filters as updated for this cycle updated_filters_ = true; + // Enforce SRDF velocity limits + enforceVelocityLimits(joint_model_group_, parameters_->publish_period, internal_joint_state_); + // Enforce SRDF position limits, might halt if needed, set prev_vel to 0 const auto joints_to_halt = enforcePositionLimits(internal_joint_state_); if (!joints_to_halt.empty()) @@ -654,13 +637,10 @@ bool ServoCalcs::internalServoUpdate(Eigen::ArrayXd& delta_theta, (servo_type == ServoType::CARTESIAN_SPACE && !parameters_->halt_all_joints_in_cartesian_mode)) { suddenHalt(internal_joint_state_, joints_to_halt); - prev_joint_velocity_ = - Eigen::ArrayXd::Map(internal_joint_state_.velocity.data(), internal_joint_state_.velocity.size()); } else { suddenHalt(internal_joint_state_, joint_model_group_->getActiveJointModels()); - prev_joint_velocity_.setZero(); } } @@ -676,13 +656,11 @@ bool ServoCalcs::internalServoUpdate(Eigen::ArrayXd& delta_theta, return true; } -bool ServoCalcs::applyJointUpdate(const Eigen::ArrayXd& delta_theta, sensor_msgs::msg::JointState& joint_state, - Eigen::ArrayXd& previous_vel) +bool ServoCalcs::applyJointUpdate(const Eigen::ArrayXd& delta_theta, sensor_msgs::msg::JointState& joint_state) { // All the sizes must match if (joint_state.position.size() != static_cast(delta_theta.size()) || - joint_state.velocity.size() != joint_state.position.size() || - static_cast(previous_vel.size()) != joint_state.position.size()) + joint_state.velocity.size() != joint_state.position.size()) { rclcpp::Clock& clock = *node_->get_clock(); RCLCPP_ERROR_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, @@ -703,9 +681,6 @@ bool ServoCalcs::applyJointUpdate(const Eigen::ArrayXd& delta_theta, sensor_msgs // Calculate joint velocity joint_state.velocity[i] = (joint_state.position.at(i) - original_joint_state_.position.at(i)) / parameters_->publish_period; - - // Save this velocity for future accel calculations - previous_vel[i] = joint_state.velocity[i]; } return true; @@ -1008,7 +983,7 @@ bool ServoCalcs::checkValidCommand(const geometry_msgs::msg::TwistStamped& cmd) return true; } -// Scale the incoming jog command +// Scale the incoming jog command. Returns a vector of position deltas Eigen::VectorXd ServoCalcs::scaleCartesianCommand(const geometry_msgs::msg::TwistStamped& command) { Eigen::VectorXd result(6); diff --git a/moveit_ros/moveit_servo/src/servo_parameters.cpp b/moveit_ros/moveit_servo/src/servo_parameters.cpp index c36abd210f..528c98cb35 100644 --- a/moveit_ros/moveit_servo/src/servo_parameters.cpp +++ b/moveit_ros/moveit_servo/src/servo_parameters.cpp @@ -122,19 +122,18 @@ void ServoParameters::declare(const std::string& ns, node_parameters->declare_parameter(ns + ".scale.linear", ParameterValue{ parameters.linear_scale }, ParameterDescriptorBuilder{} .type(PARAMETER_DOUBLE) - .description("Max linear velocity. Meters per publish_period. Unit is [m/s]. " + .description("Max linear velocity. Unit is [m/s]. " "Only used for Cartesian commands.")); node_parameters->declare_parameter(ns + ".scale.rotational", ParameterValue{ parameters.rotational_scale }, ParameterDescriptorBuilder{} .type(PARAMETER_DOUBLE) - .description("Max angular velocity. Rads per publish_period. Unit is [rad/s]. " + .description("Max angular velocity. Unit is [rad/s]. " "Only used for Cartesian commands.")); - node_parameters->declare_parameter( - ns + ".scale.joint", ParameterValue{ parameters.joint_scale }, - ParameterDescriptorBuilder{} - .type(PARAMETER_DOUBLE) - .description("Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint " - "commands on joint_command_in_topic.")); + node_parameters->declare_parameter(ns + ".scale.joint", ParameterValue{ parameters.joint_scale }, + ParameterDescriptorBuilder{} + .type(PARAMETER_DOUBLE) + .description("Max joint angular/linear velocity. Only used for joint " + "commands on joint_command_in_topic.")); // Properties of outgoing commands node_parameters->declare_parameter( diff --git a/moveit_ros/moveit_servo/test/enforce_limits_tests.cpp b/moveit_ros/moveit_servo/test/enforce_limits_tests.cpp index ab9f12cb12..12f79421a0 100644 --- a/moveit_ros/moveit_servo/test/enforce_limits_tests.cpp +++ b/moveit_ros/moveit_servo/test/enforce_limits_tests.cpp @@ -44,20 +44,19 @@ namespace { constexpr double PUBLISH_PERIOD = 0.01; -void checkVelocityLimits(const moveit::core::JointModelGroup* joint_model_group, const Eigen::ArrayXd& delta_theta) +void checkVelocityLimits(const moveit::core::JointModelGroup* joint_model_group, const Eigen::ArrayXd& velocity) { - std::size_t joint_delta_index{ 0 }; + std::size_t joint_index{ 0 }; for (const moveit::core::JointModel* joint : joint_model_group->getActiveJointModels()) { const auto& bounds = joint->getVariableBounds(joint->getName()); if (bounds.velocity_bounded_) { - auto velocity = delta_theta(joint_delta_index) / PUBLISH_PERIOD; - EXPECT_GE(velocity, bounds.min_velocity_) << "Joint " << joint_delta_index << " violates velocity limit"; - EXPECT_LE(velocity, bounds.max_velocity_) << "Joint " << joint_delta_index << " violates velocity limit"; + EXPECT_GE(velocity(joint_index), bounds.min_velocity_) << "Joint " << joint_index << " violates velocity limit"; + EXPECT_LE(velocity(joint_index), bounds.max_velocity_) << "Joint " << joint_index << " violates velocity limit"; } - ++joint_delta_index; + ++joint_index; } } @@ -78,65 +77,57 @@ class EnforceLimitsTests : public testing::Test TEST_F(EnforceLimitsTests, VelocityScalingTest) { - // Request joint angle changes that are too fast given the control period. - Eigen::ArrayXd delta_theta(7); - delta_theta[0] = 0; // rad - delta_theta[1] = 0.01; - delta_theta[2] = 0.02; - delta_theta[3] = 0.03; - delta_theta[4] = 0.04; - delta_theta[5] = 0.05; - delta_theta[6] = 0.06; - - // Store the original joint commands for comparison before applying velocity scaling. - auto result_delta_theta = moveit_servo::enforceVelocityLimits(joint_model_group_, PUBLISH_PERIOD, delta_theta); - - // Test that we don't violate velocity limits - checkVelocityLimits(joint_model_group_, result_delta_theta); + // Request velocities that are too fast + std::vector joint_position{ 0, 0, 0, 0, 0, 0, 0 }; + std::vector joint_velocity{ 0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 }; + sensor_msgs::msg::JointState joint_state; + joint_state.position = joint_position; + joint_state.velocity = joint_velocity; + + moveit_servo::enforceVelocityLimits(joint_model_group_, PUBLISH_PERIOD, joint_state); + + Eigen::ArrayXd eigen_velocity = + Eigen::Map(joint_state.velocity.data(), joint_state.velocity.size()); + checkVelocityLimits(joint_model_group_, eigen_velocity); } TEST_F(EnforceLimitsTests, NegativeJointAngleDeltasTest) { - // Now, negative joint angle deltas. Some will result to velocities - // greater than the arm joint velocity limits. - Eigen::ArrayXd delta_theta(7); - delta_theta[0] = 0.01; // rad - delta_theta[1] = -0.01; - delta_theta[2] = -0.02; - delta_theta[3] = -0.03; - delta_theta[4] = -0.04; - delta_theta[5] = -0.05; - delta_theta[6] = -0.06; - - // Store the original joint commands for comparison before applying velocity scaling. - auto result_delta_theta = moveit_servo::enforceVelocityLimits(joint_model_group_, PUBLISH_PERIOD, delta_theta); - - // Test that we don't violate velocity limits - checkVelocityLimits(joint_model_group_, result_delta_theta); + // Negative velocities exceeding the limit + std::vector joint_position{ 0, 0, 0, 0, 0, 0, 0 }; + std::vector joint_velocity{ 0, -1.0, -2.0, -3.0, -4.0, -5.0, -6.0 }; + sensor_msgs::msg::JointState joint_state; + joint_state.position = joint_position; + joint_state.velocity = joint_velocity; + + moveit_servo::enforceVelocityLimits(joint_model_group_, PUBLISH_PERIOD, joint_state); + + Eigen::ArrayXd eigen_velocity = + Eigen::Map(joint_state.velocity.data(), joint_state.velocity.size()); + checkVelocityLimits(joint_model_group_, eigen_velocity); } TEST_F(EnforceLimitsTests, LowJointVelocityDeltaTest) { - // Final test with joint angle deltas that will result in velocities - // below the lowest Panda arm joint velocity limit. - Eigen::ArrayXd delta_theta(7); - delta_theta[0] = 0; // rad - delta_theta[1] = -0.013; - delta_theta[2] = 0.023; - delta_theta[3] = -0.004; - delta_theta[4] = 0.021; - delta_theta[5] = 0.012; - delta_theta[6] = 0.0075; - - // Store the original joint commands for comparison before applying velocity scaling. - auto result_delta_theta = moveit_servo::enforceVelocityLimits(joint_model_group_, PUBLISH_PERIOD, delta_theta); - - // Test that we don't violate velocity limits - checkVelocityLimits(joint_model_group_, result_delta_theta); + // Final test with joint velocities that are acceptable + std::vector joint_position{ 0, 0, 0, 0, 0, 0, 0 }; + std::vector joint_velocity{ 0, 0.001, 0.001, -0.001, 0.001, 0.001, 0.001 }; + sensor_msgs::msg::JointState joint_state; + joint_state.position = joint_position; + joint_state.velocity = joint_velocity; + + moveit_servo::enforceVelocityLimits(joint_model_group_, PUBLISH_PERIOD, joint_state); + + Eigen::ArrayXd eigen_velocity = + Eigen::Map(joint_state.velocity.data(), joint_state.velocity.size()); + checkVelocityLimits(joint_model_group_, eigen_velocity); } int main(int argc, char** argv) { + rclcpp::init(argc, argv); ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; } From 12e9b6ccdec1bf4c64fef4ab6282c539c0ded3c4 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Mon, 10 Jan 2022 22:56:10 +0000 Subject: [PATCH 163/229] Add hybrid planning and pilz build farms to readme (#982) --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index d1736c3c47..5a292af15b 100644 --- a/README.md +++ b/README.md @@ -65,6 +65,7 @@ research and innovation programme under grant agreement no. 732287. | MoveIt 2 Package | Foxy Source | Foxy Debian | Galactic Source | Galactic Debian | Rolling Source | Rolling Debian | |:---:|:---:|:---:|:---:|:---:|:---:|:---:| | geometric_shapes | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Fsrc_uF__geometric_shapes__ubuntu_focal__source)](https://build.ros2.org/job/Fsrc_uF__geometric_shapes__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Fbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Fbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gsrc_uF__geometric_shapes__ubuntu_focal__source)](https://build.ros2.org/job/Gsrc_uF__geometric_shapes__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Gbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rsrc_uF__geometric_shapes__ubuntu_focal__source)](https://build.ros2.org/job/Rsrc_uF__geometric_shapes__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Rbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary/) | +| hybrid_planning | | | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gsrc_uF__moveit_hybrid_planning__ubuntu_focal__source)](https://build.ros2.org/job/Gsrc_uF__moveit_hybrid_planning__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gbin_uF64__moveit_hybrid_planning__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Gbin_uF64__moveit_hybrid_planning__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rsrc_uF__moveit_hybrid_planning__ubuntu_focal__source)](https://build.ros2.org/job/Rsrc_uF__moveit_hybrid_planning__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uF64__moveit_hybrid_planning__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Rbin_uF64__moveit_hybrid_planning__ubuntu_focal_amd64__binary/) | | moveit | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Fsrc_uF__moveit__ubuntu_focal__source)](https://build.ros2.org/job/Fsrc_uF__moveit__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Fbin_uF64__moveit__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Fbin_uF64__moveit__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gsrc_uF__moveit__ubuntu_focal__source)](https://build.ros2.org/job/Gsrc_uF__moveit__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gbin_uF64__moveit__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Gbin_uF64__moveit__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rsrc_uF__moveit__ubuntu_focal__source)](https://build.ros2.org/job/Rsrc_uF__moveit__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uF64__moveit__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Rbin_uF64__moveit__ubuntu_focal_amd64__binary/) | | moveit_common | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Fsrc_uF__moveit_common__ubuntu_focal__source)](https://build.ros2.org/job/Fsrc_uF__moveit_common__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Fbin_uF64__moveit_common__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Fbin_uF64__moveit_common__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gsrc_uF__moveit_common__ubuntu_focal__source)](https://build.ros2.org/job/Gsrc_uF__moveit_common__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gbin_uF64__moveit_common__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Gbin_uF64__moveit_common__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rsrc_uF__moveit_common__ubuntu_focal__source)](https://build.ros2.org/job/Rsrc_uF__moveit_common__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uF64__moveit_common__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Rbin_uF64__moveit_common__ubuntu_focal_amd64__binary/) | | moveit_core | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Fsrc_uF__moveit_core__ubuntu_focal__source)](https://build.ros2.org/job/Fsrc_uF__moveit_core__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Fbin_uF64__moveit_core__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Fbin_uF64__moveit_core__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gsrc_uF__moveit_core__ubuntu_focal__source)](https://build.ros2.org/job/Gsrc_uF__moveit_core__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gbin_uF64__moveit_core__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Gbin_uF64__moveit_core__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rsrc_uF__moveit_core__ubuntu_focal__source)](https://build.ros2.org/job/Rsrc_uF__moveit_core__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uF64__moveit_core__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Rbin_uF64__moveit_core__ubuntu_focal_amd64__binary/) | @@ -92,6 +93,8 @@ research and innovation programme under grant agreement no. 732287. | moveit_runtime | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Fsrc_uF__moveit_runtime__ubuntu_focal__source)](https://build.ros2.org/job/Fsrc_uF__moveit_runtime__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Fbin_uF64__moveit_runtime__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Fbin_uF64__moveit_runtime__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gsrc_uF__moveit_runtime__ubuntu_focal__source)](https://build.ros2.org/job/Gsrc_uF__moveit_runtime__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gbin_uF64__moveit_runtime__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Gbin_uF64__moveit_runtime__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rsrc_uF__moveit_runtime__ubuntu_focal__source)](https://build.ros2.org/job/Rsrc_uF__moveit_runtime__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uF64__moveit_runtime__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Rbin_uF64__moveit_runtime__ubuntu_focal_amd64__binary/) | | moveit_servo | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Fsrc_uF__moveit_servo__ubuntu_focal__source)](https://build.ros2.org/job/Fsrc_uF__moveit_servo__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Fbin_uF64__moveit_servo__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Fbin_uF64__moveit_servo__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gsrc_uF__moveit_servo__ubuntu_focal__source)](https://build.ros2.org/job/Gsrc_uF__moveit_servo__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gbin_uF64__moveit_servo__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Gbin_uF64__moveit_servo__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rsrc_uF__moveit_servo__ubuntu_focal__source)](https://build.ros2.org/job/Rsrc_uF__moveit_servo__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uF64__moveit_servo__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Rbin_uF64__moveit_servo__ubuntu_focal_amd64__binary/) | | moveit_simple_controller_manager | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Fsrc_uF__moveit_simple_controller_manager__ubuntu_focal__source)](https://build.ros2.org/job/Fsrc_uF__moveit_simple_controller_manager__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Fbin_uF64__moveit_simple_controller_manager__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Fbin_uF64__moveit_simple_controller_manager__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gsrc_uF__moveit_simple_controller_manager__ubuntu_focal__source)](https://build.ros2.org/job/Gsrc_uF__moveit_simple_controller_manager__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gbin_uF64__moveit_simple_controller_manager__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Gbin_uF64__moveit_simple_controller_manager__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rsrc_uF__moveit_simple_controller_manager__ubuntu_focal__source)](https://build.ros2.org/job/Rsrc_uF__moveit_simple_controller_manager__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uF64__moveit_simple_controller_manager__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Rbin_uF64__moveit_simple_controller_manager__ubuntu_focal_amd64__binary/) | +| pilz_industrial_motion_planner | | | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gsrc_uF__pilz_industrial_motion_planner__ubuntu_focal__source)](https://build.ros2.org/job/Gsrc_uF__pilz_industrial_motion_planner__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gbin_uF64__pilz_industrial_motion_planner__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Gbin_uF64__pilz_industrial_motion_planner__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rsrc_uF__pilz_industrial_motion_planner__ubuntu_focal__source)](https://build.ros2.org/job/Rsrc_uF__pilz_industrial_motion_planner__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uF64__pilz_industrial_motion_planner__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Rbin_uF64__pilz_industrial_motion_planner__ubuntu_focal_amd64__binary/) | +| pilz_industrial_motion_planner_testutils | | | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gsrc_uF__pilz_industrial_motion_planner_testutils__ubuntu_focal__source)](https://build.ros2.org/job/Gsrc_uF__pilz_industrial_motion_planner_testutils__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gbin_uF64__pilz_industrial_motion_planner_testutils__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Gbin_uF64__pilz_industrial_motion_planner_testutils__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rsrc_uF__pilz_industrial_motion_planner_testutils__ubuntu_focal__source)](https://build.ros2.org/job/Rsrc_uF__pilz_industrial_motion_planner_testutils__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uF64__pilz_industrial_motion_planner_testutils__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Rbin_uF64__pilz_industrial_motion_planner_testutils__ubuntu_focal_amd64__binary/) | | random_numbers | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Fsrc_uF__random_numbers__ubuntu_focal__source)](https://build.ros2.org/job/Fsrc_uF__random_numbers__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Fbin_uF64__random_numbers__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Fbin_uF64__random_numbers__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gsrc_uF__random_numbers__ubuntu_focal__source)](https://build.ros2.org/job/Gsrc_uF__random_numbers__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gbin_uF64__random_numbers__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Gbin_uF64__random_numbers__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rsrc_uF__random_numbers__ubuntu_focal__source)](https://build.ros2.org/job/Rsrc_uF__random_numbers__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uF64__random_numbers__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Rbin_uF64__random_numbers__ubuntu_focal_amd64__binary/) | | srdfdom | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Fsrc_uF__srdfdom__ubuntu_focal__source)](https://build.ros2.org/job/Fsrc_uF__srdfdom__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Fbin_uF64__srdfdom__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Fbin_uF64__srdfdom__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gsrc_uF__srdfdom__ubuntu_focal__source)](https://build.ros2.org/job/Gsrc_uF__srdfdom__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gbin_uF64__srdfdom__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Gbin_uF64__srdfdom__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rsrc_uF__srdfdom__ubuntu_focal__source)](https://build.ros2.org/job/Rsrc_uF__srdfdom__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uF64__srdfdom__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Rbin_uF64__srdfdom__ubuntu_focal_amd64__binary/) | | warehouse_ros | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Fsrc_uF__warehouse_ros__ubuntu_focal__source)](https://build.ros2.org/job/Fsrc_uF__warehouse_ros__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Fbin_uF64__warehouse_ros__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Fbin_uF64__warehouse_ros__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gsrc_uF__warehouse_ros__ubuntu_focal__source)](https://build.ros2.org/job/Gsrc_uF__warehouse_ros__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Gbin_uF64__warehouse_ros__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Gbin_uF64__warehouse_ros__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rsrc_uF__warehouse_ros__ubuntu_focal__source)](https://build.ros2.org/job/Rsrc_uF__warehouse_ros__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uF64__warehouse_ros__ubuntu_focal_amd64__binary)](https://build.ros2.org/job/Rbin_uF64__warehouse_ros__ubuntu_focal_amd64__binary/) | From 1faa2b16c94271a9290a6106181f8ef856f9a4ca Mon Sep 17 00:00:00 2001 From: Abishalini Date: Wed, 12 Jan 2022 07:58:21 -0700 Subject: [PATCH 164/229] Remove moveit_build_options --- moveit_core/cmake/moveit.cmake | 22 ---------------------- moveit_planners/trajopt/CMakeLists.txt | 1 - 2 files changed, 23 deletions(-) delete mode 100644 moveit_core/cmake/moveit.cmake diff --git a/moveit_core/cmake/moveit.cmake b/moveit_core/cmake/moveit.cmake deleted file mode 100644 index 831c218aef..0000000000 --- a/moveit_core/cmake/moveit.cmake +++ /dev/null @@ -1,22 +0,0 @@ -macro(moveit_build_options) - if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) - endif() - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) - - if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - message("${PROJECT_NAME}: You did not request a specific build type: Choosing 'Release' for maximum performance") - set(CMAKE_BUILD_TYPE Release) - endif() - - option(MOVEIT_CI_WARNINGS "Enable all warnings used by CI" ON) - if(MOVEIT_CI_WARNINGS) - if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") - add_compile_options(-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wcast-qual) - endif() - if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang") - add_compile_options(-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wcast-qual) - endif() - endif() -endmacro() diff --git a/moveit_planners/trajopt/CMakeLists.txt b/moveit_planners/trajopt/CMakeLists.txt index 536bf4e85a..547ba41077 100644 --- a/moveit_planners/trajopt/CMakeLists.txt +++ b/moveit_planners/trajopt/CMakeLists.txt @@ -12,7 +12,6 @@ find_package(catkin REQUIRED rosparam_shortcuts trajopt ) -moveit_build_options() catkin_package( INCLUDE_DIRS include From 800e37b546c1e91db5a5bf831cd37f1b56e556d8 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 12 Jan 2022 19:19:09 -0700 Subject: [PATCH 165/229] Use size_t for index variables (#946) --- .../moveit/robot_model/fixed_joint_model.h | 2 +- .../moveit/robot_model/floating_joint_model.h | 2 +- .../include/moveit/robot_model/joint_model.h | 49 ++++---- .../include/moveit/robot_model/link_model.h | 21 ++-- .../moveit/robot_model/planar_joint_model.h | 2 +- .../robot_model/prismatic_joint_model.h | 2 +- .../moveit/robot_model/revolute_joint_model.h | 2 +- .../include/moveit/robot_model/robot_model.h | 12 +- .../robot_model/src/fixed_joint_model.cpp | 3 +- .../robot_model/src/floating_joint_model.cpp | 5 +- moveit_core/robot_model/src/joint_model.cpp | 8 +- moveit_core/robot_model/src/link_model.cpp | 4 +- .../robot_model/src/planar_joint_model.cpp | 9 +- .../robot_model/src/prismatic_joint_model.cpp | 7 +- .../robot_model/src/revolute_joint_model.cpp | 16 ++- moveit_core/robot_model/src/robot_model.cpp | 109 +++++++++--------- moveit_core/robot_model/test/test.cpp | 4 +- 17 files changed, 133 insertions(+), 124 deletions(-) diff --git a/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h index 855cbec6c7..9f3356fdcb 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h @@ -46,7 +46,7 @@ namespace core class FixedJointModel : public JointModel { public: - FixedJointModel(const std::string& name); + FixedJointModel(const std::string& name, size_t joint_index, size_t first_variable_index); void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override; void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, diff --git a/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h index 0d0e0a60cc..484779512d 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h @@ -46,7 +46,7 @@ namespace core class FloatingJointModel : public JointModel { public: - FloatingJointModel(const std::string& name); + FloatingJointModel(const std::string& name, size_t joint_index, size_t first_variable_index); void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override; void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h index 9467c72548..349dfa4146 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h @@ -90,7 +90,7 @@ class LinkModel; class JointModel; /** \brief Data type for holding mappings from variable names to their position in a state vector */ -typedef std::map VariableIndexMap; +typedef std::map VariableIndexMap; /** \brief Data type for holding mappings from variable names to their bounds */ using VariableBoundsMap = std::map; @@ -130,8 +130,14 @@ class JointModel /** \brief The datatype for the joint bounds */ using Bounds = std::vector; - /** \brief Construct a joint named \e name */ - JointModel(const std::string& name); + /** + * @brief Constructs a joint named \e name + * + * @param[in] name The joint name + * @param[in] index The index of the joint in the RobotModel + * @param[in] first_variable_index The index of the first variable in the RobotModel + */ + JointModel(const std::string& name, size_t joint_index, size_t first_variable_index); virtual ~JointModel(); @@ -207,31 +213,19 @@ class JointModel } /** \brief Get the index of this joint's first variable within the full robot state */ - int getFirstVariableIndex() const + size_t getFirstVariableIndex() const { return first_variable_index_; } - /** \brief Set the index of this joint's first variable within the full robot state */ - void setFirstVariableIndex(int index) - { - first_variable_index_ = index; - } - /** \brief Get the index of this joint within the robot model */ - int getJointIndex() const + size_t getJointIndex() const { return joint_index_; } - /** \brief Set the index of this joint within the robot model */ - void setJointIndex(int index) - { - joint_index_ = index; - } - /** \brief Get the index of the variable within this joint */ - int getLocalVariableIndex(const std::string& variable) const; + size_t getLocalVariableIndex(const std::string& variable) const; /** @} */ @@ -462,12 +456,19 @@ class JointModel /** @} */ -protected: - void computeVariableBoundsMsg(); - +private: /** \brief Name of the joint */ std::string name_; + /** \brief Index for this joint in the array of joints of the complete model */ + size_t joint_index_; + + /** \brief The index of this joint's first variable, in the complete robot state */ + size_t first_variable_index_; + +protected: + void computeVariableBoundsMsg(); + /** \brief The type of joint */ JointType type_; @@ -519,12 +520,6 @@ class JointModel /** \brief The factor applied to the distance between two joint states */ double distance_factor_; - - /** \brief The index of this joint's first variable, in the complete robot state */ - int first_variable_index_; - - /** \brief Index for this joint in the array of joints of the complete model */ - int joint_index_; }; /** \brief Operator overload for printing variable bounds to a stream */ diff --git a/moveit_core/robot_model/include/moveit/robot_model/link_model.h b/moveit_core/robot_model/include/moveit/robot_model/link_model.h index eee845316e..a9cd17df07 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/link_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/link_model.h @@ -73,7 +73,13 @@ class LinkModel public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - LinkModel(const std::string& name); + /** + * @brief Construct a link model named \e name + * + * @param[in] name The name of the link + * @param[in] link_index The link index in the RobotModel + */ + LinkModel(const std::string& name, size_t link_index); ~LinkModel(); /** \brief The name of this link */ @@ -83,16 +89,11 @@ class LinkModel } /** \brief The index of this joint when traversing the kinematic tree in depth first fashion */ - int getLinkIndex() const + size_t getLinkIndex() const { return link_index_; } - void setLinkIndex(int index) - { - link_index_ = index; - } - int getFirstCollisionBodyTransformIndex() const { return first_collision_body_transform_index_; @@ -231,6 +232,9 @@ class LinkModel /** \brief Name of the link */ std::string name_; + /** \brief Index of the transform for this link in the full robot frame */ + size_t link_index_; + /** \brief JointModel that connects this link to the parent link */ const JointModel* parent_joint_model_; @@ -280,9 +284,6 @@ class LinkModel /** \brief Index of the transform for the first shape that makes up the geometry of this link in the full robot state */ int first_collision_body_transform_index_; - - /** \brief Index of the transform for this link in the full robot frame */ - int link_index_; }; } // namespace core } // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h index 4d947f0530..cc95677872 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h @@ -53,7 +53,7 @@ class PlanarJointModel : public JointModel DIFF_DRIVE }; - PlanarJointModel(const std::string& name); + PlanarJointModel(const std::string& name, size_t joint_index, size_t first_variable_index); void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override; void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, diff --git a/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h index ece60ffe9c..c69e523a55 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h @@ -48,7 +48,7 @@ class PrismaticJointModel : public JointModel public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - PrismaticJointModel(const std::string& name); + PrismaticJointModel(const std::string& name, size_t joint_index, size_t first_variable_index); void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override; void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, diff --git a/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h index 9d70cf0628..935a12a697 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h @@ -48,7 +48,7 @@ class RevoluteJointModel : public JointModel public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - RevoluteJointModel(const std::string& name); + RevoluteJointModel(const std::string& name, size_t joint_index, size_t first_variable_index); void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override; void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, const Bounds& other_bounds) const override; diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h index c6195d8fc6..760711fb43 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h @@ -139,7 +139,7 @@ class RobotModel const JointModel* getJointModel(const std::string& joint) const; /** \brief Get a joint by its index. Output error and return nullptr when the link is missing. */ - const JointModel* getJointModel(int index) const; + const JointModel* getJointModel(size_t index) const; /** \brief Get a joint by its name. Output error and return nullptr when the joint is missing. */ JointModel* getJointModel(const std::string& joint); @@ -248,7 +248,7 @@ class RobotModel const LinkModel* getLinkModel(const std::string& link, bool* has_link = nullptr) const; /** \brief Get a link by its index. Output error and return nullptr when the link is missing. */ - const LinkModel* getLinkModel(int index) const; + const LinkModel* getLinkModel(size_t index) const; /** \brief Get a link by its name. Output error and return nullptr when the link is missing. */ LinkModel* getLinkModel(const std::string& link, bool* has_link = nullptr); @@ -444,7 +444,7 @@ class RobotModel std::vector& missing_variables) const; /** \brief Get the index of a variable in the robot state */ - int getVariableIndex(const std::string& variable) const; + size_t getVariableIndex(const std::string& variable) const; /** \brief Get the deepest joint in the kinematic tree that is a common parent of both joints passed as argument */ const JointModel* getCommonRoot(const JointModel* a, const JointModel* b) const @@ -596,6 +596,7 @@ class RobotModel /** \brief The array of end-effectors, in alphabetical order */ std::vector end_effectors_; +private: /** \brief Given an URDF model and a SRDF model, build a full kinematic model */ void buildModel(const urdf::ModelInterface& urdf_model, const srdf::Model& srdf_model); @@ -630,10 +631,9 @@ class RobotModel /** \brief Construct a JointModelGroup given a SRDF description \e group */ bool addJointModelGroup(const srdf::Model::Group& group); - /** \brief Given a urdf joint model, a child link and a set of virtual joints, + /** \brief Given a child link and a srdf model, build up the corresponding JointModel object*/ - JointModel* constructJointModel(const urdf::Joint* urdf_joint_model, const urdf::Link* child_link, - const srdf::Model& srdf_model); + JointModel* constructJointModel(const urdf::Link* child_link, const srdf::Model& srdf_model); /** \brief Given a urdf link, build the corresponding LinkModel object*/ LinkModel* constructLinkModel(const urdf::Link* urdf_link); diff --git a/moveit_core/robot_model/src/fixed_joint_model.cpp b/moveit_core/robot_model/src/fixed_joint_model.cpp index f96dd3b3b3..5873f0c951 100644 --- a/moveit_core/robot_model/src/fixed_joint_model.cpp +++ b/moveit_core/robot_model/src/fixed_joint_model.cpp @@ -40,7 +40,8 @@ namespace moveit { namespace core { -FixedJointModel::FixedJointModel(const std::string& name) : JointModel(name) +FixedJointModel::FixedJointModel(const std::string& name, size_t joint_index, size_t first_variable_index) + : JointModel(name, joint_index, first_variable_index) { type_ = FIXED; } diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index 72909b81c0..6af1cd63b5 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -53,7 +53,8 @@ constexpr size_t STATE_SPACE_DIMENSION = 7; } // namespace -FloatingJointModel::FloatingJointModel(const std::string& name) : JointModel(name), angular_distance_weight_(1.0) +FloatingJointModel::FloatingJointModel(const std::string& name, size_t joint_index, size_t first_variable_index) + : JointModel(name, joint_index, first_variable_index), angular_distance_weight_(1.0) { type_ = FLOATING; local_variable_names_.push_back("trans_x"); @@ -65,7 +66,7 @@ FloatingJointModel::FloatingJointModel(const std::string& name) : JointModel(nam local_variable_names_.push_back("rot_w"); for (size_t i = 0; i < STATE_SPACE_DIMENSION; ++i) { - variable_names_.push_back(name_ + "/" + local_variable_names_[i]); + variable_names_.push_back(getName() + "/" + local_variable_names_[i]); variable_index_map_[variable_names_.back()] = i; } diff --git a/moveit_core/robot_model/src/joint_model.cpp b/moveit_core/robot_model/src/joint_model.cpp index a9a9b8d726..f428cb3cb8 100644 --- a/moveit_core/robot_model/src/joint_model.cpp +++ b/moveit_core/robot_model/src/joint_model.cpp @@ -44,8 +44,10 @@ namespace moveit { namespace core { -JointModel::JointModel(const std::string& name) +JointModel::JointModel(const std::string& name, size_t joint_index, size_t first_variable_index) : name_(name) + , joint_index_(joint_index) + , first_variable_index_(first_variable_index) , type_(UNKNOWN) , parent_link_model_(nullptr) , child_link_model_(nullptr) @@ -54,8 +56,6 @@ JointModel::JointModel(const std::string& name) , mimic_offset_(0.0) , passive_(false) , distance_factor_(1.0) - , first_variable_index_(-1) - , joint_index_(-1) { } @@ -82,7 +82,7 @@ std::string JointModel::getTypeName() const } } -int JointModel::getLocalVariableIndex(const std::string& variable) const +size_t JointModel::getLocalVariableIndex(const std::string& variable) const { VariableIndexMap::const_iterator it = variable_index_map_.find(variable); if (it == variable_index_map_.end()) diff --git a/moveit_core/robot_model/src/link_model.cpp b/moveit_core/robot_model/src/link_model.cpp index 91cf365af9..cd33186b92 100644 --- a/moveit_core/robot_model/src/link_model.cpp +++ b/moveit_core/robot_model/src/link_model.cpp @@ -44,14 +44,14 @@ namespace moveit { namespace core { -LinkModel::LinkModel(const std::string& name) +LinkModel::LinkModel(const std::string& name, size_t link_index) : name_(name) + , link_index_(link_index) , parent_joint_model_(nullptr) , parent_link_model_(nullptr) , is_parent_joint_fixed_(false) , joint_origin_transform_is_identity_(true) , first_collision_body_transform_index_(-1) - , link_index_(-1) { joint_origin_transform_.setIdentity(); } diff --git a/moveit_core/robot_model/src/planar_joint_model.cpp b/moveit_core/robot_model/src/planar_joint_model.cpp index 593c9eae60..2e1978ad0d 100644 --- a/moveit_core/robot_model/src/planar_joint_model.cpp +++ b/moveit_core/robot_model/src/planar_joint_model.cpp @@ -46,8 +46,11 @@ namespace moveit { namespace core { -PlanarJointModel::PlanarJointModel(const std::string& name) - : JointModel(name), angular_distance_weight_(1.0), motion_model_(HOLONOMIC), min_translational_distance_(1e-5) +PlanarJointModel::PlanarJointModel(const std::string& name, size_t joint_index, size_t first_variable_index) + : JointModel(name, joint_index, first_variable_index) + , angular_distance_weight_(1.0) + , motion_model_(HOLONOMIC) + , min_translational_distance_(1e-5) { type_ = PLANAR; @@ -56,7 +59,7 @@ PlanarJointModel::PlanarJointModel(const std::string& name) local_variable_names_.push_back("theta"); for (int i = 0; i < 3; ++i) { - variable_names_.push_back(name_ + "/" + local_variable_names_[i]); + variable_names_.push_back(getName() + "/" + local_variable_names_[i]); variable_index_map_[variable_names_.back()] = i; } diff --git a/moveit_core/robot_model/src/prismatic_joint_model.cpp b/moveit_core/robot_model/src/prismatic_joint_model.cpp index 65a95ad507..b259d506c5 100644 --- a/moveit_core/robot_model/src/prismatic_joint_model.cpp +++ b/moveit_core/robot_model/src/prismatic_joint_model.cpp @@ -41,15 +41,16 @@ namespace moveit { namespace core { -PrismaticJointModel::PrismaticJointModel(const std::string& name) : JointModel(name), axis_(0.0, 0.0, 0.0) +PrismaticJointModel::PrismaticJointModel(const std::string& name, size_t joint_index, size_t first_variable_index) + : JointModel(name, joint_index, first_variable_index), axis_(0.0, 0.0, 0.0) { type_ = PRISMATIC; - variable_names_.push_back(name_); + variable_names_.push_back(getName()); variable_bounds_.resize(1); variable_bounds_[0].position_bounded_ = true; variable_bounds_[0].min_position_ = -std::numeric_limits::max(); variable_bounds_[0].max_position_ = std::numeric_limits::max(); - variable_index_map_[name_] = 0; + variable_index_map_[getName()] = 0; computeVariableBoundsMsg(); } diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp index 6b1305b5ef..bc0a8320ad 100644 --- a/moveit_core/robot_model/src/revolute_joint_model.cpp +++ b/moveit_core/robot_model/src/revolute_joint_model.cpp @@ -45,16 +45,24 @@ namespace moveit { namespace core { -RevoluteJointModel::RevoluteJointModel(const std::string& name) - : JointModel(name), axis_(0.0, 0.0, 0.0), continuous_(false), x2_(0.0), y2_(0.0), z2_(0.0), xy_(0.0), xz_(0.0), yz_(0.0) +RevoluteJointModel::RevoluteJointModel(const std::string& name, size_t joint_index, size_t first_variable_index) + : JointModel(name, joint_index, first_variable_index) + , axis_(0.0, 0.0, 0.0) + , continuous_(false) + , x2_(0.0) + , y2_(0.0) + , z2_(0.0) + , xy_(0.0) + , xz_(0.0) + , yz_(0.0) { type_ = REVOLUTE; - variable_names_.push_back(name_); + variable_names_.push_back(getName()); variable_bounds_.resize(1); variable_bounds_[0].position_bounded_ = true; variable_bounds_[0].min_position_ = -boost::math::constants::pi(); variable_bounds_[0].max_position_ = boost::math::constants::pi(); - variable_index_map_[name_] = 0; + variable_index_map_[getName()] = 0; computeVariableBoundsMsg(); } diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index ca822dc9b3..6c5386266c 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -259,10 +259,9 @@ void RobotModel::buildJointInfo() variable_names_.reserve(joint_model_vector_.size()); joints_of_variable_.reserve(joint_model_vector_.size()); - for (std::size_t i = 0; i < joint_model_vector_.size(); ++i) + for (const auto& joint : joint_model_vector_) { - joint_model_vector_[i]->setJointIndex(i); - const std::vector& name_order = joint_model_vector_[i]->getVariableNames(); + const std::vector& name_order = joint->getVariableNames(); // compute index map if (!name_order.empty()) @@ -271,31 +270,29 @@ void RobotModel::buildJointInfo() { joint_variables_index_map_[name_order[j]] = variable_count_ + j; variable_names_.push_back(name_order[j]); - joints_of_variable_.push_back(joint_model_vector_[i]); + joints_of_variable_.push_back(joint); } - if (joint_model_vector_[i]->getMimic() == nullptr) + if (joint->getMimic() == nullptr) { active_joint_model_start_index_.push_back(variable_count_); - active_joint_model_vector_.push_back(joint_model_vector_[i]); - active_joint_model_names_vector_.push_back(joint_model_vector_[i]->getName()); - active_joint_model_vector_const_.push_back(joint_model_vector_[i]); - active_joint_models_bounds_.push_back(&joint_model_vector_[i]->getVariableBounds()); + active_joint_model_vector_.push_back(joint); + active_joint_model_names_vector_.push_back(joint->getName()); + active_joint_model_vector_const_.push_back(joint); + active_joint_models_bounds_.push_back(&joint->getVariableBounds()); } - if (joint_model_vector_[i]->getType() == JointModel::REVOLUTE && - static_cast(joint_model_vector_[i])->isContinuous()) - continuous_joint_model_vector_.push_back(joint_model_vector_[i]); + if (joint->getType() == JointModel::REVOLUTE && static_cast(joint)->isContinuous()) + continuous_joint_model_vector_.push_back(joint); - joint_model_vector_[i]->setFirstVariableIndex(variable_count_); - joint_variables_index_map_[joint_model_vector_[i]->getName()] = variable_count_; + joint_variables_index_map_[joint->getName()] = variable_count_; // compute variable count - std::size_t vc = joint_model_vector_[i]->getVariableCount(); + std::size_t vc = joint->getVariableCount(); variable_count_ += vc; if (vc == 1) - single_dof_joints_.push_back(joint_model_vector_[i]); + single_dof_joints_.push_back(joint); else - multi_dof_joints_.push_back(joint_model_vector_[i]); + multi_dof_joints_.push_back(joint); } } @@ -789,16 +786,14 @@ bool RobotModel::addJointModelGroup(const srdf::Model::Group& gc) JointModel* RobotModel::buildRecursive(LinkModel* parent, const urdf::Link* urdf_link, const srdf::Model& srdf_model) { // construct the joint - JointModel* joint = urdf_link->parent_joint ? - constructJointModel(urdf_link->parent_joint.get(), urdf_link, srdf_model) : - constructJointModel(nullptr, urdf_link, srdf_model); + JointModel* joint = constructJointModel(urdf_link, srdf_model); + if (joint == nullptr) return nullptr; // bookkeeping for the joint - joint_model_map_[joint->getName()] = joint; - joint->setJointIndex(joint_model_vector_.size()); joint_model_vector_.push_back(joint); + joint_model_map_[joint->getName()] = joint; joint_model_vector_const_.push_back(joint); joint_model_names_vector_.push_back(joint->getName()); joint->setParentLinkModel(parent); @@ -810,7 +805,6 @@ JointModel* RobotModel::buildRecursive(LinkModel* parent, const urdf::Link* urdf // bookkeeping for the link link_model_map_[joint->getChildLinkModel()->getName()] = link; - link->setLinkIndex(link_model_vector_.size()); link_model_vector_.push_back(link); link_model_vector_const_.push_back(link); link_model_names_vector_.push_back(link->getName()); @@ -871,57 +865,61 @@ static inline VariableBounds jointBoundsFromURDF(const urdf::Joint* urdf_joint) } } // namespace -JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const urdf::Link* child_link, - const srdf::Model& srdf_model) +JointModel* RobotModel::constructJointModel(const urdf::Link* child_link, const srdf::Model& srdf_model) { JointModel* new_joint_model = nullptr; - - // if urdf_joint exists, must be the root link transform - if (urdf_joint) + auto parent_joint = child_link->parent_joint ? child_link->parent_joint.get() : nullptr; + auto joint_index = joint_model_vector_.size(); + auto first_variable_index = joint_model_vector_.empty() ? 0 : + joint_model_vector_.back()->getFirstVariableIndex() + + joint_model_vector_.back()->getVariableCount(); + + // if parent_joint exists, must be the root link transform + if (parent_joint) { - switch (urdf_joint->type) + switch (parent_joint->type) { case urdf::Joint::REVOLUTE: { - RevoluteJointModel* j = new RevoluteJointModel(urdf_joint->name); - j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint)); + RevoluteJointModel* j = new RevoluteJointModel(parent_joint->name, joint_index, first_variable_index); + j->setVariableBounds(j->getName(), jointBoundsFromURDF(parent_joint)); j->setContinuous(false); - j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z)); + j->setAxis(Eigen::Vector3d(parent_joint->axis.x, parent_joint->axis.y, parent_joint->axis.z)); new_joint_model = j; } break; case urdf::Joint::CONTINUOUS: { - RevoluteJointModel* j = new RevoluteJointModel(urdf_joint->name); - j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint)); + RevoluteJointModel* j = new RevoluteJointModel(parent_joint->name, joint_index, first_variable_index); + j->setVariableBounds(j->getName(), jointBoundsFromURDF(parent_joint)); j->setContinuous(true); - j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z)); + j->setAxis(Eigen::Vector3d(parent_joint->axis.x, parent_joint->axis.y, parent_joint->axis.z)); new_joint_model = j; } break; case urdf::Joint::PRISMATIC: { - PrismaticJointModel* j = new PrismaticJointModel(urdf_joint->name); - j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint)); - j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z)); + PrismaticJointModel* j = new PrismaticJointModel(parent_joint->name, joint_index, first_variable_index); + j->setVariableBounds(j->getName(), jointBoundsFromURDF(parent_joint)); + j->setAxis(Eigen::Vector3d(parent_joint->axis.x, parent_joint->axis.y, parent_joint->axis.z)); new_joint_model = j; } break; case urdf::Joint::FLOATING: - new_joint_model = new FloatingJointModel(urdf_joint->name); + new_joint_model = new FloatingJointModel(parent_joint->name, joint_index, first_variable_index); break; case urdf::Joint::PLANAR: - new_joint_model = new PlanarJointModel(urdf_joint->name); + new_joint_model = new PlanarJointModel(parent_joint->name, joint_index, first_variable_index); break; case urdf::Joint::FIXED: - new_joint_model = new FixedJointModel(urdf_joint->name); + new_joint_model = new FixedJointModel(parent_joint->name, joint_index, first_variable_index); break; default: - RCLCPP_ERROR(LOGGER, "Unknown joint type: %d", static_cast(urdf_joint->type)); + RCLCPP_ERROR(LOGGER, "Unknown joint type: %d", static_cast(parent_joint->type)); break; } } - else // if urdf_joint passed in as null, then we're at root of URDF model + else // if parent_joint passed in as null, then we're at root of URDF model { const std::vector& virtual_joints = srdf_model.getVirtualJoints(); for (const srdf::Model::VirtualJoint& virtual_joint : virtual_joints) @@ -941,11 +939,11 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const else { if (virtual_joint.type_ == "fixed") - new_joint_model = new FixedJointModel(virtual_joint.name_); + new_joint_model = new FixedJointModel(virtual_joint.name_, joint_index, first_variable_index); else if (virtual_joint.type_ == "planar") - new_joint_model = new PlanarJointModel(virtual_joint.name_); + new_joint_model = new PlanarJointModel(virtual_joint.name_, joint_index, first_variable_index); else if (virtual_joint.type_ == "floating") - new_joint_model = new FloatingJointModel(virtual_joint.name_); + new_joint_model = new FloatingJointModel(virtual_joint.name_, joint_index, first_variable_index); if (new_joint_model) { // for fixed frames we still use the robot root link @@ -960,7 +958,7 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const if (!new_joint_model) { RCLCPP_INFO(LOGGER, "No root/virtual joint specified in SRDF. Assuming fixed joint"); - new_joint_model = new FixedJointModel("ASSUMED_FIXED_ROOT_JOINT"); + new_joint_model = new FixedJointModel("ASSUMED_FIXED_ROOT_JOINT", joint_index, first_variable_index); } } @@ -1096,7 +1094,8 @@ static inline Eigen::Isometry3d urdfPose2Isometry3d(const urdf::Pose& pose) LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link) { - LinkModel* new_link_model = new LinkModel(urdf_link->name); + auto link_index = link_model_vector_.size(); + LinkModel* new_link_model = new LinkModel(urdf_link->name, link_index); const std::vector& col_array = urdf_link->collision_array.empty() ? std::vector(1, urdf_link->collision) : @@ -1228,11 +1227,11 @@ const JointModel* RobotModel::getJointModel(const std::string& name) const return nullptr; } -const JointModel* RobotModel::getJointModel(int index) const +const JointModel* RobotModel::getJointModel(size_t index) const { - if (index < 0 || index >= static_cast(joint_model_vector_.size())) + if (index >= joint_model_vector_.size()) { - RCLCPP_ERROR(LOGGER, "Joint index '%i' out of bounds of joints in model '%s'", index, model_name_.c_str()); + RCLCPP_ERROR(LOGGER, "Joint index '%li' out of bounds of joints in model '%s'", index, model_name_.c_str()); return nullptr; } assert(joint_model_vector_[index]->getJointIndex() == index); @@ -1253,11 +1252,11 @@ const LinkModel* RobotModel::getLinkModel(const std::string& name, bool* has_lin return const_cast(this)->getLinkModel(name, has_link); } -const LinkModel* RobotModel::getLinkModel(int index) const +const LinkModel* RobotModel::getLinkModel(size_t index) const { - if (index < 0 || index >= static_cast(link_model_vector_.size())) + if (index >= link_model_vector_.size()) { - RCLCPP_ERROR(LOGGER, "Link index '%i' out of bounds of links in model '%s'", index, model_name_.c_str()); + RCLCPP_ERROR(LOGGER, "Link index '%li' out of bounds of links in model '%s'", index, model_name_.c_str()); return nullptr; } assert(link_model_vector_[index]->getLinkIndex() == index); @@ -1349,7 +1348,7 @@ void RobotModel::getMissingVariableNames(const std::vector& variabl missing_variables.push_back(variable_name); } -int RobotModel::getVariableIndex(const std::string& variable) const +size_t RobotModel::getVariableIndex(const std::string& variable) const { VariableIndexMap::const_iterator it = joint_variables_index_map_.find(variable); if (it == joint_variables_index_map_.end()) diff --git a/moveit_core/robot_model/test/test.cpp b/moveit_core/robot_model/test/test.cpp index ed3553ba9a..8e2cd31fc5 100644 --- a/moveit_core/robot_model/test/test.cpp +++ b/moveit_core/robot_model/test/test.cpp @@ -71,13 +71,13 @@ TEST_F(LoadPlanningModelsPr2, Model) const std::vector& joints = robot_model_->getJointModels(); for (std::size_t i = 0; i < joints.size(); ++i) { - ASSERT_EQ(joints[i]->getJointIndex(), static_cast(i)); + ASSERT_EQ(joints[i]->getJointIndex(), i); ASSERT_EQ(robot_model_->getJointModel(joints[i]->getName()), joints[i]); } const std::vector& links = robot_model_->getLinkModels(); for (std::size_t i = 0; i < links.size(); ++i) { - ASSERT_EQ(links[i]->getLinkIndex(), static_cast(i)); + ASSERT_EQ(links[i]->getLinkIndex(), i); } moveit::tools::Profiler::Status(); From 86a24a494264045883119c41a02188735f025f40 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Thu, 13 Jan 2022 07:17:21 -0700 Subject: [PATCH 166/229] Fix missing ament_cmake_gtest dependency (#981) --- moveit_planners/pilz_industrial_motion_planner/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_planners/pilz_industrial_motion_planner/package.xml b/moveit_planners/pilz_industrial_motion_planner/package.xml index c2f2f2dbaa..1ac13921f7 100644 --- a/moveit_planners/pilz_industrial_motion_planner/package.xml +++ b/moveit_planners/pilz_industrial_motion_planner/package.xml @@ -51,6 +51,7 @@ ament_lint_auto ament_lint_common + ament_cmake_gtest From 7da00c19bf99f9f6cb28a588dba3fd86d9dd669a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 14 Jan 2022 09:08:16 +0100 Subject: [PATCH 167/229] Fix CI: update apt packages before install --- moveit_kinematics/test/test_ikfast_plugins.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_kinematics/test/test_ikfast_plugins.sh b/moveit_kinematics/test/test_ikfast_plugins.sh index 17f3102989..ac8806d854 100755 --- a/moveit_kinematics/test/test_ikfast_plugins.sh +++ b/moveit_kinematics/test/test_ikfast_plugins.sh @@ -9,6 +9,7 @@ set -e # fail script on error sudo update-alternatives --install /usr/bin/python python /usr/bin/python2 1 sudo update-alternatives --install /usr/bin/python python /usr/bin/python3 2 +sudo apt-get -q update if [ "$ROS_DISTRO" == "noetic" ]; then sudo update-alternatives --set python /usr/bin/python3 From d01ef481e2a65323842496f3732b0fa3bd03548b Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Sat, 15 Jan 2022 15:29:45 +0100 Subject: [PATCH 168/229] Fix IKFast test dependency (#993) --- moveit_kinematics/test/test_ikfast_plugins.sh | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/moveit_kinematics/test/test_ikfast_plugins.sh b/moveit_kinematics/test/test_ikfast_plugins.sh index 410824f5c6..cdda01aa2e 100755 --- a/moveit_kinematics/test/test_ikfast_plugins.sh +++ b/moveit_kinematics/test/test_ikfast_plugins.sh @@ -7,6 +7,7 @@ set -e # fail script on error +sudo apt-get -qq update sudo apt-get -qq install python3-lxml python3-yaml # Clone moveit_resources for URDFs. They are not available before running docker. @@ -14,10 +15,6 @@ git clone -q --depth=1 -b ros2 https://github.com/ros-planning/moveit_resources fanuc=/tmp/resources/fanuc_description/urdf/fanuc.urdf panda=/tmp/resources/panda_description/urdf/panda.urdf -# Install lxml required for create_ikfast_moveit_plugin.py -sudo apt-get -qq update -sudo apt-get -qq install -y python-lxml - export QUIET=${QUIET:=1} # Create ikfast plugins for Fanuc and Panda From 024e663b97159b7866be98dc61a89c8f7a6e2ea5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 17 Jan 2022 09:52:52 +0100 Subject: [PATCH 169/229] CI: Fetch srdfdom from source --- .github/workflows/upstream.rosinstall | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/upstream.rosinstall b/.github/workflows/upstream.rosinstall index 4582a6f25b..504fea52f7 100644 --- a/.github/workflows/upstream.rosinstall +++ b/.github/workflows/upstream.rosinstall @@ -6,3 +6,7 @@ local-name: geometric_shapes uri: https://github.com/ros-planning/geometric_shapes.git version: noetic-devel +- git: + local-name: srdfdom + uri: https://github.com/ubi-agni/srdfdom + version: noetic-devel From 877d0bd81e0c3ca977ea42526df497b91dcb75e2 Mon Sep 17 00:00:00 2001 From: Cory Crean Date: Mon, 17 Jan 2022 08:50:05 -0700 Subject: [PATCH 170/229] Remove 'using namespace' from header files. (#994) --- .../trajectory_generator.h | 2 - .../trajectory_generator_circ.h | 2 - .../trajectory_generator_lin.h | 2 - .../trajectory_generator_ptp.h | 2 - .../src/joint_limits_aggregator.cpp | 2 +- .../src/joint_limits_container.cpp | 7 +- .../src/joint_limits_validator.cpp | 2 +- .../src/trajectory_generator_ptp.cpp | 2 +- .../test/test_utils.cpp | 2 +- .../src/unittest_joint_limits_container.cpp | 30 ++++---- .../src/unittest_joint_limits_validator.cpp | 68 +++++++++---------- .../src/unittest_trajectory_functions.cpp | 6 +- .../src/unittest_trajectory_generator_ptp.cpp | 12 ++-- .../test/servo_launch_test_common.hpp | 14 ++-- .../test/test_servo_interface.cpp | 2 + 15 files changed, 74 insertions(+), 81 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h index 6d88170f4a..d0d680eb82 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h @@ -49,8 +49,6 @@ #include "pilz_industrial_motion_planner/trajectory_functions.h" #include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h" -using namespace pilz_industrial_motion_planner; - namespace pilz_industrial_motion_planner { CREATE_MOVEIT_ERROR_CODE_EXCEPTION(TrajectoryGeneratorInvalidLimitsException, diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h index 2940be8bb4..769e1a35b3 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h @@ -41,8 +41,6 @@ #include "pilz_industrial_motion_planner/trajectory_generator.h" -using namespace pilz_industrial_motion_planner; - namespace pilz_industrial_motion_planner { CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircleNoPlane, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h index 3501ca7d3c..2b49ee87be 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h @@ -41,8 +41,6 @@ #include "pilz_industrial_motion_planner/trajectory_generator.h" #include "pilz_industrial_motion_planner/velocity_profile_atrap.h" -using namespace pilz_industrial_motion_planner; - namespace pilz_industrial_motion_planner { // TODO date type of units diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h index 6ad5c9b51f..53736e7ae4 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h @@ -41,8 +41,6 @@ #include "pilz_industrial_motion_planner/trajectory_generator.h" #include "pilz_industrial_motion_planner/velocity_profile_atrap.h" -using namespace pilz_industrial_motion_planner; - namespace pilz_industrial_motion_planner { // TODO date type of units diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp index cf65718581..8f85f6b753 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp @@ -57,7 +57,7 @@ pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( // Iterate over all joint models and generate the map for (auto joint_model : joint_models) { - JointLimit joint_limit; + pilz_industrial_motion_planner::JointLimit joint_limit; // NOTE: declareParameters fails (=returns false) if the parameters have already been declared. // The function should be checked in the if condition below when we disable diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp index 4cd5a354aa..9151b6c872 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp @@ -40,7 +40,8 @@ namespace pilz_industrial_motion_planner { static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.joint_limits_container"); -bool JointLimitsContainer::addLimit(const std::string& joint_name, JointLimit joint_limit) +bool JointLimitsContainer::addLimit(const std::string& joint_name, + pilz_industrial_motion_planner::JointLimit joint_limit) { if (joint_limit.has_deceleration_limits && joint_limit.max_deceleration >= 0) { @@ -73,7 +74,7 @@ bool JointLimitsContainer::empty() const JointLimit JointLimitsContainer::getCommonLimit() const { - JointLimit common_limit; + pilz_industrial_motion_planner::JointLimit common_limit; for (const auto& limit : container_) { updateCommonLimit(limit.second, common_limit); @@ -83,7 +84,7 @@ JointLimit JointLimitsContainer::getCommonLimit() const JointLimit JointLimitsContainer::getCommonLimit(const std::vector& joint_names) const { - JointLimit common_limit; + pilz_industrial_motion_planner::JointLimit common_limit; for (const auto& joint_name : joint_names) { updateCommonLimit(container_.at(joint_name), common_limit); diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_validator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_validator.cpp index 66c5b198a9..6f360245a7 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_validator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_validator.cpp @@ -74,7 +74,7 @@ bool pilz_industrial_motion_planner::JointLimitsValidator::validateWithEqualFunc return true; } - JointLimit reference = joint_limits.begin()->second; + pilz_industrial_motion_planner::JointLimit reference = joint_limits.begin()->second; for (auto it = std::next(joint_limits.begin()); it != joint_limits.end(); ++it) { 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 6539f1d020..dd90feadc4 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 @@ -76,7 +76,7 @@ TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const moveit::core::RobotModelCon continue; } - JointLimit most_strict_limit = joint_limits_.getCommonLimit(active_joints); + pilz_industrial_motion_planner::JointLimit most_strict_limit = joint_limits_.getCommonLimit(active_joints); if (!most_strict_limit.has_velocity_limits) { 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 ba31c7627f..4ba978b7b2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -59,7 +59,7 @@ testutils::createFakeLimits(const std::vector& joint_names) for (const std::string& name : joint_names) { - JointLimit limit; + pilz_industrial_motion_planner::JointLimit limit; limit.has_position_limits = true; limit.max_position = 2.967; limit.min_position = -2.967; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_container.cpp index 5a1d064946..3863973613 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_container.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_container.cpp @@ -44,25 +44,25 @@ class JointLimitsContainerTest : public ::testing::Test protected: void SetUp() override { - JointLimit lim1; + pilz_industrial_motion_planner::JointLimit lim1; lim1.has_position_limits = true; lim1.min_position = -2; lim1.max_position = 2; lim1.has_acceleration_limits = true; lim1.max_acceleration = 3; //<- Expected for common_limit_.max_acceleration - JointLimit lim2; + pilz_industrial_motion_planner::JointLimit lim2; lim2.has_position_limits = true; lim2.min_position = -1; //<- Expected for common_limit_.min_position lim2.max_position = 1; //<- Expected for common_limit_.max_position lim2.has_deceleration_limits = true; lim2.max_deceleration = -5; //<- Expected for common_limit_.max_deceleration - JointLimit lim3; + pilz_industrial_motion_planner::JointLimit lim3; lim3.has_velocity_limits = true; lim3.max_velocity = 10; - JointLimit lim4; + pilz_industrial_motion_planner::JointLimit lim4; lim4.has_position_limits = true; lim4.min_position = -1; lim4.max_position = 1; @@ -71,14 +71,14 @@ class JointLimitsContainerTest : public ::testing::Test lim4.has_deceleration_limits = false; lim4.max_deceleration = -1; - JointLimit lim5; + pilz_industrial_motion_planner::JointLimit lim5; lim5.has_position_limits = true; lim5.min_position = -1; lim5.max_position = 1; lim5.has_acceleration_limits = false; lim5.max_acceleration = 1; - JointLimit lim6; + pilz_industrial_motion_planner::JointLimit lim6; lim6.has_velocity_limits = true; lim6.max_velocity = 2; //<- Expected for common_limit_.max_velocity lim6.has_deceleration_limits = true; @@ -95,7 +95,7 @@ class JointLimitsContainerTest : public ::testing::Test } pilz_industrial_motion_planner::JointLimitsContainer container_; - JointLimit common_limit_; + pilz_industrial_motion_planner::JointLimit common_limit_; }; /** @@ -136,15 +136,15 @@ TEST_F(JointLimitsContainerTest, CheckDecelerationUnification) */ TEST_F(JointLimitsContainerTest, CheckAddLimitDeceleration) { - JointLimit lim_invalid1; + pilz_industrial_motion_planner::JointLimit lim_invalid1; lim_invalid1.has_deceleration_limits = true; lim_invalid1.max_deceleration = 0; - JointLimit lim_invalid2; + pilz_industrial_motion_planner::JointLimit lim_invalid2; lim_invalid2.has_deceleration_limits = true; lim_invalid2.max_deceleration = 1; - JointLimit lim_valid; + pilz_industrial_motion_planner::JointLimit lim_valid; lim_valid.has_deceleration_limits = true; lim_valid.max_deceleration = -1; @@ -160,7 +160,7 @@ TEST_F(JointLimitsContainerTest, CheckAddLimitDeceleration) */ TEST_F(JointLimitsContainerTest, CheckAddLimitAlreadyContained) { - JointLimit lim_valid; + pilz_industrial_motion_planner::JointLimit lim_valid; lim_valid.has_deceleration_limits = true; lim_valid.max_deceleration = -1; @@ -175,7 +175,7 @@ TEST_F(JointLimitsContainerTest, CheckAddLimitAlreadyContained) TEST_F(JointLimitsContainerTest, CheckEmptyContainer) { pilz_industrial_motion_planner::JointLimitsContainer container; - JointLimit limits = container.getCommonLimit(); + pilz_industrial_motion_planner::JointLimit limits = container.getCommonLimit(); EXPECT_FALSE(limits.has_position_limits); EXPECT_FALSE(limits.has_velocity_limits); EXPECT_FALSE(limits.has_acceleration_limits); @@ -186,9 +186,9 @@ TEST_F(JointLimitsContainerTest, CheckEmptyContainer) */ TEST_F(JointLimitsContainerTest, FirstPositionEmpty) { - JointLimit lim1; + pilz_industrial_motion_planner::JointLimit lim1; - JointLimit lim2; + pilz_industrial_motion_planner::JointLimit lim2; lim2.has_position_limits = true; lim2.min_position = -1; //<- Expected for common_limit_.min_position lim2.max_position = 1; //<- Expected for common_limit_.max_position @@ -197,7 +197,7 @@ TEST_F(JointLimitsContainerTest, FirstPositionEmpty) container.addLimit("joint1", lim1); container.addLimit("joint2", lim2); - JointLimit limits = container.getCommonLimit(); + pilz_industrial_motion_planner::JointLimit limits = container.getCommonLimit(); EXPECT_TRUE(limits.has_position_limits); EXPECT_EQ(1, limits.max_position); EXPECT_EQ(-1, limits.min_position); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_validator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_validator.cpp index a959d19d94..680a4a6420 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_validator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_validator.cpp @@ -51,7 +51,7 @@ TEST_F(JointLimitsValidatorTest, CheckPositionEquality) { JointLimitsContainer container; - JointLimit lim1; + pilz_industrial_motion_planner::JointLimit lim1; lim1.has_position_limits = true; lim1.min_position = -1; lim1.max_position = 1; @@ -72,12 +72,12 @@ TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMinPosition) { JointLimitsContainer container; - JointLimit lim1; + pilz_industrial_motion_planner::JointLimit lim1; lim1.has_position_limits = true; lim1.min_position = -1; lim1.max_position = 1; - JointLimit lim2; + pilz_industrial_motion_planner::JointLimit lim2; lim2.has_position_limits = true; lim2.min_position = -2; lim2.max_position = 1; @@ -98,12 +98,12 @@ TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxPosition1) { JointLimitsContainer container; - JointLimit lim1; + pilz_industrial_motion_planner::JointLimit lim1; lim1.has_position_limits = true; lim1.min_position = -1; lim1.max_position = 2; - JointLimit lim2; + pilz_industrial_motion_planner::JointLimit lim2; lim2.has_position_limits = true; lim2.min_position = -1; lim2.max_position = 1; @@ -124,17 +124,17 @@ TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxPosition2) { JointLimitsContainer container; - JointLimit lim1; + pilz_industrial_motion_planner::JointLimit lim1; lim1.has_position_limits = true; lim1.min_position = -1; lim1.max_position = 2; - JointLimit lim2; + pilz_industrial_motion_planner::JointLimit lim2; lim2.has_position_limits = true; lim2.min_position = -1; lim2.max_position = 1; - JointLimit lim3; + pilz_industrial_motion_planner::JointLimit lim3; lim3.has_position_limits = true; lim3.min_position = -1; lim3.max_position = 1; @@ -156,12 +156,12 @@ TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityHasPositionLimits) { JointLimitsContainer container; - JointLimit lim1; + pilz_industrial_motion_planner::JointLimit lim1; lim1.has_position_limits = true; lim1.min_position = -1; lim1.max_position = 1; - JointLimit lim2; + pilz_industrial_motion_planner::JointLimit lim2; lim2.has_position_limits = false; lim2.min_position = -1; lim2.max_position = 1; @@ -188,7 +188,7 @@ TEST_F(JointLimitsValidatorTest, CheckVelocityEquality) { JointLimitsContainer container; - JointLimit lim1; + pilz_industrial_motion_planner::JointLimit lim1; lim1.has_velocity_limits = true; lim1.max_velocity = 1; @@ -208,11 +208,11 @@ TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxVelocity1) { JointLimitsContainer container; - JointLimit lim1; + pilz_industrial_motion_planner::JointLimit lim1; lim1.has_velocity_limits = true; lim1.max_velocity = 1; - JointLimit lim2; + pilz_industrial_motion_planner::JointLimit lim2; lim2.has_velocity_limits = true; lim2.max_velocity = 2; @@ -232,15 +232,15 @@ TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxVelocity2) { JointLimitsContainer container; - JointLimit lim1; + pilz_industrial_motion_planner::JointLimit lim1; lim1.has_velocity_limits = true; lim1.max_velocity = 1; - JointLimit lim2; + pilz_industrial_motion_planner::JointLimit lim2; lim2.has_velocity_limits = true; lim2.max_velocity = 2; - JointLimit lim3; + pilz_industrial_motion_planner::JointLimit lim3; lim3.has_velocity_limits = true; lim3.max_velocity = 2; @@ -261,10 +261,10 @@ TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityHasVelocityLimits) { JointLimitsContainer container; - JointLimit lim1; + pilz_industrial_motion_planner::JointLimit lim1; lim1.has_velocity_limits = true; - JointLimit lim2; + pilz_industrial_motion_planner::JointLimit lim2; lim2.has_velocity_limits = false; container.addLimit("joint1", lim1); @@ -289,7 +289,7 @@ TEST_F(JointLimitsValidatorTest, CheckAccelerationEquality) { JointLimitsContainer container; - JointLimit lim1; + pilz_industrial_motion_planner::JointLimit lim1; lim1.has_acceleration_limits = true; lim1.max_acceleration = 1; @@ -309,11 +309,11 @@ TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxAcceleration1) { JointLimitsContainer container; - JointLimit lim1; + pilz_industrial_motion_planner::JointLimit lim1; lim1.has_acceleration_limits = true; lim1.max_acceleration = 1; - JointLimit lim2; + pilz_industrial_motion_planner::JointLimit lim2; lim2.has_acceleration_limits = true; lim2.max_acceleration = 2; @@ -333,15 +333,15 @@ TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxAcceleration2) { JointLimitsContainer container; - JointLimit lim1; + pilz_industrial_motion_planner::JointLimit lim1; lim1.has_acceleration_limits = true; lim1.max_acceleration = 1; - JointLimit lim2; + pilz_industrial_motion_planner::JointLimit lim2; lim2.has_acceleration_limits = true; lim2.max_acceleration = 2; - JointLimit lim3; + pilz_industrial_motion_planner::JointLimit lim3; lim3.has_acceleration_limits = true; lim3.max_acceleration = 2; @@ -363,10 +363,10 @@ TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityHasAccelerationLimits) { JointLimitsContainer container; - JointLimit lim1; + pilz_industrial_motion_planner::JointLimit lim1; lim1.has_acceleration_limits = true; - JointLimit lim2; + pilz_industrial_motion_planner::JointLimit lim2; lim2.has_acceleration_limits = false; container.addLimit("joint1", lim1); @@ -391,7 +391,7 @@ TEST_F(JointLimitsValidatorTest, CheckDecelerationEquality) { JointLimitsContainer container; - JointLimit lim1; + pilz_industrial_motion_planner::JointLimit lim1; lim1.has_deceleration_limits = true; lim1.max_deceleration = 1; @@ -411,11 +411,11 @@ TEST_F(JointLimitsValidatorTest, CheckInEqualityMaxDeceleration1) { JointLimitsContainer container; - JointLimit lim1; + pilz_industrial_motion_planner::JointLimit lim1; lim1.has_deceleration_limits = true; lim1.max_deceleration = -1; - JointLimit lim2; + pilz_industrial_motion_planner::JointLimit lim2; lim2.has_deceleration_limits = true; lim2.max_deceleration = -2; @@ -435,15 +435,15 @@ TEST_F(JointLimitsValidatorTest, CheckInEqualityMaxDeceleration2) { JointLimitsContainer container; - JointLimit lim1; + pilz_industrial_motion_planner::JointLimit lim1; lim1.has_deceleration_limits = true; lim1.max_deceleration = -1; - JointLimit lim2; + pilz_industrial_motion_planner::JointLimit lim2; lim2.has_deceleration_limits = true; lim2.max_deceleration = -2; - JointLimit lim3; + pilz_industrial_motion_planner::JointLimit lim3; lim3.has_deceleration_limits = true; lim3.max_deceleration = -2; @@ -465,11 +465,11 @@ TEST_F(JointLimitsValidatorTest, CheckInEqualityHasDecelerationLimits) { JointLimitsContainer container; - JointLimit lim1; + pilz_industrial_motion_planner::JointLimit lim1; lim1.has_deceleration_limits = true; lim1.max_deceleration = -1; - JointLimit lim2; + pilz_industrial_motion_planner::JointLimit lim2; lim2.has_deceleration_limits = false; container.addLimit("joint1", lim1); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp index 00f55f3637..f509a1dd47 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp @@ -576,7 +576,7 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsVeloc double duration_last{ 0.0 }; pilz_industrial_motion_planner::JointLimitsContainer joint_limits; - JointLimit test_joint_limits; + pilz_industrial_motion_planner::JointLimit test_joint_limits; // Calculate the max allowed velocity in such a way that it is always smaller // than the current velocity. test_joint_limits.max_velocity = @@ -612,7 +612,7 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsAccel std::map velocity_last{ { test_joint_name, 9.0 } }; pilz_industrial_motion_planner::JointLimitsContainer joint_limits; - JointLimit test_joint_limits; + pilz_industrial_motion_planner::JointLimit test_joint_limits; // Calculate the max allowed velocity in such a way that it is always bigger // than the current velocity. test_joint_limits.max_velocity = velocity_current + 1.0; @@ -655,7 +655,7 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsDecel std::map velocity_last{ { test_joint_name, 19.0 } }; pilz_industrial_motion_planner::JointLimitsContainer joint_limits; - JointLimit test_joint_limits; + pilz_industrial_motion_planner::JointLimit test_joint_limits; // Calculate the max allowed velocity in such a way that it is always bigger // than the current velocity. test_joint_limits.max_velocity = fabs(velocity_current) + 1.0; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp index b817452ea2..3478abddce 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp @@ -97,7 +97,7 @@ class TrajectoryGeneratorPTPTest : public testing::Test for (const auto& jmg : robot_model_->getJointModelGroups()) { std::vector joint_names = jmg->getActiveJointModelNames(); - JointLimit joint_limit; + pilz_industrial_motion_planner::JointLimit joint_limit; joint_limit.max_position = 3.124; joint_limit.min_position = -3.124; joint_limit.has_velocity_limits = true; @@ -214,7 +214,7 @@ TEST_F(TrajectoryGeneratorPTPTest, missingVelocityLimits) JointLimitsContainer joint_limits; auto joint_models = robot_model_->getActiveJointModels(); - JointLimit joint_limit; + pilz_industrial_motion_planner::JointLimit joint_limit; joint_limit.has_velocity_limits = false; joint_limit.has_acceleration_limits = true; joint_limit.max_deceleration = -1; @@ -238,7 +238,7 @@ TEST_F(TrajectoryGeneratorPTPTest, missingDecelerationimits) JointLimitsContainer joint_limits; const auto& joint_models = robot_model_->getActiveJointModels(); - JointLimit joint_limit; + pilz_industrial_motion_planner::JointLimit joint_limit; joint_limit.has_velocity_limits = true; joint_limit.has_acceleration_limits = true; joint_limit.has_deceleration_limits = false; @@ -272,7 +272,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testInsufficientLimit) ASSERT_TRUE(joint_models.size()); // joint limit with insufficient limits (no acc/dec limits) - JointLimit insufficient_limit; + pilz_industrial_motion_planner::JointLimit insufficient_limit; insufficient_limit.has_position_limits = true; insufficient_limit.max_position = 2.5; insufficient_limit.min_position = -2.5; @@ -300,7 +300,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testInsufficientLimit) /* Step 2 */ /**********/ // joint limit with sufficient limits - JointLimit sufficient_limit; + pilz_industrial_motion_planner::JointLimit sufficient_limit; sufficient_limit.has_position_limits = true; sufficient_limit.max_position = 2.356; sufficient_limit.min_position = -2.356; @@ -486,7 +486,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testJointGoalAlreadyReached) TEST_F(TrajectoryGeneratorPTPTest, testScalingFactor) { // create ptp generator with different limits - JointLimit joint_limit; + pilz_industrial_motion_planner::JointLimit joint_limit; JointLimitsContainer joint_limits; // set the joint limits diff --git a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp b/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp index da82f89981..3680e01d00 100644 --- a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp +++ b/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp @@ -63,8 +63,6 @@ #pragma once -using namespace std::chrono_literals; - static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_launch_test_common.hpp"); namespace moveit_servo @@ -143,7 +141,7 @@ class ServoFixture : public ::testing::Test return false; } RCLCPP_INFO(LOGGER, "client_servo_start_ service not available, waiting again..."); - rclcpp::sleep_for(500ms); + rclcpp::sleep_for(std::chrono::milliseconds(500)); } // If we setup the start client, also setup the stop client... @@ -156,7 +154,7 @@ class ServoFixture : public ::testing::Test return false; } RCLCPP_INFO(LOGGER, "client_servo_stop_ service not available, waiting again..."); - rclcpp::sleep_for(500ms); + rclcpp::sleep_for(std::chrono::milliseconds(500)); } // Status sub (we need this to check that we've started / stopped) @@ -177,7 +175,7 @@ class ServoFixture : public ::testing::Test return false; } RCLCPP_INFO(LOGGER, "client_servo_pause_ service not available, waiting again..."); - rclcpp::sleep_for(500ms); + rclcpp::sleep_for(std::chrono::milliseconds(500)); } return true; } @@ -193,7 +191,7 @@ class ServoFixture : public ::testing::Test return false; } RCLCPP_INFO(LOGGER, "client_servo_unpause_ service not available, waiting again..."); - rclcpp::sleep_for(500ms); + rclcpp::sleep_for(std::chrono::milliseconds(500)); } return true; } @@ -210,7 +208,7 @@ class ServoFixture : public ::testing::Test return false; } RCLCPP_INFO(LOGGER, "client_change_control_dims_ service not available, waiting again..."); - rclcpp::sleep_for(500ms); + rclcpp::sleep_for(std::chrono::milliseconds(500)); } return true; } @@ -227,7 +225,7 @@ class ServoFixture : public ::testing::Test return false; } RCLCPP_INFO(LOGGER, "client_change_drift_dims_ service not available, waiting again..."); - rclcpp::sleep_for(500ms); + rclcpp::sleep_for(std::chrono::milliseconds(500)); } return true; } diff --git a/moveit_ros/moveit_servo/test/test_servo_interface.cpp b/moveit_ros/moveit_servo/test/test_servo_interface.cpp index 6a7a18f856..a1e2e49940 100644 --- a/moveit_ros/moveit_servo/test/test_servo_interface.cpp +++ b/moveit_ros/moveit_servo/test/test_servo_interface.cpp @@ -38,6 +38,8 @@ #include "servo_launch_test_common.hpp" +using namespace std::chrono_literals; + namespace moveit_servo { TEST_F(ServoFixture, SendTwistStampedTest) From 4804ffa6b40a6354e0f135e0295dbc72272aa34c Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 17 Jan 2022 09:38:18 -0700 Subject: [PATCH 171/229] Delete backtrace hack (#995) --- moveit_core/CMakeLists.txt | 2 - moveit_core/backtrace/CMakeLists.txt | 1 - .../include/moveit/backtrace/backtrace.h | 64 ------------------- moveit_core/robot_state/src/robot_state.cpp | 1 - 4 files changed, 68 deletions(-) delete mode 100644 moveit_core/backtrace/CMakeLists.txt delete mode 100644 moveit_core/backtrace/include/moveit/backtrace/backtrace.h diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index ceb8b52865..71fc6514a0 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -58,7 +58,6 @@ set(VERSION_FILE_PATH ${CMAKE_BINARY_DIR}/include) set(THIS_PACKAGE_INCLUDE_DIRS background_processing/include exceptions/include - backtrace/include collision_detection/include collision_detection_fcl/include collision_detection_bullet/include @@ -159,7 +158,6 @@ configure_file("version/version.h.in" "${VERSION_FILE_PATH}/moveit/version.h") install(FILES "${VERSION_FILE_PATH}/moveit/version.h" DESTINATION include/moveit) add_subdirectory(background_processing) -add_subdirectory(backtrace) add_subdirectory(collision_distance_field) add_subdirectory(constraint_samplers) add_subdirectory(controller_manager) diff --git a/moveit_core/backtrace/CMakeLists.txt b/moveit_core/backtrace/CMakeLists.txt deleted file mode 100644 index 3aa21699d5..0000000000 --- a/moveit_core/backtrace/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -install(DIRECTORY include/ DESTINATION include) diff --git a/moveit_core/backtrace/include/moveit/backtrace/backtrace.h b/moveit_core/backtrace/include/moveit/backtrace/backtrace.h deleted file mode 100644 index 50869a52fc..0000000000 --- a/moveit_core/backtrace/include/moveit/backtrace/backtrace.h +++ /dev/null @@ -1,64 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, Willow Garage, Inc. - * 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 Willow Garage 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. - *********************************************************************/ - -#pragma once - -#include - -namespace moveit -{ -/** \brief Get the current backtrace. This may not work on all compilers */ -void get_backtrace(std::ostream& out); - -#ifdef __GLIBC__ -#include -void get_backtrace(std::ostream& out) -{ - void* array[500]; - size_t size = backtrace(array, 500); - char** strings = backtrace_symbols((void* const*)array, size); - out << "Backtrace: \n"; - for (size_t i = 0; i < size; ++i) - { - out << " " << strings[i] << '\n'; - } - free(strings); -} -#else -void get_backtrace(std::ostream& out) -{ - out << "Unable to get backtrace with the used compiler.\n"; -} -#endif -} // namespace moveit diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 7f4f6170c0..44933ac49a 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -45,7 +45,6 @@ #else #include #endif -#include #include #include #include From 2efe1b2599c813de11b7984b1e8b9e40f0ce76f0 Mon Sep 17 00:00:00 2001 From: Akash Date: Mon, 17 Jan 2022 13:25:31 -0800 Subject: [PATCH 172/229] Fix boost linking errors for Windows (#957) --- moveit_core/CMakeLists.txt | 6 +++--- moveit_core/distance_field/CMakeLists.txt | 1 - moveit_core/utils/CMakeLists.txt | 2 +- moveit_ros/occupancy_map_monitor/CMakeLists.txt | 7 +++---- moveit_ros/perception/CMakeLists.txt | 6 +++--- moveit_ros/planning_interface/CMakeLists.txt | 1 - moveit_ros/planning_interface/ConfigExtras.cmake | 1 - 7 files changed, 10 insertions(+), 14 deletions(-) diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 71fc6514a0..28a6eff9e5 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -10,9 +10,6 @@ find_package(rclcpp REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) -# Finds Boost Components -include(ConfigExtras.cmake) - find_package(PkgConfig REQUIRED) pkg_check_modules(LIBFCL REQUIRED "fcl>=0.5.0") # replace LIBFCL_LIBRARIES with full paths to the libraries @@ -52,6 +49,9 @@ find_package(pluginlib REQUIRED) # TODO: Port python bindings # find_package(pybind11 REQUIRED) +# Finds Boost Components +include(ConfigExtras.cmake) + # Set target file path for version.h set(VERSION_FILE_PATH ${CMAKE_BINARY_DIR}/include) diff --git a/moveit_core/distance_field/CMakeLists.txt b/moveit_core/distance_field/CMakeLists.txt index f25acaa4da..0f4e5dd710 100644 --- a/moveit_core/distance_field/CMakeLists.txt +++ b/moveit_core/distance_field/CMakeLists.txt @@ -16,7 +16,6 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} tf2_eigen OCTOMAP ) -target_link_libraries(${MOVEIT_LIB_NAME} Boost::iostreams) install(DIRECTORY include/ DESTINATION include) diff --git a/moveit_core/utils/CMakeLists.txt b/moveit_core/utils/CMakeLists.txt index 57bf319f3d..f94540da35 100644 --- a/moveit_core/utils/CMakeLists.txt +++ b/moveit_core/utils/CMakeLists.txt @@ -14,7 +14,7 @@ install(DIRECTORY include/ DESTINATION include) find_package(ament_index_cpp REQUIRED) set(MOVEIT_TEST_LIB_NAME moveit_test_utils) add_library(${MOVEIT_TEST_LIB_NAME} SHARED src/robot_model_test_utils.cpp) -target_link_libraries(${MOVEIT_TEST_LIB_NAME} moveit_robot_model Boost::regex) +target_link_libraries(${MOVEIT_TEST_LIB_NAME} moveit_robot_model) ament_target_dependencies(${MOVEIT_TEST_LIB_NAME} ament_index_cpp Boost diff --git a/moveit_ros/occupancy_map_monitor/CMakeLists.txt b/moveit_ros/occupancy_map_monitor/CMakeLists.txt index f6a417c91e..fb8c0caed4 100644 --- a/moveit_ros/occupancy_map_monitor/CMakeLists.txt +++ b/moveit_ros/occupancy_map_monitor/CMakeLists.txt @@ -12,9 +12,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wno-pedantic) endif() -# Finds Boost Components -include(ConfigExtras.cmake) - if(APPLE) find_package(X11 REQUIRED) endif() @@ -27,6 +24,9 @@ find_package(octomap REQUIRED) find_package(geometric_shapes REQUIRED) find_package(tf2_ros REQUIRED) +# Finds Boost Components +include(ConfigExtras.cmake) + include_directories(include) include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS} @@ -52,7 +52,6 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V ament_target_dependencies(${MOVEIT_LIB_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS} ) -target_link_libraries(${MOVEIT_LIB_NAME} Boost::thread) add_executable(moveit_ros_occupancy_map_server src/occupancy_map_server.cpp) ament_target_dependencies(moveit_ros_occupancy_map_server diff --git a/moveit_ros/perception/CMakeLists.txt b/moveit_ros/perception/CMakeLists.txt index f2fef34935..f7c00d9b45 100644 --- a/moveit_ros/perception/CMakeLists.txt +++ b/moveit_ros/perception/CMakeLists.txt @@ -11,9 +11,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wpedantic) endif() -# Finds Boost Components -include(ConfigExtras.cmake) - if(WITH_OPENGL) # Prefer newer vendor-specific OpenGL library if(POLICY CMP0072) @@ -62,6 +59,9 @@ find_package(Eigen3 REQUIRED) find_package(OpenMP REQUIRED) find_package(OpenCV) +# Finds Boost Components +include(ConfigExtras.cmake) + set(THIS_PACKAGE_INCLUDE_DIRS depth_image_octomap_updater/include lazy_free_space_updater/include diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index 4bb31d68a8..1efc97912b 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -79,7 +79,6 @@ add_subdirectory(move_group_interface) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ros_testing REQUIRED) - find_package(Boost REQUIRED COMPONENTS filesystem) # TODO (vatanaksoytezer): Enable once this test is not flaky # Move group interface cpp ompl constrained planning integration test diff --git a/moveit_ros/planning_interface/ConfigExtras.cmake b/moveit_ros/planning_interface/ConfigExtras.cmake index f23809f4f9..c1b88a79a6 100644 --- a/moveit_ros/planning_interface/ConfigExtras.cmake +++ b/moveit_ros/planning_interface/ConfigExtras.cmake @@ -1,6 +1,5 @@ # Extras module needed for dependencies to find boost components -find_package(Boost REQUIRED) #if(Boost_VERSION LESS 106700) # set(BOOST_PYTHON_COMPONENT python) #else() From 37699f80c520d8e41c7688d9860c3a0ff1f752fc Mon Sep 17 00:00:00 2001 From: livanov93 Date: Mon, 17 Jan 2022 18:27:08 -0700 Subject: [PATCH 173/229] Doxygen support (#992) Co-authored-by: Jafar Abdi Co-authored-by: Tyler Weaver --- Doxyfile | 2427 +++++++++++++++++++++++++++++++++++++++++++++++++++++ README.md | 3 + 2 files changed, 2430 insertions(+) create mode 100644 Doxyfile diff --git a/Doxyfile b/Doxyfile new file mode 100644 index 0000000000..21de608249 --- /dev/null +++ b/Doxyfile @@ -0,0 +1,2427 @@ +# Doxyfile 1.8.11 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project. +# +# All text after a double hash (##) is considered a comment and is placed in +# front of the TAG it is preceding. +# +# All text after a single hash (#) is considered a comment and will be ignored. +# The format is: +# TAG = value [value, ...] +# For lists, items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (\" \"). + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the config file +# that follow. The default is UTF-8 which is also the encoding used for all text +# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv +# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv +# for the list of possible encodings. +# The default value is: UTF-8. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by +# double-quotes, unless you are using Doxywizard) that should identify the +# project for which the documentation is generated. This name is used in the +# title of most generated pages and in a few other places. +# The default value is: My Project. + +PROJECT_NAME = "moveit2" + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. This +# could be handy for archiving the generated documentation or if some version +# control system is used. + +PROJECT_NUMBER = + +# Using the PROJECT_BRIEF tag one can provide an optional one line description +# for a project that appears at the top of each page and should give viewer a +# quick idea about the purpose of the project. Keep the description short. + +PROJECT_BRIEF = "The MoveIt Motion Planning Framework for ROS 2." + +# With the PROJECT_LOGO tag one can specify a logo or an icon that is included +# in the documentation. The maximum height of the logo should not exceed 55 +# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy +# the logo to the output directory. + +PROJECT_LOGO = + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path +# into which the generated documentation will be written. If a relative path is +# entered, it will be relative to the location where doxygen was started. If +# left blank the current directory will be used. + +OUTPUT_DIRECTORY = docs + +# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub- +# directories (in 2 levels) under the output directory of each output format and +# will distribute the generated files over these directories. Enabling this +# option can be useful when feeding doxygen a huge amount of source files, where +# putting all generated files in the same directory would otherwise causes +# performance problems for the file system. +# The default value is: NO. + +CREATE_SUBDIRS = NO + +# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII +# characters to appear in the names of generated files. If set to NO, non-ASCII +# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode +# U+3044. +# The default value is: NO. + +ALLOW_UNICODE_NAMES = NO + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese, +# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States), +# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian, +# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages), +# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian, +# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian, +# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish, +# Ukrainian and Vietnamese. +# The default value is: English. + +OUTPUT_LANGUAGE = English + +# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member +# descriptions after the members that are listed in the file and class +# documentation (similar to Javadoc). Set to NO to disable this. +# The default value is: YES. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief +# description of a member or function before the detailed description +# +# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. +# The default value is: YES. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator that is +# used to form the text in various listings. Each string in this list, if found +# as the leading text of the brief description, will be stripped from the text +# and the result, after processing the whole list, is used as the annotated +# text. Otherwise, the brief description is used as-is. If left blank, the +# following values are used ($name is automatically replaced with the name of +# the entity):The $name class, The $name widget, The $name file, is, provides, +# specifies, contains, represents, a, an and the. + +ABBREVIATE_BRIEF = + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# doxygen will generate a detailed section even if there is only a brief +# description. +# The default value is: NO. + +ALWAYS_DETAILED_SEC = NO + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. +# The default value is: NO. + +INLINE_INHERITED_MEMB = NO + +# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path +# before files name in the file list and in the header files. If set to NO the +# shortest path that makes the file name unique will be used +# The default value is: YES. + +FULL_PATH_NAMES = YES + +# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path. +# Stripping is only done if one of the specified strings matches the left-hand +# part of the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the path to +# strip. +# +# Note that you can specify absolute paths here, but also relative paths, which +# will be relative from the directory where doxygen is started. +# This tag requires that the tag FULL_PATH_NAMES is set to YES. + +STRIP_FROM_PATH = + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the +# path mentioned in the documentation of a class, which tells the reader which +# header file to include in order to use a class. If left blank only the name of +# the header file containing the class definition is used. Otherwise one should +# specify the list of include paths that are normally passed to the compiler +# using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but +# less readable) file names. This can be useful is your file systems doesn't +# support long names like on DOS, Mac, or CD-ROM. +# The default value is: NO. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the +# first line (until the first dot) of a Javadoc-style comment as the brief +# description. If set to NO, the Javadoc-style will behave just like regular Qt- +# style comments (thus requiring an explicit @brief command for a brief +# description.) +# The default value is: NO. + +JAVADOC_AUTOBRIEF = NO + +# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first +# line (until the first dot) of a Qt-style comment as the brief description. If +# set to NO, the Qt-style will behave just like regular Qt-style comments (thus +# requiring an explicit \brief command for a brief description.) +# The default value is: NO. + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a +# multi-line C++ special comment block (i.e. a block of //! or /// comments) as +# a brief description. This used to be the default behavior. The new default is +# to treat a multi-line C++ comment block as a detailed description. Set this +# tag to YES if you prefer the old behavior instead. +# +# Note that setting this tag to YES also means that rational rose comments are +# not recognized any more. +# The default value is: NO. + +MULTILINE_CPP_IS_BRIEF = NO + +# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the +# documentation from any documented member that it re-implements. +# The default value is: YES. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new +# page for each member. If set to NO, the documentation of a member will be part +# of the file/class/namespace that contains it. +# The default value is: NO. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen +# uses this value to replace tabs by spaces in code fragments. +# Minimum value: 1, maximum value: 16, default value: 4. + +TAB_SIZE = 4 + +# This tag can be used to specify a number of aliases that act as commands in +# the documentation. An alias has the form: +# name=value +# For example adding +# "sideeffect=@par Side Effects:\n" +# will allow you to put the command \sideeffect (or @sideeffect) in the +# documentation, which will result in a user-defined paragraph with heading +# "Side Effects:". You can put \n's in the value part of an alias to insert +# newlines. + +ALIASES = + +# This tag can be used to specify a number of word-keyword mappings (TCL only). +# A mapping has the form "name=value". For example adding "class=itcl::class" +# will allow you to use the command class in the itcl::class meaning. + +TCL_SUBST = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources +# only. Doxygen will then generate output that is more tailored for C. For +# instance, some of the names that are used will be different. The list of all +# members will be omitted, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_FOR_C = NO + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or +# Python sources only. Doxygen will then generate output that is more tailored +# for that language. For instance, namespaces will be presented as packages, +# qualified scopes will look different, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources. Doxygen will then generate output that is tailored for Fortran. +# The default value is: NO. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for VHDL. +# The default value is: NO. + +OPTIMIZE_OUTPUT_VHDL = NO + +# Doxygen selects the parser to use depending on the extension of the files it +# parses. With this tag you can assign which parser to use for a given +# extension. Doxygen has a built-in mapping, but you can override or extend it +# using this tag. The format is ext=language, where ext is a file extension, and +# language is one of the parsers supported by doxygen: IDL, Java, Javascript, +# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran: +# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran: +# Fortran. In the later case the parser tries to guess whether the code is fixed +# or free formatted code, this is the default for Fortran type files), VHDL. For +# instance to make doxygen treat .inc files as Fortran files (default is PHP), +# and .f files as C (default is Fortran), use: inc=Fortran f=C. +# +# Note: For files without extension you can use no_extension as a placeholder. +# +# Note that for custom extensions you also need to set FILE_PATTERNS otherwise +# the files are not read by doxygen. + +EXTENSION_MAPPING = + +# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments +# according to the Markdown format, which allows for more readable +# documentation. See http://daringfireball.net/projects/markdown/ for details. +# The output of markdown processing is further processed by doxygen, so you can +# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in +# case of backward compatibilities issues. +# The default value is: YES. + +MARKDOWN_SUPPORT = YES + +# When enabled doxygen tries to link words that correspond to documented +# classes, or namespaces to their corresponding documentation. Such a link can +# be prevented in individual cases by putting a % sign in front of the word or +# globally by setting AUTOLINK_SUPPORT to NO. +# The default value is: YES. + +AUTOLINK_SUPPORT = YES + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should set this +# tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); +# versus func(std::string) {}). This also make the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. +# The default value is: NO. + +BUILTIN_STL_SUPPORT = NO + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. +# The default value is: NO. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip (see: +# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen +# will parse them like normal C++ but will assume all classes use public instead +# of private inheritance when no explicit protection keyword is present. +# The default value is: NO. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate +# getter and setter methods for a property. Setting this option to YES will make +# doxygen to replace the get and set methods by a property in the documentation. +# This will only work if the methods are indeed getting or setting a simple +# type. If this is not the case, or you want to show the methods anyway, you +# should set this option to NO. +# The default value is: YES. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. +# The default value is: NO. + +DISTRIBUTE_GROUP_DOC = NO + +# If one adds a struct or class to a group and this option is enabled, then also +# any nested class or struct is added to the same group. By default this option +# is disabled and one has to add nested compounds explicitly via \ingroup. +# The default value is: NO. + +GROUP_NESTED_COMPOUNDS = NO + +# Set the SUBGROUPING tag to YES to allow class member groups of the same type +# (for instance a group of public functions) to be put as a subgroup of that +# type (e.g. under the Public Functions section). Set it to NO to prevent +# subgrouping. Alternatively, this can be done per class using the +# \nosubgrouping command. +# The default value is: YES. + +SUBGROUPING = YES + +# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions +# are shown inside the group in which they are included (e.g. using \ingroup) +# instead of on a separate page (for HTML and Man pages) or section (for LaTeX +# and RTF). +# +# Note that this feature does not work in combination with +# SEPARATE_MEMBER_PAGES. +# The default value is: NO. + +INLINE_GROUPED_CLASSES = NO + +# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions +# with only public data fields or simple typedef fields will be shown inline in +# the documentation of the scope in which they are defined (i.e. file, +# namespace, or group documentation), provided this scope is documented. If set +# to NO, structs, classes, and unions are shown on a separate page (for HTML and +# Man pages) or section (for LaTeX and RTF). +# The default value is: NO. + +INLINE_SIMPLE_STRUCTS = NO + +# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or +# enum is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically be +# useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. +# The default value is: NO. + +TYPEDEF_HIDES_STRUCT = NO + +# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This +# cache is used to resolve symbols given their name and scope. Since this can be +# an expensive process and often the same symbol appears multiple times in the +# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small +# doxygen will become slower. If the cache is too large, memory is wasted. The +# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range +# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536 +# symbols. At the end of a run doxygen will report the cache usage and suggest +# the optimal cache size from a speed point of view. +# Minimum value: 0, maximum value: 9, default value: 0. + +LOOKUP_CACHE_SIZE = 0 + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in +# documentation are documented, even if no documentation was available. Private +# class members and static file members will be hidden unless the +# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES. +# Note: This will also disable the warnings about undocumented members that are +# normally produced when WARNINGS is set to YES. +# The default value is: NO. + +EXTRACT_ALL = YES + +# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will +# be included in the documentation. +# The default value is: NO. + +EXTRACT_PRIVATE = NO + +# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal +# scope will be included in the documentation. +# The default value is: NO. + +EXTRACT_PACKAGE = NO + +# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be +# included in the documentation. +# The default value is: NO. + +EXTRACT_STATIC = NO + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined +# locally in source files will be included in the documentation. If set to NO, +# only classes defined in header files are included. Does not have any effect +# for Java sources. +# The default value is: YES. + +EXTRACT_LOCAL_CLASSES = YES + +# This flag is only useful for Objective-C code. If set to YES, local methods, +# which are defined in the implementation section but not in the interface are +# included in the documentation. If set to NO, only methods in the interface are +# included. +# The default value is: NO. + +EXTRACT_LOCAL_METHODS = NO + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base name of +# the file that contains the anonymous namespace. By default anonymous namespace +# are hidden. +# The default value is: NO. + +EXTRACT_ANON_NSPACES = NO + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all +# undocumented members inside documented classes or files. If set to NO these +# members will be included in the various overviews, but no documentation +# section is generated. This option has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. If set +# to NO, these classes will be included in the various overviews. This option +# has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend +# (class|struct|union) declarations. If set to NO, these declarations will be +# included in the documentation. +# The default value is: NO. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any +# documentation blocks found inside the body of a function. If set to NO, these +# blocks will be appended to the function's detailed documentation block. +# The default value is: NO. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation that is typed after a +# \internal command is included. If the tag is set to NO then the documentation +# will be excluded. Set it to YES to include the internal documentation. +# The default value is: NO. + +INTERNAL_DOCS = NO + +# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file +# names in lower-case letters. If set to YES, upper-case letters are also +# allowed. This is useful if you have classes or files whose names only differ +# in case and if your file system supports case sensitive file names. Windows +# and Mac users are advised to set this option to NO. +# The default value is: system dependent. + +CASE_SENSE_NAMES = YES + +# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with +# their full class and namespace scopes in the documentation. If set to YES, the +# scope will be hidden. +# The default value is: NO. + +HIDE_SCOPE_NAMES = NO + +# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will +# append additional text to a page's title, such as Class Reference. If set to +# YES the compound reference will be hidden. +# The default value is: NO. + +HIDE_COMPOUND_REFERENCE= NO + +# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of +# the files that are included by a file in the documentation of that file. +# The default value is: YES. + +SHOW_INCLUDE_FILES = YES + +# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each +# grouped member an include statement to the documentation, telling the reader +# which file to include in order to use the member. +# The default value is: NO. + +SHOW_GROUPED_MEMB_INC = NO + +# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include +# files with double quotes in the documentation rather than with sharp brackets. +# The default value is: NO. + +FORCE_LOCAL_INCLUDES = NO + +# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the +# documentation for inline members. +# The default value is: YES. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the +# (detailed) documentation of file and class members alphabetically by member +# name. If set to NO, the members will appear in declaration order. +# The default value is: YES. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief +# descriptions of file, namespace and class members alphabetically by member +# name. If set to NO, the members will appear in declaration order. Note that +# this will also influence the order of the classes in the class list. +# The default value is: NO. + +SORT_BRIEF_DOCS = NO + +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the +# (brief and detailed) documentation of class members so that constructors and +# destructors are listed first. If set to NO the constructors will appear in the +# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS. +# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief +# member documentation. +# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting +# detailed member documentation. +# The default value is: NO. + +SORT_MEMBERS_CTORS_1ST = NO + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy +# of group names into alphabetical order. If set to NO the group names will +# appear in their defined order. +# The default value is: NO. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by +# fully-qualified names, including namespaces. If set to NO, the class list will +# be sorted only by class name, not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the alphabetical +# list. +# The default value is: NO. + +SORT_BY_SCOPE_NAME = NO + +# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper +# type resolution of all parameters of a function it will reject a match between +# the prototype and the implementation of a member function even if there is +# only one candidate or it is obvious which candidate to choose by doing a +# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still +# accept a match between prototype and implementation in such cases. +# The default value is: NO. + +STRICT_PROTO_MATCHING = NO + +# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo +# list. This list is created by putting \todo commands in the documentation. +# The default value is: YES. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test +# list. This list is created by putting \test commands in the documentation. +# The default value is: YES. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug +# list. This list is created by putting \bug commands in the documentation. +# The default value is: YES. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO) +# the deprecated list. This list is created by putting \deprecated commands in +# the documentation. +# The default value is: YES. + +GENERATE_DEPRECATEDLIST= YES + +# The ENABLED_SECTIONS tag can be used to enable conditional documentation +# sections, marked by \if ... \endif and \cond +# ... \endcond blocks. + +ENABLED_SECTIONS = + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the +# initial value of a variable or macro / define can have for it to appear in the +# documentation. If the initializer consists of more lines than specified here +# it will be hidden. Use a value of 0 to hide initializers completely. The +# appearance of the value of individual variables and macros / defines can be +# controlled using \showinitializer or \hideinitializer command in the +# documentation regardless of this setting. +# Minimum value: 0, maximum value: 10000, default value: 30. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at +# the bottom of the documentation of classes and structs. If set to YES, the +# list will mention the files that were used to generate the documentation. +# The default value is: YES. + +SHOW_USED_FILES = YES + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This +# will remove the Files entry from the Quick Index and from the Folder Tree View +# (if specified). +# The default value is: YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces +# page. This will remove the Namespaces entry from the Quick Index and from the +# Folder Tree View (if specified). +# The default value is: YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command command input-file, where command is the value of the +# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided +# by doxygen. Whatever the program writes to standard output is used as the file +# version. For an example see the documentation. + +FILE_VERSION_FILTER = + +# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed +# by doxygen. The layout file controls the global structure of the generated +# output files in an output format independent way. To create the layout file +# that represents doxygen's defaults, run doxygen with the -l option. You can +# optionally specify a file name after the option, if omitted DoxygenLayout.xml +# will be used as the name of the layout file. +# +# Note that if you run doxygen from a directory containing a file called +# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE +# tag is left empty. + +LAYOUT_FILE = + +# The CITE_BIB_FILES tag can be used to specify one or more bib files containing +# the reference definitions. This must be a list of .bib files. The .bib +# extension is automatically appended if omitted. This requires the bibtex tool +# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info. +# For LaTeX the style of the bibliography can be controlled using +# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the +# search path. See also \cite for info how to create references. + +CITE_BIB_FILES = + +#--------------------------------------------------------------------------- +# Configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated to +# standard output by doxygen. If QUIET is set to YES this implies that the +# messages are off. +# The default value is: NO. + +QUIET = NO + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES +# this implies that the warnings are on. +# +# Tip: Turn warnings on while writing the documentation. +# The default value is: YES. + +WARNINGS = YES + +# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate +# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag +# will automatically be disabled. +# The default value is: YES. + +WARN_IF_UNDOCUMENTED = NO + +# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as not documenting some parameters +# in a documented function, or documenting parameters that don't exist or using +# markup commands wrongly. +# The default value is: YES. + +WARN_IF_DOC_ERROR = YES + +# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that +# are documented, but have no documentation for their parameters or return +# value. If set to NO, doxygen will only warn about wrong or incomplete +# parameter documentation, but not about the absence of documentation. +# The default value is: NO. + +WARN_NO_PARAMDOC = NO + +# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when +# a warning is encountered. +# The default value is: NO. + +WARN_AS_ERROR = NO + +# The WARN_FORMAT tag determines the format of the warning messages that doxygen +# can produce. The string should contain the $file, $line, and $text tags, which +# will be replaced by the file and line number from which the warning originated +# and the warning text. Optionally the format may contain $version, which will +# be replaced by the version of the file (if it could be obtained via +# FILE_VERSION_FILTER) +# The default value is: $file:$line: $text. + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning and error +# messages should be written. If left blank the output is written to standard +# error (stderr). + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# Configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag is used to specify the files and/or directories that contain +# documented source files. You may enter file names like myfile.cpp or +# directories like /usr/src/myproject. Separate the files or directories with +# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING +# Note: If this tag is empty the current directory is searched. + +INPUT = ./README.md ./ + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses +# libiconv (or the iconv built into libc) for the transcoding. See the libiconv +# documentation (see: http://www.gnu.org/software/libiconv) for the list of +# possible encodings. +# The default value is: UTF-8. + +INPUT_ENCODING = UTF-8 + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and +# *.h) to filter out the source-files in the directories. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# read by doxygen. +# +# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp, +# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, +# *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc, +# *.m, *.markdown, *.md, *.mm, *.dox, *.py, *.pyw, *.f90, *.f, *.for, *.tcl, +# *.vhd, *.vhdl, *.ucf, *.qsf, *.as and *.js. + +FILE_PATTERNS = + +# The RECURSIVE tag can be used to specify whether or not subdirectories should +# be searched for input files as well. +# The default value is: NO. + +RECURSIVE = YES + +# The EXCLUDE tag can be used to specify files and/or directories that should be +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. +# +# Note that relative paths are relative to the directory from which doxygen is +# run. + +EXCLUDE = + +# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or +# directories that are symbolic links (a Unix file system feature) are excluded +# from the input. +# The default value is: NO. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories for example use the pattern */test/* + +EXCLUDE_PATTERNS = + +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# AClass::ANamespace, ANamespace::*Test +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories use the pattern */test/* + +EXCLUDE_SYMBOLS = + +# The EXAMPLE_PATH tag can be used to specify one or more files or directories +# that contain example code fragments that are included (see the \include +# command). + +EXAMPLE_PATH = + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and +# *.h) to filter out the source-files in the directories. If left blank all +# files are included. + +EXAMPLE_PATTERNS = + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude commands +# irrespective of the value of the RECURSIVE tag. +# The default value is: NO. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or directories +# that contain images that are to be included in the documentation (see the +# \image command). + +IMAGE_PATH = doc + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command: +# +# +# +# where is the value of the INPUT_FILTER tag, and is the +# name of an input file. Doxygen will then use the output that the filter +# program writes to standard output. If FILTER_PATTERNS is specified, this tag +# will be ignored. +# +# Note that the filter must not add or remove lines; it is applied before the +# code is scanned, but not when the output code is generated. If lines are added +# or removed, the anchors will not be placed correctly. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# properly processed by doxygen. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. The filters are a list of the form: pattern=filter +# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how +# filters are used. If the FILTER_PATTERNS tag is empty or if none of the +# patterns match the file name, INPUT_FILTER is applied. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# properly processed by doxygen. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER) will also be used to filter the input files that are used for +# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES). +# The default value is: NO. + +FILTER_SOURCE_FILES = NO + +# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file +# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and +# it is also possible to disable source filtering for a specific pattern using +# *.ext= (so without naming a filter). +# This tag requires that the tag FILTER_SOURCE_FILES is set to YES. + +FILTER_SOURCE_PATTERNS = + +# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that +# is part of the input, its contents will be placed on the main page +# (index.html). This can be useful if you have a project on for instance GitHub +# and want to reuse the introduction page also for the doxygen output. + +USE_MDFILE_AS_MAINPAGE = README.md + +#--------------------------------------------------------------------------- +# Configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will be +# generated. Documented entities will be cross-referenced with these sources. +# +# Note: To get rid of all source code in the generated output, make sure that +# also VERBATIM_HEADERS is set to NO. +# The default value is: NO. + +SOURCE_BROWSER = YES + +# Setting the INLINE_SOURCES tag to YES will include the body of functions, +# classes and enums directly into the documentation. +# The default value is: NO. + +INLINE_SOURCES = NO + +# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any +# special comment blocks from generated source code fragments. Normal C, C++ and +# Fortran comments will always remain visible. +# The default value is: YES. + +STRIP_CODE_COMMENTS = YES + +# If the REFERENCED_BY_RELATION tag is set to YES then for each documented +# function all documented functions referencing it will be listed. +# The default value is: NO. + +REFERENCED_BY_RELATION = NO + +# If the REFERENCES_RELATION tag is set to YES then for each documented function +# all documented entities called/used by that function will be listed. +# The default value is: NO. + +REFERENCES_RELATION = NO + +# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set +# to YES then the hyperlinks from functions in REFERENCES_RELATION and +# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will +# link to the documentation. +# The default value is: YES. + +REFERENCES_LINK_SOURCE = YES + +# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the +# source code will show a tooltip with additional information such as prototype, +# brief description and links to the definition and documentation. Since this +# will make the HTML file larger and loading of large files a bit slower, you +# can opt to disable this feature. +# The default value is: YES. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +SOURCE_TOOLTIPS = YES + +# If the USE_HTAGS tag is set to YES then the references to source code will +# point to the HTML generated by the htags(1) tool instead of doxygen built-in +# source browser. The htags tool is part of GNU's global source tagging system +# (see http://www.gnu.org/software/global/global.html). You will need version +# 4.8.6 or higher. +# +# To use it do the following: +# - Install the latest version of global +# - Enable SOURCE_BROWSER and USE_HTAGS in the config file +# - Make sure the INPUT points to the root of the source tree +# - Run doxygen as normal +# +# Doxygen will invoke htags (and that will in turn invoke gtags), so these +# tools must be available from the command line (i.e. in the search path). +# +# The result: instead of the source browser generated by doxygen, the links to +# source code will now point to the output of htags. +# The default value is: NO. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a +# verbatim copy of the header file for each class for which an include is +# specified. Set to NO to disable this. +# See also: Section \class. +# The default value is: YES. + +VERBATIM_HEADERS = YES + +# If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the +# clang parser (see: http://clang.llvm.org/) for more accurate parsing at the +# cost of reduced performance. This can be particularly helpful with template +# rich C++ code for which doxygen's built-in parser lacks the necessary type +# information. +# Note: The availability of this option depends on whether or not doxygen was +# generated with the -Duse-libclang=ON option for CMake. +# The default value is: NO. + +CLANG_ASSISTED_PARSING = NO + +# If clang assisted parsing is enabled you can provide the compiler with command +# line options that you would normally use when invoking the compiler. Note that +# the include paths will already be set by doxygen for the files and directories +# specified with INPUT and INCLUDE_PATH. +# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES. + +CLANG_OPTIONS = + +#--------------------------------------------------------------------------- +# Configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all +# compounds will be generated. Enable this if the project contains a lot of +# classes, structs, unions or interfaces. +# The default value is: YES. + +ALPHABETICAL_INDEX = YES + +# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in +# which the alphabetical index list will be split. +# Minimum value: 1, maximum value: 20, default value: 5. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +COLS_IN_ALPHA_INDEX = 5 + +# In case all classes in a project start with a common prefix, all classes will +# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag +# can be used to specify a prefix (or a list of prefixes) that should be ignored +# while generating the index headers. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output +# The default value is: YES. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a +# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of +# it. +# The default directory is: html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_OUTPUT = "../docs" + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each +# generated HTML page (for example: .htm, .php, .asp). +# The default value is: .html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a user-defined HTML header file for +# each generated HTML page. If the tag is left blank doxygen will generate a +# standard header. +# +# To get valid HTML the header file that includes any scripts and style sheets +# that doxygen needs, which is dependent on the configuration options used (e.g. +# the setting GENERATE_TREEVIEW). It is highly recommended to start with a +# default header using +# doxygen -w html new_header.html new_footer.html new_stylesheet.css +# YourConfigFile +# and then modify the file new_header.html. See also section "Doxygen usage" +# for information on how to generate the default header that doxygen normally +# uses. +# Note: The header is subject to change so you typically have to regenerate the +# default header when upgrading to a newer version of doxygen. For a description +# of the possible markers and block names see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each +# generated HTML page. If the tag is left blank doxygen will generate a standard +# footer. See HTML_HEADER for more information on how to generate a default +# footer and what special commands can be used inside the footer. See also +# section "Doxygen usage" for information on how to generate the default footer +# that doxygen normally uses. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style +# sheet that is used by each HTML page. It can be used to fine-tune the look of +# the HTML output. If left blank doxygen will generate a default style sheet. +# See also section "Doxygen usage" for information on how to generate the style +# sheet that doxygen normally uses. +# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as +# it is more robust and this tag (HTML_STYLESHEET) will in the future become +# obsolete. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_STYLESHEET = + +# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined +# cascading style sheets that are included after the standard style sheets +# created by doxygen. Using this option one can overrule certain style aspects. +# This is preferred over using HTML_STYLESHEET since it does not replace the +# standard style sheet and is therefore more robust against future updates. +# Doxygen will copy the style sheet files to the output directory. +# Note: The order of the extra style sheet files is of importance (e.g. the last +# style sheet in the list overrules the setting of the previous ones in the +# list). For an example see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_STYLESHEET = + +# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the HTML output directory. Note +# that these files will be copied to the base HTML output directory. Use the +# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these +# files. In the HTML_STYLESHEET file, use the file name only. Also note that the +# files will be copied as-is; there are no commands or markers available. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_FILES = + +# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen +# will adjust the colors in the style sheet and background images according to +# this color. Hue is specified as an angle on a colorwheel, see +# http://en.wikipedia.org/wiki/Hue for more information. For instance the value +# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300 +# purple, and 360 is red again. +# Minimum value: 0, maximum value: 359, default value: 220. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_HUE = 220 + +# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors +# in the HTML output. For a value of 0 the output will use grayscales only. A +# value of 255 will produce the most vivid colors. +# Minimum value: 0, maximum value: 255, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_SAT = 100 + +# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the +# luminance component of the colors in the HTML output. Values below 100 +# gradually make the output lighter, whereas values above 100 make the output +# darker. The value divided by 100 is the actual gamma applied, so 80 represents +# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not +# change the gamma. +# Minimum value: 40, maximum value: 240, default value: 80. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_GAMMA = 80 + +# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML +# page will contain the date and time when the page was generated. Setting this +# to YES can help to show when doxygen was last run and thus if the +# documentation is up to date. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_TIMESTAMP = NO + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_DYNAMIC_SECTIONS = NO + +# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries +# shown in the various tree structured indices initially; the user can expand +# and collapse entries dynamically later on. Doxygen will expand the tree to +# such a level that at most the specified number of entries are visible (unless +# a fully collapsed tree already exceeds this amount). So setting the number of +# entries 1 will produce a full collapsed tree by default. 0 is a special value +# representing an infinite number of entries and will result in a full expanded +# tree by default. +# Minimum value: 0, maximum value: 9999, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_INDEX_NUM_ENTRIES = 100 + +# If the GENERATE_DOCSET tag is set to YES, additional index files will be +# generated that can be used as input for Apple's Xcode 3 integrated development +# environment (see: http://developer.apple.com/tools/xcode/), introduced with +# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a +# Makefile in the HTML output directory. Running make will produce the docset in +# that directory and running make install will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at +# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html +# for more information. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_DOCSET = NO + +# This tag determines the name of the docset feed. A documentation feed provides +# an umbrella under which multiple documentation sets from a single provider +# (such as a company or product suite) can be grouped. +# The default value is: Doxygen generated docs. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# This tag specifies a string that should uniquely identify the documentation +# set bundle. This should be a reverse domain-name style string, e.g. +# com.mycompany.MyDocSet. Doxygen will append .docset to the name. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify +# the documentation publisher. This should be a reverse domain-name style +# string, e.g. com.mycompany.MyDocSet.documentation. +# The default value is: org.doxygen.Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_ID = org.doxygen.Publisher + +# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher. +# The default value is: Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_NAME = Publisher + +# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three +# additional HTML index files: index.hhp, index.hhc, and index.hhk. The +# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop +# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on +# Windows. +# +# The HTML Help Workshop contains a compiler that can convert all HTML output +# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML +# files are now used as the Windows 98 help format, and will replace the old +# Windows help format (.hlp) on all Windows platforms in the future. Compressed +# HTML files also contain an index, a table of contents, and you can search for +# words in the documentation. The HTML workshop also contains a viewer for +# compressed HTML files. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_HTMLHELP = NO + +# The CHM_FILE tag can be used to specify the file name of the resulting .chm +# file. You can add a path in front of the file if the result should not be +# written to the html output directory. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_FILE = + +# The HHC_LOCATION tag can be used to specify the location (absolute path +# including file name) of the HTML help compiler (hhc.exe). If non-empty, +# doxygen will try to run the HTML help compiler on the generated index.hhp. +# The file has to be specified with full path. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +HHC_LOCATION = + +# The GENERATE_CHI flag controls if a separate .chi index file is generated +# (YES) or that it should be included in the master .chm file (NO). +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +GENERATE_CHI = NO + +# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc) +# and project file content. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_INDEX_ENCODING = + +# The BINARY_TOC flag controls whether a binary table of contents is generated +# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it +# enables the Previous and Next buttons. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members to +# the table of contents of the HTML help documentation and to the tree view. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +TOC_EXPAND = NO + +# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and +# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that +# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help +# (.qch) of the generated HTML documentation. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_QHP = NO + +# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify +# the file name of the resulting .qch file. The path specified is relative to +# the HTML output folder. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QCH_FILE = + +# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help +# Project output. For more information please see Qt Help Project / Namespace +# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace). +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_NAMESPACE = org.doxygen.Project + +# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt +# Help Project output. For more information please see Qt Help Project / Virtual +# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual- +# folders). +# The default value is: doc. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_VIRTUAL_FOLDER = doc + +# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom +# filter to add. For more information please see Qt Help Project / Custom +# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom- +# filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_NAME = + +# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the +# custom filter to add. For more information please see Qt Help Project / Custom +# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom- +# filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_ATTRS = + +# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this +# project's filter section matches. Qt Help Project / Filter Attributes (see: +# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_SECT_FILTER_ATTRS = + +# The QHG_LOCATION tag can be used to specify the location of Qt's +# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the +# generated .qhp file. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHG_LOCATION = + +# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be +# generated, together with the HTML files, they form an Eclipse help plugin. To +# install this plugin and make it available under the help contents menu in +# Eclipse, the contents of the directory containing the HTML and XML files needs +# to be copied into the plugins directory of eclipse. The name of the directory +# within the plugins directory should be the same as the ECLIPSE_DOC_ID value. +# After copying Eclipse needs to be restarted before the help appears. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_ECLIPSEHELP = NO + +# A unique identifier for the Eclipse help plugin. When installing the plugin +# the directory name containing the HTML and XML files should also have this +# name. Each documentation set should have its own identifier. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES. + +ECLIPSE_DOC_ID = org.doxygen.Project + +# If you want full control over the layout of the generated HTML pages it might +# be necessary to disable the index and replace it with your own. The +# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top +# of each HTML page. A value of NO enables the index and the value YES disables +# it. Since the tabs in the index contain the same information as the navigation +# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +DISABLE_INDEX = NO + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. If the tag +# value is set to YES, a side panel will be generated containing a tree-like +# index structure (just like the one that is generated for HTML Help). For this +# to work a browser that supports JavaScript, DHTML, CSS and frames is required +# (i.e. any modern browser). Windows users are probably better off using the +# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can +# further fine-tune the look of the index. As an example, the default style +# sheet generated by doxygen has an example that shows how to put an image at +# the root of the tree instead of the PROJECT_NAME. Since the tree basically has +# the same information as the tab index, you could consider setting +# DISABLE_INDEX to YES when enabling this option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_TREEVIEW = NO + +# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that +# doxygen will group on one line in the generated HTML documentation. +# +# Note that a value of 0 will completely suppress the enum values from appearing +# in the overview section. +# Minimum value: 0, maximum value: 20, default value: 4. +# This tag requires that the tag GENERATE_HTML is set to YES. + +ENUM_VALUES_PER_LINE = 4 + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used +# to set the initial width (in pixels) of the frame in which the tree is shown. +# Minimum value: 0, maximum value: 1500, default value: 250. +# This tag requires that the tag GENERATE_HTML is set to YES. + +TREEVIEW_WIDTH = 250 + +# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to +# external symbols imported via tag files in a separate window. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +EXT_LINKS_IN_WINDOW = NO + +# Use this tag to change the font size of LaTeX formulas included as images in +# the HTML documentation. When you change the font size after a successful +# doxygen run you need to manually remove any form_*.png images from the HTML +# output directory to force them to be regenerated. +# Minimum value: 8, maximum value: 50, default value: 10. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_FONTSIZE = 10 + +# Use the FORMULA_TRANPARENT tag to determine whether or not the images +# generated for formulas are transparent PNGs. Transparent PNGs are not +# supported properly for IE 6.0, but are supported on all modern browsers. +# +# Note that when changing this option you need to delete any form_*.png files in +# the HTML output directory before the changes have effect. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_TRANSPARENT = YES + +# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see +# http://www.mathjax.org) which uses client side Javascript for the rendering +# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX +# installed or if you want to formulas look prettier in the HTML output. When +# enabled you may also need to install MathJax separately and configure the path +# to it using the MATHJAX_RELPATH option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +USE_MATHJAX = NO + +# When MathJax is enabled you can set the default output format to be used for +# the MathJax output. See the MathJax site (see: +# http://docs.mathjax.org/en/latest/output.html) for more details. +# Possible values are: HTML-CSS (which is slower, but has the best +# compatibility), NativeMML (i.e. MathML) and SVG. +# The default value is: HTML-CSS. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_FORMAT = HTML-CSS + +# When MathJax is enabled you need to specify the location relative to the HTML +# output directory using the MATHJAX_RELPATH option. The destination directory +# should contain the MathJax.js script. For instance, if the mathjax directory +# is located at the same level as the HTML output directory, then +# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax +# Content Delivery Network so you can quickly see the result without installing +# MathJax. However, it is strongly recommended to install a local copy of +# MathJax from http://www.mathjax.org before deployment. +# The default value is: http://cdn.mathjax.org/mathjax/latest. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest + +# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax +# extension names that should be enabled during MathJax rendering. For example +# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_EXTENSIONS = + +# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces +# of code that will be used on startup of the MathJax code. See the MathJax site +# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an +# example see the documentation. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_CODEFILE = + +# When the SEARCHENGINE tag is enabled doxygen will generate a search box for +# the HTML output. The underlying search engine uses javascript and DHTML and +# should work on any modern browser. Note that when using HTML help +# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET) +# there is already a search function so this one should typically be disabled. +# For large projects the javascript based search engine can be slow, then +# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to +# search using the keyboard; to jump to the search box use + S +# (what the is depends on the OS and browser, but it is typically +# , /