Skip to content

Commit

Permalink
only use max velocity/effor when specified
Browse files Browse the repository at this point in the history
  • Loading branch information
@MarqRazz
MarqRazz committed Jan 15, 2025
1 parent 46769b9 commit 34c88fe
Show file tree
Hide file tree
Showing 4 changed files with 32 additions and 13 deletions.
3 changes: 2 additions & 1 deletion ...t_plugins/moveit_ros_control_interface/src/parallel_gripper_command_controller_plugin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,8 @@ class ParallelGripperCommandControllerAllocator : public ControllerHandleAllocat
const std::string& name,
const std::vector<std::string>& /* resources */) override
{
return std::make_shared<moveit_simple_controller_manager::ParallelGripperCommandControllerHandle>(node, name, "gripper_cmd");
return std::make_shared<moveit_simple_controller_manager::ParallelGripperCommandControllerHandle>(node, name,
"gripper_cmd");
}
};

Expand Down
2 changes: 1 addition & 1 deletion ...er_manager/include/moveit_simple_controller_manager/gripper_command_controller_handle.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ class GripperCommandControllerHandle : public ActionBasedControllerHandle<contro
public:
/* Topics will map to name/ns/goal, name/ns/result, etc */
GripperCommandControllerHandle(const rclcpp::Node::SharedPtr& node, const std::string& name, const std::string& ns,
const double max_effort = 0.0)
const double max_effort = 0.0)
: ActionBasedControllerHandle<control_msgs::action::GripperCommand>(
node, name, ns, "moveit.simple_controller_manager.gripper_controller_handle")
, allow_failure_(false)
Expand Down
26 changes: 18 additions & 8 deletions ...r/include/moveit_simple_controller_manager/parallel_gripper_command_controller_handle.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<control_msgs::action::ParallelGripperCommand>
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)
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")
, allow_stalling_(false)
Expand Down Expand Up @@ -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<std::size_t> gripper_joint_indexes;
for (std::size_t i = 0; i < trajectory.joint_trajectory.joint_names.size(); ++i)
Expand Down Expand Up @@ -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<control_msgs::action::ParallelGripperCommand>::SendGoalOptions send_goal_options;
// Active callback
Expand Down Expand Up @@ -167,7 +176,8 @@ class ParallelGripperCommandControllerHandle : public ActionBasedControllerHandl

private:
void controllerDoneCallback(
const rclcpp_action::ClientGoalHandle<control_msgs::action::ParallelGripperCommand>::WrappedResult& wrapped_result) override
const rclcpp_action::ClientGoalHandle<control_msgs::action::ParallelGripperCommand>::WrappedResult& wrapped_result)
override
{
if (wrapped_result.code == rclcpp_action::ResultCode::ABORTED && wrapped_result.result->stalled && allow_stalling_)
{
Expand Down
14 changes: 11 additions & 3 deletions moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<ParallelGripperCommandControllerHandle>(node_, controller_name, action_ns,
max_effort, max_velocity);
Expand All @@ -216,7 +224,7 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo
false);
static_cast<GripperCommandControllerHandle*>(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")
Expand Down

0 comments on commit 34c88fe

Please sign in to comment.