From 459cfca5eb0b7c9f9b6b9a64b0c18226a1df5c38 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Tue, 9 Feb 2021 14:53:58 +0000 Subject: [PATCH 1/4] robot_trajectory: Fix bugs in getRobotTrajectoryMsg function --- .../robot_trajectory/src/robot_trajectory.cpp | 22 +++++-------------- 1 file changed, 5 insertions(+), 17 deletions(-) diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index 80f06ee852..f6fdbd6111 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -223,9 +223,6 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory& t const std::vector& joint_filter) const { trajectory = moveit_msgs::msg::RobotTrajectory(); - - builtin_interfaces::msg::Time stamp = rclcpp::Clock(RCL_ROS_TIME).now(); - if (waypoints_.empty()) return; const std::vector& jnts = @@ -258,27 +255,24 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory& t if (!onedof.empty()) { trajectory.joint_trajectory.header.frame_id = robot_model_->getModelFrame(); - trajectory.joint_trajectory.header.stamp = stamp; + trajectory.joint_trajectory.header.stamp = rclcpp::Time(0, 0, RCL_ROS_TIME); trajectory.joint_trajectory.points.resize(waypoints_.size()); } if (!mdof.empty()) { trajectory.multi_dof_joint_trajectory.header.frame_id = robot_model_->getModelFrame(); - trajectory.multi_dof_joint_trajectory.header.stamp = stamp; + trajectory.multi_dof_joint_trajectory.header.stamp = rclcpp::Time(0, 0, RCL_ROS_TIME); trajectory.multi_dof_joint_trajectory.points.resize(waypoints_.size()); } static const rclcpp::Duration ZERO_DURATION(0.0); double total_time = 0.0; - rclcpp::Duration dur_total(0, 0); for (std::size_t i = 0; i < waypoints_.size(); ++i) { if (duration_from_previous_.size() > i) total_time += duration_from_previous_[i]; - dur_total = rclcpp::Duration(1, 0) * total_time; - if (!onedof.empty()) { trajectory.joint_trajectory.points[i].positions.resize(onedof.size()); @@ -310,8 +304,7 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory& t trajectory.joint_trajectory.points[i].effort.clear(); if (duration_from_previous_.size() > i) - - trajectory.joint_trajectory.points[i].time_from_start = dur_total; + trajectory.joint_trajectory.points[i].time_from_start = rclcpp::Duration::from_seconds(total_time); else trajectory.joint_trajectory.points[i].time_from_start = ZERO_DURATION; } @@ -353,7 +346,7 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory& t } } if (duration_from_previous_.size() > i) - trajectory.multi_dof_joint_trajectory.points[i].time_from_start = dur_total; + trajectory.multi_dof_joint_trajectory.points[i].time_from_start = rclcpp::Duration::from_seconds(total_time); else trajectory.multi_dof_joint_trajectory.points[i].time_from_start = ZERO_DURATION; } @@ -364,19 +357,15 @@ void RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& refe const trajectory_msgs::msg::JointTrajectory& trajectory) { // make a copy just in case the next clear() removes the memory for the reference passed in - const moveit::core::RobotState& copy = reference_state; clear(); std::size_t state_count = trajectory.points.size(); rclcpp::Time last_time_stamp = trajectory.header.stamp; rclcpp::Time this_time_stamp = last_time_stamp; - rclcpp::Time traj_stamp = trajectory.header.stamp; - rclcpp::Duration dur_from_start(0, 0); - for (std::size_t i = 0; i < state_count; ++i) { - this_time_stamp = traj_stamp + trajectory.points[i].time_from_start; + this_time_stamp = rclcpp::Time(trajectory.header.stamp) + trajectory.points[i].time_from_start; moveit::core::RobotStatePtr st(new moveit::core::RobotState(copy)); st->setVariablePositions(trajectory.joint_names, trajectory.points[i].positions); if (!trajectory.points[i].velocities.empty()) @@ -393,7 +382,6 @@ void RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& refe void RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, const moveit_msgs::msg::RobotTrajectory& trajectory) { - geometry_msgs::msg::TransformStamped tf_stamped; // make a copy just in case the next clear() removes the memory for the reference passed in const moveit::core::RobotState& copy = reference_state; clear(); From ba20ace35db87676694da21bf23228180eba8b22 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Tue, 9 Feb 2021 14:55:15 +0000 Subject: [PATCH 2/4] controller_manager: Use Duration(-1) as infinite timeout --- .../include/moveit/controller_manager/controller_manager.h | 4 ++-- .../action_based_controller_handle.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h b/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h index e7ff8946e2..5d5f8ad8b8 100644 --- a/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h +++ b/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h @@ -137,8 +137,8 @@ class MoveItControllerHandle * * Return true if the execution is complete (whether successful or not). * Return false if timeout was reached. - * If timeout is 0 (default argument), wait until the execution is complete (no timeout). */ - virtual bool waitForExecution(const rclcpp::Duration& timeout = rclcpp::Duration(0.0)) = 0; + * If timeout is -1 (default argument), wait until the execution is complete (no timeout). */ + virtual bool waitForExecution(const rclcpp::Duration& timeout = rclcpp::Duration(-1)) = 0; /** \brief Return the execution status of the last trajectory sent to the controller. */ virtual ExecutionStatus getLastExecutionStatus() = 0; 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 fdc4264999..61e9d245ab 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 @@ -139,7 +139,7 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase virtual void controllerDoneCallback(const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) = 0; - bool waitForExecution(const rclcpp::Duration& timeout = rclcpp::Duration(0)) override + bool waitForExecution(const rclcpp::Duration& timeout = rclcpp::Duration(-1)) override { std::promise result_callback_done; auto result_future = controller_action_client_->async_get_result( @@ -147,7 +147,7 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase controllerDoneCallback(wrapped_result); result_callback_done.set_value(true); }); - if (timeout.seconds() == 0.0) + if (timeout < std::chrono::nanoseconds(0)) { result_future.wait(); } From 52f8441f47b3290901f1e028996612a27eba5d4a Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Tue, 9 Feb 2021 14:55:51 +0000 Subject: [PATCH 3/4] ActionBasedControllerHandle: fix dangling reference in case of timeout --- .../action_based_controller_handle.h | 8 ++++---- 1 file changed, 4 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 61e9d245ab..a9abec3dad 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 @@ -141,11 +141,11 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase bool waitForExecution(const rclcpp::Duration& timeout = rclcpp::Duration(-1)) override { - std::promise result_callback_done; + auto result_callback_done = std::make_shared>(); auto result_future = controller_action_client_->async_get_result( - current_goal_, [this, &result_callback_done](const auto& wrapped_result) { + current_goal_, [this, result_callback_done](const auto& wrapped_result) { controllerDoneCallback(wrapped_result); - result_callback_done.set_value(true); + result_callback_done->set_value(true); }); if (timeout < std::chrono::nanoseconds(0)) { @@ -161,7 +161,7 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase } } // To accommodate for the delay after the future for the result is ready and the time controllerDoneCallback takes to finish - result_callback_done.get_future().wait(); + result_callback_done->get_future().wait(); return true; } From b1202e2bb0b2fa6237e8dd5ea13c60183c46f8fa Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Tue, 9 Feb 2021 15:00:44 +0000 Subject: [PATCH 4/4] TfPublisher: tf frame name can't start with '/' --- .../default_capabilities/tf_publisher_capability.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp index 426e54a02b..cc8ee84468 100644 --- a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp @@ -124,12 +124,12 @@ void TfPublisher::publishPlanningSceneFrames() void TfPublisher::initialize() { - rclcpp::Node::SharedPtr n(new rclcpp::Node("TFPublisher")); + std::string prefix = context_->node_->get_name(); + context_->node_->get_parameter_or("planning_scene_frame_publishing_rate", rate_, 10); + context_->node_->get_parameter_or("planning_scene_tf_prefix", prefix_, prefix); + if (!prefix_.empty()) + prefix_ += "/"; - std::string prefix = n->get_namespace(); - prefix += "/"; - n->get_parameter_or("planning_scene_frame_publishing_rate", rate_, 10); - n->get_parameter_or("planning_scene_tf_prefix", prefix_, prefix); keep_running_ = true; RCLCPP_INFO(LOGGER, "Initializing MoveGroupTfPublisher with a frame publishing rate of %d", rate_);