Skip to content

Commit 660175c

Browse files
authored
Fix Servo JointJog Crash (#3351)
1 parent 5946a4e commit 660175c

File tree

3 files changed

+131
-105
lines changed

3 files changed

+131
-105
lines changed

moveit_ros/moveit_servo/src/servo_node.cpp

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -218,12 +218,23 @@ std::optional<KinematicState> ServoNode::processJointJogCommand(const moveit::co
218218
// Reject any other command types that had arrived simultaneously.
219219
new_twist_msg_ = new_pose_msg_ = false;
220220

221+
if (!latest_joint_jog_.displacements.empty())
222+
{
223+
RCLCPP_WARN(node_->get_logger(), "Joint jog command displacements field is not yet supported, ignoring.");
224+
latest_joint_jog_.displacements.clear(); // Only warn once per message.
225+
}
226+
221227
const bool command_stale = (node_->now() - latest_joint_jog_.header.stamp) >=
222228
rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout);
223229
if (!command_stale)
224230
{
225231
JointJogCommand command{ latest_joint_jog_.joint_names, latest_joint_jog_.velocities };
226232
next_joint_state = servo_->getNextJointState(robot_state, command);
233+
// If the command failed, stop trying to process this message
234+
if (servo_->getStatus() == StatusCode::INVALID)
235+
{
236+
new_joint_jog_msg_ = false;
237+
}
227238
}
228239
else
229240
{
@@ -256,6 +267,10 @@ std::optional<KinematicState> ServoNode::processTwistCommand(const moveit::core:
256267
latest_twist_.twist.angular.y, latest_twist_.twist.angular.z };
257268
const TwistCommand command{ latest_twist_.header.frame_id, velocities };
258269
next_joint_state = servo_->getNextJointState(robot_state, command);
270+
if (servo_->getStatus() == StatusCode::INVALID)
271+
{
272+
new_twist_msg_ = false;
273+
}
259274
}
260275
else
261276
{
@@ -285,6 +300,10 @@ std::optional<KinematicState> ServoNode::processPoseCommand(const moveit::core::
285300
{
286301
const PoseCommand command = poseFromPoseStamped(latest_pose_);
287302
next_joint_state = servo_->getNextJointState(robot_state, command);
303+
if (servo_->getStatus() == StatusCode::INVALID)
304+
{
305+
new_pose_msg_ = false;
306+
}
288307
}
289308
else
290309
{

moveit_ros/moveit_servo/src/utils/command.cpp

Lines changed: 93 additions & 105 deletions
Original file line numberDiff line numberDiff line change
@@ -84,7 +84,6 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo
8484
const JointNameToMoveGroupIndexMap& joint_name_group_index_map)
8585
{
8686
// Find the target joint position based on the commanded joint velocity
87-
StatusCode status = StatusCode::NO_WARNING;
8887
const auto& group_name =
8988
servo_params.active_subgroup.empty() ? servo_params.move_group_name : servo_params.active_subgroup;
9089
const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(group_name);
@@ -93,7 +92,14 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo
9392
Eigen::VectorXd velocities(joint_names.size());
9493

9594
velocities.setZero();
96-
bool names_valid = true;
95+
if (command.velocities.size() != command.names.size())
96+
{
97+
RCLCPP_WARN_STREAM(getLogger(), "Invalid joint jog command. Each joint name must have one corresponding "
98+
"velocity command. Received "
99+
<< command.names.size() << " joints with " << command.velocities.size()
100+
<< " commands.");
101+
return std::make_pair(StatusCode::INVALID, joint_position_delta);
102+
}
97103

98104
for (size_t i = 0; i < command.names.size(); ++i)
99105
{
@@ -104,44 +110,34 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo
104110
}
105111
else
106112
{
107-
RCLCPP_WARN_STREAM(getLogger(), "Invalid joint name: " << command.names[i]);
108-
109-
names_valid = false;
110-
break;
113+
RCLCPP_WARN_STREAM(getLogger(), "Invalid joint name: " << command.names[i]
114+
<< "Either you're sending commands for a joint "
115+
"that is not part of the move group or certain joints "
116+
"cannot be moved because a "
117+
"subgroup is active and they are not part of it.");
118+
return std::make_pair(StatusCode::INVALID, joint_position_delta);
111119
}
112120
}
113-
const bool velocity_valid = isValidCommand(velocities);
114-
if (names_valid && velocity_valid)
121+
122+
if (!isValidCommand(velocities))
115123
{
116-
joint_position_delta = velocities * servo_params.publish_period;
117-
if (servo_params.command_in_type == "unitless")
118-
{
119-
joint_position_delta *= servo_params.scale.joint;
120-
}
124+
RCLCPP_WARN_STREAM(getLogger(), "Invalid velocity values in joint jog command");
125+
return std::make_pair(StatusCode::INVALID, joint_position_delta);
121126
}
122-
else
127+
128+
joint_position_delta = velocities * servo_params.publish_period;
129+
if (servo_params.command_in_type == "unitless")
123130
{
124-
status = StatusCode::INVALID;
125-
if (!names_valid)
126-
{
127-
RCLCPP_WARN_STREAM(getLogger(),
128-
"Invalid joint names in joint jog command. Either you're sending commands for a joint "
129-
"that is not part of the move group or certain joints cannot be moved because a "
130-
"subgroup is active and they are not part of it.");
131-
}
132-
if (!velocity_valid)
133-
{
134-
RCLCPP_WARN_STREAM(getLogger(), "Invalid velocity values in joint jog command");
135-
}
131+
joint_position_delta *= servo_params.scale.joint;
136132
}
137133

138134
if (!servo_params.active_subgroup.empty() && servo_params.active_subgroup != servo_params.move_group_name)
139135
{
140-
return std::make_pair(status, createMoveGroupDelta(joint_position_delta, robot_state, servo_params,
141-
joint_name_group_index_map));
136+
return std::make_pair(StatusCode::NO_WARNING, createMoveGroupDelta(joint_position_delta, robot_state, servo_params,
137+
joint_name_group_index_map));
142138
}
143139

144-
return std::make_pair(status, joint_position_delta);
140+
return std::make_pair(StatusCode::NO_WARNING, joint_position_delta);
145141
}
146142

147143
JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::core::RobotStatePtr& robot_state,
@@ -154,10 +150,23 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::
154150
Eigen::VectorXd joint_position_delta(num_joints);
155151
Eigen::Vector<double, 6> cartesian_position_delta;
156152

157-
const bool valid_command = isValidCommand(command);
158-
const bool is_planning_frame = (command.frame_id == planning_frame);
159-
const bool is_zero = command.velocities.isZero();
160-
if (!is_zero && is_planning_frame && valid_command)
153+
if (command.frame_id != planning_frame)
154+
{
155+
RCLCPP_WARN_STREAM(getLogger(), "Command frame is: " << command.frame_id << ", expected: " << planning_frame);
156+
return std::make_pair(StatusCode::INVALID, joint_position_delta);
157+
}
158+
159+
if (!isValidCommand(command))
160+
{
161+
RCLCPP_WARN_STREAM(getLogger(), "Invalid twist command.");
162+
return std::make_pair(StatusCode::INVALID, joint_position_delta);
163+
}
164+
165+
if (command.velocities.isZero())
166+
{
167+
joint_position_delta.setZero();
168+
}
169+
else
161170
{
162171
// Compute the Cartesian position delta based on incoming twist command.
163172
cartesian_position_delta = command.velocities * servo_params.publish_period;
@@ -208,22 +217,7 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::
208217
}
209218
}
210219
}
211-
else if (is_zero)
212-
{
213-
joint_position_delta.setZero();
214-
}
215-
else
216-
{
217-
status = StatusCode::INVALID;
218-
if (!valid_command)
219-
{
220-
RCLCPP_ERROR_STREAM(getLogger(), "Invalid twist command.");
221-
}
222-
if (!is_planning_frame)
223-
{
224-
RCLCPP_ERROR_STREAM(getLogger(), "Command frame is: " << command.frame_id << ", expected: " << planning_frame);
225-
}
226-
}
220+
227221
return std::make_pair(status, joint_position_delta);
228222
}
229223

