From 34c88fe6ee5153e93afefe8c9bfa608a1be2315e Mon Sep 17 00:00:00 2001 From: marqrazz Date: Wed, 15 Jan 2025 11:10:36 -0700 Subject: [PATCH] only use max velocity/effor when specified --- ...llel_gripper_command_controller_plugin.cpp | 3 ++- .../gripper_command_controller_handle.hpp | 2 +- ...llel_gripper_command_controller_handle.hpp | 26 +++++++++++++------ .../src/moveit_simple_controller_manager.cpp | 14 +++++++--- 4 files changed, 32 insertions(+), 13 deletions(-) diff --git a/moveit_plugins/moveit_ros_control_interface/src/parallel_gripper_command_controller_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/parallel_gripper_command_controller_plugin.cpp index a33e7fd33c..a3fbdcf2fe 100644 --- a/moveit_plugins/moveit_ros_control_interface/src/parallel_gripper_command_controller_plugin.cpp +++ b/moveit_plugins/moveit_ros_control_interface/src/parallel_gripper_command_controller_plugin.cpp @@ -52,7 +52,8 @@ class ParallelGripperCommandControllerAllocator : public ControllerHandleAllocat const std::string& name, const std::vector& /* resources */) override { - return std::make_shared(node, name, "gripper_cmd"); + return std::make_shared(node, name, + "gripper_cmd"); } }; diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_command_controller_handle.hpp b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_command_controller_handle.hpp index 803ad21149..71298ee3f1 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_command_controller_handle.hpp +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_command_controller_handle.hpp @@ -52,7 +52,7 @@ class GripperCommandControllerHandle : public ActionBasedControllerHandle( node, name, ns, "moveit.simple_controller_manager.gripper_controller_handle") , allow_failure_(false) diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/parallel_gripper_command_controller_handle.hpp b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/parallel_gripper_command_controller_handle.hpp index 7c93c8b50e..e313bafc90 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/parallel_gripper_command_controller_handle.hpp +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/parallel_gripper_command_controller_handle.hpp @@ -46,12 +46,14 @@ namespace moveit_simple_controller_manager /* * This is an interface for a gripper using the control_msgs/ParallelGripperCommand action interface. */ -class ParallelGripperCommandControllerHandle : public ActionBasedControllerHandle +class ParallelGripperCommandControllerHandle + : public ActionBasedControllerHandle { public: /* Topics will map to name/ns/goal, name/ns/result, etc */ - ParallelGripperCommandControllerHandle(const rclcpp::Node::SharedPtr& node, const std::string& name, const std::string& ns, - const double max_effort = 0.0, const double max_velocity = 0.0) + ParallelGripperCommandControllerHandle(const rclcpp::Node::SharedPtr& node, const std::string& name, + const std::string& ns, const double max_effort = 0.0, + const double max_velocity = 0.0) : ActionBasedControllerHandle( node, name, ns, "moveit.simple_controller_manager.parallel_gripper_controller_handle") , allow_stalling_(false) @@ -93,7 +95,7 @@ class ParallelGripperCommandControllerHandle : public ActionBasedControllerHandl // goal to be sent control_msgs::action::ParallelGripperCommand::Goal goal; - auto &cmd_state = goal.command; + auto& cmd_state = goal.command; std::vector gripper_joint_indexes; for (std::size_t i = 0; i < trajectory.joint_trajectory.joint_names.size(); ++i) @@ -123,12 +125,19 @@ class ParallelGripperCommandControllerHandle : public ActionBasedControllerHandl RCLCPP_ERROR(logger_, "ParallelGripperCommand expects a joint trajectory with one \ point that specifies at least the position of joint \ '%s', but insufficient positions provided.", - trajectory.joint_trajectory.joint_names[idx].c_str()); + trajectory.joint_trajectory.joint_names[idx].c_str()); return false; } cmd_state.position.push_back(trajectory.joint_trajectory.points[tpoint].positions[idx]); - cmd_state.velocity.push_back(max_velocity_); // TODO: Search received trajectory for max velocity. - cmd_state.effort.push_back(max_effort_); + // only set the velocity or effort if the user has specified a positive non-zero max value + if (max_velocity_ > 0.0) + { + cmd_state.velocity.push_back(max_velocity_); + } + if (max_effort_ > 0.0) + { + cmd_state.effort.push_back(max_effort_); + } } rclcpp_action::Client::SendGoalOptions send_goal_options; // Active callback @@ -167,7 +176,8 @@ class ParallelGripperCommandControllerHandle : public ActionBasedControllerHandl private: void controllerDoneCallback( - const rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) override + const rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) + override { if (wrapped_result.code == rclcpp_action::ResultCode::ABORTED && wrapped_result.result->stalled && allow_stalling_) { diff --git a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp index 47f4745b8e..94cc9a6874 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp @@ -193,16 +193,24 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo const std::string& max_effort_param = makeParameterName(PARAM_BASE_NAME, controller_name, "max_effort"); if (!node->get_parameter(max_effort_param, max_effort)) { - RCLCPP_INFO_STREAM(getLogger(), controller_name << " max effort set to 0.0"); + RCLCPP_DEBUG_STREAM(getLogger(), controller_name << " max effort is not specified and will not be used."); max_effort = 0.0; } + else + { + RCLCPP_INFO_STREAM(getLogger(), controller_name << " max effort set to: " << max_effort); + } double max_velocity; const std::string& max_velocity_param = makeParameterName(PARAM_BASE_NAME, controller_name, "max_velocity"); if (!node->get_parameter(max_velocity_param, max_velocity)) { - RCLCPP_INFO_STREAM(getLogger(), controller_name << " max velocity set to 0.0"); + RCLCPP_DEBUG_STREAM(getLogger(), controller_name << " max velocity is not specified and will not be used."); max_velocity = 0.0; } + else + { + RCLCPP_INFO_STREAM(getLogger(), controller_name << " max velocity set to: " << max_velocity); + } new_handle = std::make_shared(node_, controller_name, action_ns, max_effort, max_velocity); @@ -216,7 +224,7 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo false); static_cast(new_handle.get())->allowFailure(allow_stalling); - RCLCPP_INFO_STREAM(getLogger(), "Added GripperCommand controller for " << controller_name); + RCLCPP_INFO_STREAM(getLogger(), "Added ParallelGripperCommand controller for " << controller_name); controllers_[controller_name] = new_handle; } else if (type == "FollowJointTrajectory")