diff --git a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt
index 38a203fe27..a23dac9f96 100644
--- a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt
+++ b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt
@@ -38,7 +38,7 @@ ament_target_dependencies(moveit_ros_control_interface_trajectory_plugin
${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
add_library(moveit_ros_control_interface_gripper_plugin SHARED
- src/gripper_controller_plugin.cpp)
+ src/gripper_command_controller_plugin.cpp)
set_target_properties(
moveit_ros_control_interface_gripper_plugin
PROPERTIES VERSION "${moveit_ros_control_interface_VERSION}")
@@ -47,6 +47,16 @@ target_include_directories(moveit_ros_control_interface_gripper_plugin
ament_target_dependencies(moveit_ros_control_interface_gripper_plugin
${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
+add_library(moveit_ros_control_interface_parallel_gripper_plugin SHARED
+ src/parallel_gripper_command_controller_plugin.cpp)
+set_target_properties(
+ moveit_ros_control_interface_parallel_gripper_plugin
+ PROPERTIES VERSION "${moveit_ros_control_interface_VERSION}")
+target_include_directories(moveit_ros_control_interface_parallel_gripper_plugin
+ PRIVATE include)
+ament_target_dependencies(moveit_ros_control_interface_parallel_gripper_plugin
+ ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
+
add_library(moveit_ros_control_interface_empty_plugin SHARED
src/empty_controller_plugin.cpp)
set_target_properties(
@@ -65,6 +75,7 @@ set(TARGET_LIBRARIES
moveit_ros_control_interface_plugin
moveit_ros_control_interface_trajectory_plugin
moveit_ros_control_interface_gripper_plugin
+ moveit_ros_control_interface_parallel_gripper_plugin
moveit_ros_control_interface_empty_plugin)
# Mark executables and/or libraries for installation
diff --git a/moveit_plugins/moveit_ros_control_interface/moveit_ros_control_interface_plugins.xml b/moveit_plugins/moveit_ros_control_interface/moveit_ros_control_interface_plugins.xml
index fc6777c13c..928be71cc3 100644
--- a/moveit_plugins/moveit_ros_control_interface/moveit_ros_control_interface_plugins.xml
+++ b/moveit_plugins/moveit_ros_control_interface/moveit_ros_control_interface_plugins.xml
@@ -10,10 +10,15 @@
</class>
</library>
<library path="moveit_ros_control_interface_gripper_plugin">
- <class name="position_controllers/GripperActionController" type="moveit_ros_control_interface::GripperControllerAllocator" base_class_type="moveit_ros_control_interface::ControllerHandleAllocator">
+ <class name="position_controllers/GripperActionController" type="moveit_ros_control_interface::GripperCommandControllerAllocator" base_class_type="moveit_ros_control_interface::ControllerHandleAllocator">
<description></description>
</class>
- <class name="effort_controllers/GripperActionController" type="moveit_ros_control_interface::GripperControllerAllocator" base_class_type="moveit_ros_control_interface::ControllerHandleAllocator">
+ <class name="effort_controllers/GripperActionController" type="moveit_ros_control_interface::GripperCommandControllerAllocator" base_class_type="moveit_ros_control_interface::ControllerHandleAllocator">
+ <description></description>
+ </class>
+ </library>
+ <library path="moveit_ros_control_interface_parallel_gripper_plugin">
+ <class name="parallel_gripper_action_controller/GripperActionController" type="moveit_ros_control_interface::ParallelGripperCommandControllerAllocator" base_class_type="moveit_ros_control_interface::ControllerHandleAllocator">
<description></description>
</class>
</library>
diff --git a/moveit_plugins/moveit_ros_control_interface/src/gripper_controller_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/gripper_command_controller_plugin.cpp
similarity index 88%
rename from moveit_plugins/moveit_ros_control_interface/src/gripper_controller_plugin.cpp
rename to moveit_plugins/moveit_ros_control_interface/src/gripper_command_controller_plugin.cpp
index 8d6e9fafad..ba0e0cefd9 100644
--- a/moveit_plugins/moveit_ros_control_interface/src/gripper_controller_plugin.cpp
+++ b/moveit_plugins/moveit_ros_control_interface/src/gripper_command_controller_plugin.cpp
@@ -36,27 +36,27 @@
#include <moveit_ros_control_interface/ControllerHandle.hpp>
#include <pluginlib/class_list_macros.hpp>
-#include <moveit_simple_controller_manager/gripper_controller_handle.hpp>
+#include <moveit_simple_controller_manager/gripper_command_controller_handle.hpp>
#include <rclcpp/node.hpp>
#include <memory>
namespace moveit_ros_control_interface
{
/**
- * \brief Simple allocator for moveit_simple_controller_manager::FollowJointTrajectoryControllerHandle instances.
+ * \brief Simple allocator for moveit_simple_controller_manager::GripperCommandControllerHandle instances.
*/
-class GripperControllerAllocator : public ControllerHandleAllocator
+class GripperCommandControllerAllocator : public ControllerHandleAllocator
{
public:
moveit_controller_manager::MoveItControllerHandlePtr alloc(const rclcpp::Node::SharedPtr& node,
const std::string& name,
const std::vector<std::string>& /* resources */) override
{
- return std::make_shared<moveit_simple_controller_manager::GripperControllerHandle>(node, name, "gripper_cmd");
+ return std::make_shared<moveit_simple_controller_manager::GripperCommandControllerHandle>(node, name, "gripper_cmd");
}
};
} // namespace moveit_ros_control_interface
-PLUGINLIB_EXPORT_CLASS(moveit_ros_control_interface::GripperControllerAllocator,
+PLUGINLIB_EXPORT_CLASS(moveit_ros_control_interface::GripperCommandControllerAllocator,
moveit_ros_control_interface::ControllerHandleAllocator);
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
new file mode 100644
index 0000000000..9666cb11dc
--- /dev/null
+++ b/moveit_plugins/moveit_ros_control_interface/src/parallel_gripper_command_controller_plugin.cpp
@@ -0,0 +1,63 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2025, Marq Rasmussen
+ * 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 Marq Rasmussen 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: Marq Rasmussen */
+
+#include <moveit_ros_control_interface/ControllerHandle.hpp>
+#include <pluginlib/class_list_macros.hpp>
+#include <moveit_simple_controller_manager/parallel_gripper_command_controller_handle.hpp>
+#include <rclcpp/node.hpp>
+#include <memory>
+
+namespace moveit_ros_control_interface
+{
+/**
+ * \brief Simple allocator for moveit_simple_controller_manager::ParallelGripperCommandControllerHandle instances.
+ */
+class ParallelGripperCommandControllerAllocator : public ControllerHandleAllocator
+{
+public:
+ moveit_controller_manager::MoveItControllerHandlePtr alloc(const rclcpp::Node::SharedPtr& node,
+ const std::string& name,
+ const std::vector<std::string>& /* resources */) override
+ {
+ return std::make_shared<moveit_simple_controller_manager::ParallelGripperCommandControllerHandle>(node, name,
+ "gripper_cmd");
+ }
+};
+
+} // namespace moveit_ros_control_interface
+
+PLUGINLIB_EXPORT_CLASS(moveit_ros_control_interface::ParallelGripperCommandControllerAllocator,
+ moveit_ros_control_interface::ControllerHandleAllocator);
diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.hpp b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_command_controller_handle.hpp
similarity index 92%
rename from moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.hpp
rename to moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_command_controller_handle.hpp
index 6f2dc9a21b..2284e81e3d 100644
--- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.hpp
+++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_command_controller_handle.hpp
@@ -47,12 +47,12 @@ namespace moveit_simple_controller_manager
* This is an interface for a gripper using control_msgs/GripperCommandAction
* action interface (single DOF).
*/
-class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs::action::GripperCommand>
+class GripperCommandControllerHandle : public ActionBasedControllerHandle<control_msgs::action::GripperCommand>
{
public:
/* Topics will map to name/ns/goal, name/ns/result, etc */
- GripperControllerHandle(const rclcpp::Node::SharedPtr& node, const std::string& name, const std::string& ns,
- const double max_effort = 0.0)
+ GripperCommandControllerHandle(const rclcpp::Node::SharedPtr& node, const std::string& name, const std::string& ns,
+ const double max_effort = 0.0)
: ActionBasedControllerHandle<control_msgs::action::GripperCommand>(
node, name, ns, "moveit.simple_controller_manager.gripper_controller_handle")
, allow_failure_(false)
@@ -98,23 +98,23 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
return false;
}
- std::vector<std::size_t> gripper_joint_indexes;
+ std::vector<std::size_t> gripper_joint_indices;
for (std::size_t i = 0; i < trajectory.joint_trajectory.joint_names.size(); ++i)
{
if (command_joints_.find(trajectory.joint_trajectory.joint_names[i]) != command_joints_.end())
{
- gripper_joint_indexes.push_back(i);
+ gripper_joint_indices.push_back(i);
if (!parallel_jaw_gripper_)
break;
}
}
- if (gripper_joint_indexes.empty())
+ if (gripper_joint_indices.empty())
{
RCLCPP_WARN(logger_, "No command_joint was specified for the MoveIt controller gripper handle. \
- Please see GripperControllerHandle::addCommandJoint() and \
- GripperControllerHandle::setCommandJoint(). Assuming index 0.");
- gripper_joint_indexes.push_back(0);
+ Please see GripperCommandControllerHandle::addCommandJoint() and \
+ GripperCommandControllerHandle::setCommandJoint(). Assuming index 0.");
+ gripper_joint_indices.push_back(0);
}
// goal to be sent
@@ -126,7 +126,7 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
RCLCPP_DEBUG(logger_, "Sending command from trajectory point %d", tpoint);
// fill in goal from last point
- for (std::size_t idx : gripper_joint_indexes)
+ for (std::size_t idx : gripper_joint_indices)
{
if (trajectory.joint_trajectory.points[tpoint].positions.size() <= idx)
{
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 761eaf3ef9..da78a57dfc 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
@@ -48,5 +48,5 @@
/* Author: Michael Ferguson, Ioan Sucan, E. Gil Jones */
#pragma once
-#pragma message(".h header is obsolete. Please use the .hpp header instead.")
-#include <moveit_simple_controller_manager/gripper_controller_handle.hpp>
+#pragma message(".h header is obsolete. Please use the gripper_command_controller_handle.hpp header instead.")
+#include <moveit_simple_controller_manager/gripper_command_controller_handle.hpp>
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
new file mode 100644
index 0000000000..4df57ecacc
--- /dev/null
+++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/parallel_gripper_command_controller_handle.hpp
@@ -0,0 +1,178 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2025, Marq Rasmussen
+ * 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 Marq Rasmussen 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.
+ *********************************************************************/
+/* Design inspired by gripper_command_controller_handle.hpp */
+/* Author: Marq Rasmussen */
+
+#pragma once
+
+#include <moveit_simple_controller_manager/action_based_controller_handle.hpp>
+#include <control_msgs/action/parallel_gripper_command.hpp>
+#include <set>
+
+namespace moveit_simple_controller_manager
+{
+/*
+ * This is an interface for a gripper using the control_msgs/ParallelGripperCommand action interface.
+ */
+class ParallelGripperCommandControllerHandle
+ : public ActionBasedControllerHandle<control_msgs::action::ParallelGripperCommand>
+{
+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)
+ : ActionBasedControllerHandle<control_msgs::action::ParallelGripperCommand>(
+ node, name, ns, "moveit.simple_controller_manager.parallel_gripper_controller_handle")
+ , max_effort_(max_effort)
+ , max_velocity_(max_velocity)
+ {
+ }
+
+ bool sendTrajectory(const moveit_msgs::msg::RobotTrajectory& trajectory) override
+ {
+ RCLCPP_DEBUG_STREAM(logger_, "Received new trajectory for " << name_);
+
+ if (!controller_action_client_)
+ return false;
+
+ if (!isConnected())
+ {
+ RCLCPP_ERROR_STREAM(logger_, "Action client not connected to action server: " << getActionName());
+ return false;
+ }
+
+ if (!trajectory.multi_dof_joint_trajectory.points.empty())
+ {
+ RCLCPP_ERROR(logger_, "%s cannot execute multi-dof trajectories.", name_.c_str());
+ return false;
+ }
+
+ if (trajectory.joint_trajectory.points.empty())
+ {
+ RCLCPP_ERROR(logger_, "%s requires at least one joint trajectory point, but none received.", name_.c_str());
+ return false;
+ }
+
+ if (trajectory.joint_trajectory.joint_names.empty())
+ {
+ RCLCPP_ERROR(logger_, "%s received a trajectory with no joint names specified.", name_.c_str());
+ return false;
+ }
+
+ // goal to be sent
+ control_msgs::action::ParallelGripperCommand::Goal goal;
+ auto& cmd_state = goal.command;
+
+ auto& joint_names = trajectory.joint_trajectory.joint_names;
+ auto it = std::find(joint_names.begin(), joint_names.end(), command_joint_);
+ if (it != joint_names.end())
+ {
+ cmd_state.name.push_back(command_joint_);
+ std::size_t gripper_joint_index = std::distance(joint_names.begin(), it);
+ // send last trajectory point
+ if (trajectory.joint_trajectory.points.back().positions.size() <= gripper_joint_index)
+ {
+ 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[gripper_joint_index].c_str());
+ return false;
+ }
+ cmd_state.position.push_back(trajectory.joint_trajectory.points.back().positions[gripper_joint_index]);
+ // 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_);
+ }
+ }
+ else
+ {
+ RCLCPP_ERROR(logger_, "Received trajectory does not include a command for joint name %s.", command_joint_.c_str());
+ return false;
+ }
+
+ rclcpp_action::Client<control_msgs::action::ParallelGripperCommand>::SendGoalOptions send_goal_options;
+ // Active callback
+ send_goal_options.goal_response_callback =
+ [this](const rclcpp_action::Client<control_msgs::action::ParallelGripperCommand>::GoalHandle::SharedPtr&
+ /* unused-arg */) { RCLCPP_DEBUG_STREAM(logger_, name_ << " started execution."); };
+ // Send goal
+ auto current_goal_future = controller_action_client_->async_send_goal(goal, send_goal_options);
+ current_goal_ = current_goal_future.get();
+ if (!current_goal_)
+ {
+ RCLCPP_ERROR(logger_, "%s goal was rejected by server.", name_.c_str());
+ return false;
+ }
+
+ done_ = false;
+ last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING;
+ return true;
+ }
+
+ void setCommandJoint(const std::string& name)
+ {
+ command_joint_ = name;
+ }
+
+private:
+ void controllerDoneCallback(
+ const rclcpp_action::ClientGoalHandle<control_msgs::action::ParallelGripperCommand>::WrappedResult& wrapped_result)
+ override
+ {
+ finishControllerExecution(wrapped_result.code);
+ }
+
+ /*
+ * The ``max_effort`` used in the ParallelGripperCommand message.
+ */
+ double max_effort_ = 0.0;
+
+ /*
+ * The ``max_velocity_`` used in the ParallelGripperCommand message.
+ */
+ double max_velocity_ = 0.0;
+
+ /*
+ * The joint to command in the ParallelGripperCommand message
+ */
+ std::string command_joint_;
+}; // namespace moveit_simple_controller_manager
+
+} // end namespace moveit_simple_controller_manager
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 394765c8cb..b38c84a596 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
@@ -36,7 +36,8 @@
/* Author: Michael Ferguson, Ioan Sucan, E. Gil Jones */
#include <moveit_simple_controller_manager/action_based_controller_handle.hpp>
-#include <moveit_simple_controller_manager/gripper_controller_handle.hpp>
+#include <moveit_simple_controller_manager/gripper_command_controller_handle.hpp>
+#include <moveit_simple_controller_manager/parallel_gripper_command_controller_handle.hpp>
#include <moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.hpp>
#include <boost/algorithm/string/join.hpp>
#include <pluginlib/class_list_macros.hpp>
@@ -147,14 +148,10 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo
if (type == "GripperCommand")
{
double max_effort;
- 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(), "Max effort set to 0.0");
- max_effort = 0.0;
- }
+ node_->get_parameter_or(makeParameterName(PARAM_BASE_NAME, controller_name, "max_effort"), max_effort, 0.0);
+ RCLCPP_INFO_STREAM(getLogger(), controller_name << " will command a max effort of: " << max_effort);
- new_handle = std::make_shared<GripperControllerHandle>(node_, controller_name, action_ns, max_effort);
+ new_handle = std::make_shared<GripperCommandControllerHandle>(node_, controller_name, action_ns, max_effort);
bool parallel_gripper = false;
if (node_->get_parameter(makeParameterName(PARAM_BASE_NAME, controller_name, "parallel"), parallel_gripper) &&
parallel_gripper)
@@ -165,27 +162,57 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo
<< controller_joints.size() << " are specified");
continue;
}
- static_cast<GripperControllerHandle*>(new_handle.get())
+ static_cast<GripperCommandControllerHandle*>(new_handle.get())
->setParallelJawGripper(controller_joints[0], controller_joints[1]);
}
else
{
std::string command_joint;
- if (!node_->get_parameter(makeParameterName(PARAM_BASE_NAME, controller_name, "command_joint"),
- command_joint))
- command_joint = controller_joints[0];
+ node_->get_parameter_or(makeParameterName(PARAM_BASE_NAME, controller_name, "command_joint"), command_joint,
+ controller_joints[0]);
- static_cast<GripperControllerHandle*>(new_handle.get())->setCommandJoint(command_joint);
+ static_cast<GripperCommandControllerHandle*>(new_handle.get())->setCommandJoint(command_joint);
}
bool allow_failure;
node_->get_parameter_or(makeParameterName(PARAM_BASE_NAME, controller_name, "allow_failure"), allow_failure,
false);
- static_cast<GripperControllerHandle*>(new_handle.get())->allowFailure(allow_failure);
+ static_cast<GripperCommandControllerHandle*>(new_handle.get())->allowFailure(allow_failure);
RCLCPP_INFO_STREAM(getLogger(), "Added GripperCommand controller for " << controller_name);
controllers_[controller_name] = new_handle;
}
+ else if (type == "ParallelGripperCommand")
+ {
+ double max_effort;
+ node_->get_parameter_or(makeParameterName(PARAM_BASE_NAME, controller_name, "max_effort"), max_effort, 0.0);
+ if (max_effort > 0.0)
+ {
+ RCLCPP_INFO_STREAM(getLogger(), controller_name << " will command a max effort of: " << max_effort);
+ }
+
+ double max_velocity;
+ node_->get_parameter_or(makeParameterName(PARAM_BASE_NAME, controller_name, "max_velocity"), max_velocity,
+ 0.0);
+ if (max_effort > 0.0)
+ {
+ RCLCPP_INFO_STREAM(getLogger(), controller_name << " will command a max velocity of: " << max_velocity);
+ }
+
+ new_handle = std::make_shared<ParallelGripperCommandControllerHandle>(node_, controller_name, action_ns,
+ max_effort, max_velocity);
+
+ if (controller_joints.size() > 1)
+ {
+ RCLCPP_WARN_STREAM(getLogger(), "ParallelGripperCommand controller only supports commanding a single joint "
+ "and multiple joint names were specified. Assuming control of joint: "
+ << controller_joints[0]);
+ }
+ static_cast<ParallelGripperCommandControllerHandle*>(new_handle.get())->setCommandJoint(controller_joints[0]);
+
+ RCLCPP_INFO_STREAM(getLogger(), "Added ParallelGripperCommand controller for " << controller_name);
+ controllers_[controller_name] = new_handle;
+ }
else if (type == "FollowJointTrajectory")
{
new_handle = std::make_shared<FollowJointTrajectoryControllerHandle>(node_, controller_name, action_ns);