@@ -237,74 +231,68 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co
237231
robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames().size();
238232
Eigen::VectorXd joint_position_delta(num_joints);
239233

240-
const bool valid_command = isValidCommand(command);
241-
const bool is_planning_frame = (command.frame_id == planning_frame);
242-
243-
if (valid_command && is_planning_frame)
234+
if (!isValidCommand(command))
244235
{
245-
Eigen::Vector<double, 6> cartesian_position_delta;
236+
RCLCPP_WARN_STREAM(getLogger(), "Invalid pose command.");
237+
return std::make_pair(StatusCode::INVALID, joint_position_delta);
238+
}
246239

247-
// Compute linear and angular change needed.
248-
const Eigen::Isometry3d ee_pose{ robot_state->getGlobalLinkTransform(planning_frame).inverse() *
249-
robot_state->getGlobalLinkTransform(ee_frame) };
250-
const Eigen::Quaterniond q_current(ee_pose.rotation());
251-
Eigen::Quaterniond q_target(command.pose.rotation());
252-
Eigen::Vector3d translation_error = command.pose.translation() - ee_pose.translation();
240+
if (command.frame_id != planning_frame)
241+
{
242+
RCLCPP_WARN_STREAM(getLogger(), "Command frame is: " << command.frame_id << " expected: " << planning_frame);
243+
return std::make_pair(StatusCode::INVALID, joint_position_delta);
244+
}
253245

