From cce943457990cb2bafe84c85a5fb9028b3ce722b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 15 Sep 2021 15:00:44 +0200 Subject: [PATCH 01/14] 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 02/14] 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 03/14] 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 04/14] 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 05/14] 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 06/14] 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 07/14] 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 08/14] 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 09/14] 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 10/14] 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 11/14] 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 12/14] 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 13/14] 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 14/14] 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