254-
// Limit the commands by the maximum linear and angular speeds provided.
255-
if (servo_params.scale.linear > 0.0)
256-
{
257-
const auto linear_speed_scale =
258-
(translation_error.norm() / servo_params.publish_period) / servo_params.scale.linear;
259-
if (linear_speed_scale > 1.0)
260-
{
261-
translation_error /= linear_speed_scale;
262-
}
263-
}
264-
if (servo_params.scale.rotational > 0.0)
265-
{
266-
const auto angular_speed_scale =
267-
(std::abs(q_target.angularDistance(q_current)) / servo_params.publish_period) / servo_params.scale.rotational;
268-
if (angular_speed_scale > 1.0)
269-
{
270-
q_target = q_current.slerp(1.0 / angular_speed_scale, q_target);
271-
}
272-
}
246+
Eigen::Vector<double, 6> cartesian_position_delta;
273247

274-
// Compute the Cartesian deltas from the velocity-scaled values.
275-
const auto angle_axis_error = Eigen::AngleAxisd(q_target * q_current.inverse());
276-
cartesian_position_delta.head<3>() = translation_error;
277-
cartesian_position_delta.tail<3>() = angle_axis_error.axis() * angle_axis_error.angle();
248+
// Compute linear and angular change needed.
249+
const Eigen::Isometry3d ee_pose{ robot_state->getGlobalLinkTransform(planning_frame).inverse() *
250+
robot_state->getGlobalLinkTransform(ee_frame) };
251+
const Eigen::Quaterniond q_current(ee_pose.rotation());
252+
Eigen::Quaterniond q_target(command.pose.rotation());
253+
Eigen::Vector3d translation_error = command.pose.translation() - ee_pose.translation();
278254

279-
// Compute the required change in joint angles.
280-
const auto delta_result =
281-
jointDeltaFromIK(cartesian_position_delta, robot_state, servo_params, joint_name_group_index_map);
282-
status = delta_result.first;
283-
if (status != StatusCode::INVALID)
255+
// Limit the commands by the maximum linear and angular speeds provided.
256+
if (servo_params.scale.linear > 0.0)
257+
{
258+
const auto linear_speed_scale =
259+
(translation_error.norm() / servo_params.publish_period) / servo_params.scale.linear;
260+
if (linear_speed_scale > 1.0)
284261
{
285-
joint_position_delta = delta_result.second;
286-
// Get velocity scaling information for singularity.
287-
const auto singularity_scaling_info =
288-
velocityScalingFactorForSingularity(robot_state, cartesian_position_delta, servo_params);
289-
// Apply velocity scaling for singularity, if there was any scaling.
290-
if (singularity_scaling_info.second != StatusCode::NO_WARNING)
291-
{
292-
status = singularity_scaling_info.second;
293-
RCLCPP_WARN_STREAM(getLogger(), SERVO_STATUS_CODE_MAP.at(status));
294-
joint_position_delta *= singularity_scaling_info.first;
295-
}
262+
translation_error /= linear_speed_scale;
296263
}
297264
}
298-
else
265+
if (servo_params.scale.rotational > 0.0)
299266
{
300-
status = StatusCode::INVALID;
301-
if (!valid_command)
267+
const auto angular_speed_scale =
268+
(std::abs(q_target.angularDistance(q_current)) / servo_params.publish_period) / servo_params.scale.rotational;
269+
if (angular_speed_scale > 1.0)
302270
{
303-
RCLCPP_WARN_STREAM(getLogger(), "Invalid pose command.");
271+
q_target = q_current.slerp(1.0 / angular_speed_scale, q_target);
304272
}
305-
if (!is_planning_frame)
273+
}
274+
275+
// Compute the Cartesian deltas from the velocity-scaled values.
276+
const auto angle_axis_error = Eigen::AngleAxisd(q_target * q_current.inverse());
277+
cartesian_position_delta.head<3>() = translation_error;
278+
cartesian_position_delta.tail<3>() = angle_axis_error.axis() * angle_axis_error.angle();
279+
280+
// Compute the required change in joint angles.
281+
const auto delta_result =
282+
jointDeltaFromIK(cartesian_position_delta, robot_state, servo_params, joint_name_group_index_map);
283+
status = delta_result.first;
284+
if (status != StatusCode::INVALID)
285+
{
286+
joint_position_delta = delta_result.second;
287+
// Get velocity scaling information for singularity.
288+
const auto singularity_scaling_info =
289+
velocityScalingFactorForSingularity(robot_state, cartesian_position_delta, servo_params);
290+
// Apply velocity scaling for singularity, if there was any scaling.
291+
if (singularity_scaling_info.second != StatusCode::NO_WARNING)
306292
{
307-
RCLCPP_WARN_STREAM(getLogger(), "Command frame is: " << command.frame_id << " expected: " << planning_frame);
293+
status = singularity_scaling_info.second;
294+
RCLCPP_WARN_STREAM(getLogger(), SERVO_STATUS_CODE_MAP.at(status));
295+
joint_position_delta *= singularity_scaling_info.first;
308296
}
309297
}
310298
return std::make_pair(status, joint_position_delta);

moveit_ros/moveit_servo/tests/test_ros_integration.cpp

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -106,6 +106,25 @@ TEST_F(ServoRosFixture, testJointJog)
106106
moveit_servo::StatusCode status = status_;
107107
RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "Status after jointjog: " << static_cast<size_t>(status));
108108
ASSERT_EQ(status_, moveit_servo::StatusCode::NO_WARNING);
109+
110+
// Ensure error when number of commands doesn't match number of joints
111+
int traj_count_before = traj_count_;
112+
jog_cmd.velocities.pop_back();
113+
jog_cmd.header.stamp = servo_test_node_->now();
114+
joint_jog_publisher->publish(jog_cmd);
115+
rclcpp::sleep_for(std::chrono::milliseconds(100));
116+
ASSERT_EQ(status_, moveit_servo::StatusCode::INVALID);
117+
jog_cmd.velocities.push_back(1.0);
118+
jog_cmd.velocities.push_back(1.0);
119+
jog_cmd.header.stamp = servo_test_node_->now();
120+
joint_jog_publisher->publish(jog_cmd);
121+
rclcpp::sleep_for(std::chrono::milliseconds(100));
122+
ASSERT_EQ(status_, moveit_servo::StatusCode::INVALID);
123+
124+
// No additional trajectories were generated with the invalid commands
125+
ASSERT_EQ(traj_count_, traj_count_before);
126+
status = status_;
127+
RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "Status after invalid jointjog: " << static_cast<size_t>(status));
109128
}
110129

111130
TEST_F(ServoRosFixture, testTwist)

0 commit comments

Comments
 (0)