diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 609d1e6585..7b842c8ca3 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -218,12 +218,23 @@ std::optional ServoNode::processJointJogCommand(const moveit::co // Reject any other command types that had arrived simultaneously. new_twist_msg_ = new_pose_msg_ = false; + if (!latest_joint_jog_.displacements.empty()) + { + RCLCPP_WARN(node_->get_logger(), "Joint jog command displacements field is not yet supported, ignoring."); + latest_joint_jog_.displacements.clear(); // Only warn once per message. + } + const bool command_stale = (node_->now() - latest_joint_jog_.header.stamp) >= rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout); if (!command_stale) { JointJogCommand command{ latest_joint_jog_.joint_names, latest_joint_jog_.velocities }; next_joint_state = servo_->getNextJointState(robot_state, command); + // If the command failed, stop trying to process this message + if (servo_->getStatus() == StatusCode::INVALID) + { + new_joint_jog_msg_ = false; + } } else { @@ -256,6 +267,10 @@ std::optional ServoNode::processTwistCommand(const moveit::core: latest_twist_.twist.angular.y, latest_twist_.twist.angular.z }; const TwistCommand command{ latest_twist_.header.frame_id, velocities }; next_joint_state = servo_->getNextJointState(robot_state, command); + if (servo_->getStatus() == StatusCode::INVALID) + { + new_twist_msg_ = false; + } } else { @@ -285,6 +300,10 @@ std::optional ServoNode::processPoseCommand(const moveit::core:: { const PoseCommand command = poseFromPoseStamped(latest_pose_); next_joint_state = servo_->getNextJointState(robot_state, command); + if (servo_->getStatus() == StatusCode::INVALID) + { + new_pose_msg_ = false; + } } else { diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index 0470edebe8..6005ddd1be 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -84,7 +84,6 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo const JointNameToMoveGroupIndexMap& joint_name_group_index_map) { // Find the target joint position based on the commanded joint velocity - StatusCode status = StatusCode::NO_WARNING; const auto& group_name = servo_params.active_subgroup.empty() ? servo_params.move_group_name : servo_params.active_subgroup; const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(group_name); @@ -93,7 +92,14 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo Eigen::VectorXd velocities(joint_names.size()); velocities.setZero(); - bool names_valid = true; + if (command.velocities.size() != command.names.size()) + { + RCLCPP_WARN_STREAM(getLogger(), "Invalid joint jog command. Each joint name must have one corresponding " + "velocity command. Received " + << command.names.size() << " joints with " << command.velocities.size() + << " commands."); + return std::make_pair(StatusCode::INVALID, joint_position_delta); + } for (size_t i = 0; i < command.names.size(); ++i) { @@ -104,44 +110,34 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo } else { - RCLCPP_WARN_STREAM(getLogger(), "Invalid joint name: " << command.names[i]); - - names_valid = false; - break; + RCLCPP_WARN_STREAM(getLogger(), "Invalid joint name: " << command.names[i] + << "Either you're sending commands for a joint " + "that is not part of the move group or certain joints " + "cannot be moved because a " + "subgroup is active and they are not part of it."); + return std::make_pair(StatusCode::INVALID, joint_position_delta); } } - const bool velocity_valid = isValidCommand(velocities); - if (names_valid && velocity_valid) + + if (!isValidCommand(velocities)) { - joint_position_delta = velocities * servo_params.publish_period; - if (servo_params.command_in_type == "unitless") - { - joint_position_delta *= servo_params.scale.joint; - } + RCLCPP_WARN_STREAM(getLogger(), "Invalid velocity values in joint jog command"); + return std::make_pair(StatusCode::INVALID, joint_position_delta); } - else + + joint_position_delta = velocities * servo_params.publish_period; + if (servo_params.command_in_type == "unitless") { - status = StatusCode::INVALID; - if (!names_valid) - { - RCLCPP_WARN_STREAM(getLogger(), - "Invalid joint names in joint jog command. Either you're sending commands for a joint " - "that is not part of the move group or certain joints cannot be moved because a " - "subgroup is active and they are not part of it."); - } - if (!velocity_valid) - { - RCLCPP_WARN_STREAM(getLogger(), "Invalid velocity values in joint jog command"); - } + joint_position_delta *= servo_params.scale.joint; } if (!servo_params.active_subgroup.empty() && servo_params.active_subgroup != servo_params.move_group_name) { - return std::make_pair(status, createMoveGroupDelta(joint_position_delta, robot_state, servo_params, - joint_name_group_index_map)); + return std::make_pair(StatusCode::NO_WARNING, createMoveGroupDelta(joint_position_delta, robot_state, servo_params, + joint_name_group_index_map)); } - return std::make_pair(status, joint_position_delta); + return std::make_pair(StatusCode::NO_WARNING, joint_position_delta); } JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::core::RobotStatePtr& robot_state, @@ -154,10 +150,23 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit:: Eigen::VectorXd joint_position_delta(num_joints); Eigen::Vector cartesian_position_delta; - const bool valid_command = isValidCommand(command); - const bool is_planning_frame = (command.frame_id == planning_frame); - const bool is_zero = command.velocities.isZero(); - if (!is_zero && is_planning_frame && valid_command) + if (command.frame_id != planning_frame) + { + RCLCPP_WARN_STREAM(getLogger(), "Command frame is: " << command.frame_id << ", expected: " << planning_frame); + return std::make_pair(StatusCode::INVALID, joint_position_delta); + } + + if (!isValidCommand(command)) + { + RCLCPP_WARN_STREAM(getLogger(), "Invalid twist command."); + return std::make_pair(StatusCode::INVALID, joint_position_delta); + } + + if (command.velocities.isZero()) + { + joint_position_delta.setZero(); + } + else { // Compute the Cartesian position delta based on incoming twist command. cartesian_position_delta = command.velocities * servo_params.publish_period; @@ -208,22 +217,7 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit:: } } } - else if (is_zero) - { - joint_position_delta.setZero(); - } - else - { - status = StatusCode::INVALID; - if (!valid_command) - { - RCLCPP_ERROR_STREAM(getLogger(), "Invalid twist command."); - } - if (!is_planning_frame) - { - RCLCPP_ERROR_STREAM(getLogger(), "Command frame is: " << command.frame_id << ", expected: " << planning_frame); - } - } + return std::make_pair(status, joint_position_delta); } @@ -237,74 +231,68 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames().size(); Eigen::VectorXd joint_position_delta(num_joints); - const bool valid_command = isValidCommand(command); - const bool is_planning_frame = (command.frame_id == planning_frame); - - if (valid_command && is_planning_frame) + if (!isValidCommand(command)) { - Eigen::Vector cartesian_position_delta; + RCLCPP_WARN_STREAM(getLogger(), "Invalid pose command."); + return std::make_pair(StatusCode::INVALID, joint_position_delta); + } - // Compute linear and angular change needed. - const Eigen::Isometry3d ee_pose{ robot_state->getGlobalLinkTransform(planning_frame).inverse() * - robot_state->getGlobalLinkTransform(ee_frame) }; - const Eigen::Quaterniond q_current(ee_pose.rotation()); - Eigen::Quaterniond q_target(command.pose.rotation()); - Eigen::Vector3d translation_error = command.pose.translation() - ee_pose.translation(); + if (command.frame_id != planning_frame) + { + RCLCPP_WARN_STREAM(getLogger(), "Command frame is: " << command.frame_id << " expected: " << planning_frame); + return std::make_pair(StatusCode::INVALID, joint_position_delta); + } - // Limit the commands by the maximum linear and angular speeds provided. - if (servo_params.scale.linear > 0.0) - { - const auto linear_speed_scale = - (translation_error.norm() / servo_params.publish_period) / servo_params.scale.linear; - if (linear_speed_scale > 1.0) - { - translation_error /= linear_speed_scale; - } - } - if (servo_params.scale.rotational > 0.0) - { - const auto angular_speed_scale = - (std::abs(q_target.angularDistance(q_current)) / servo_params.publish_period) / servo_params.scale.rotational; - if (angular_speed_scale > 1.0) - { - q_target = q_current.slerp(1.0 / angular_speed_scale, q_target); - } - } + Eigen::Vector cartesian_position_delta; - // Compute the Cartesian deltas from the velocity-scaled values. - const auto angle_axis_error = Eigen::AngleAxisd(q_target * q_current.inverse()); - cartesian_position_delta.head<3>() = translation_error; - cartesian_position_delta.tail<3>() = angle_axis_error.axis() * angle_axis_error.angle(); + // Compute linear and angular change needed. + const Eigen::Isometry3d ee_pose{ robot_state->getGlobalLinkTransform(planning_frame).inverse() * + robot_state->getGlobalLinkTransform(ee_frame) }; + const Eigen::Quaterniond q_current(ee_pose.rotation()); + Eigen::Quaterniond q_target(command.pose.rotation()); + Eigen::Vector3d translation_error = command.pose.translation() - ee_pose.translation(); - // Compute the required change in joint angles. - const auto delta_result = - jointDeltaFromIK(cartesian_position_delta, robot_state, servo_params, joint_name_group_index_map); - status = delta_result.first; - if (status != StatusCode::INVALID) + // Limit the commands by the maximum linear and angular speeds provided. + if (servo_params.scale.linear > 0.0) + { + const auto linear_speed_scale = + (translation_error.norm() / servo_params.publish_period) / servo_params.scale.linear; + if (linear_speed_scale > 1.0) { - joint_position_delta = delta_result.second; - // Get velocity scaling information for singularity. - const auto singularity_scaling_info = - velocityScalingFactorForSingularity(robot_state, cartesian_position_delta, servo_params); - // Apply velocity scaling for singularity, if there was any scaling. - if (singularity_scaling_info.second != StatusCode::NO_WARNING) - { - status = singularity_scaling_info.second; - RCLCPP_WARN_STREAM(getLogger(), SERVO_STATUS_CODE_MAP.at(status)); - joint_position_delta *= singularity_scaling_info.first; - } + translation_error /= linear_speed_scale; } } - else + if (servo_params.scale.rotational > 0.0) { - status = StatusCode::INVALID; - if (!valid_command) + const auto angular_speed_scale = + (std::abs(q_target.angularDistance(q_current)) / servo_params.publish_period) / servo_params.scale.rotational; + if (angular_speed_scale > 1.0) { - RCLCPP_WARN_STREAM(getLogger(), "Invalid pose command."); + q_target = q_current.slerp(1.0 / angular_speed_scale, q_target); } - if (!is_planning_frame) + } + + // Compute the Cartesian deltas from the velocity-scaled values. + const auto angle_axis_error = Eigen::AngleAxisd(q_target * q_current.inverse()); + cartesian_position_delta.head<3>() = translation_error; + cartesian_position_delta.tail<3>() = angle_axis_error.axis() * angle_axis_error.angle(); + + // Compute the required change in joint angles. + const auto delta_result = + jointDeltaFromIK(cartesian_position_delta, robot_state, servo_params, joint_name_group_index_map); + status = delta_result.first; + if (status != StatusCode::INVALID) + { + joint_position_delta = delta_result.second; + // Get velocity scaling information for singularity. + const auto singularity_scaling_info = + velocityScalingFactorForSingularity(robot_state, cartesian_position_delta, servo_params); + // Apply velocity scaling for singularity, if there was any scaling. + if (singularity_scaling_info.second != StatusCode::NO_WARNING) { - RCLCPP_WARN_STREAM(getLogger(), "Command frame is: " << command.frame_id << " expected: " << planning_frame); + status = singularity_scaling_info.second; + RCLCPP_WARN_STREAM(getLogger(), SERVO_STATUS_CODE_MAP.at(status)); + joint_position_delta *= singularity_scaling_info.first; } } return std::make_pair(status, joint_position_delta); diff --git a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp index 4485200b7e..aaa4faa545 100644 --- a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp +++ b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp @@ -106,6 +106,25 @@ TEST_F(ServoRosFixture, testJointJog) moveit_servo::StatusCode status = status_; RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "Status after jointjog: " << static_cast(status)); ASSERT_EQ(status_, moveit_servo::StatusCode::NO_WARNING); + + // Ensure error when number of commands doesn't match number of joints + int traj_count_before = traj_count_; + jog_cmd.velocities.pop_back(); + jog_cmd.header.stamp = servo_test_node_->now(); + joint_jog_publisher->publish(jog_cmd); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + ASSERT_EQ(status_, moveit_servo::StatusCode::INVALID); + jog_cmd.velocities.push_back(1.0); + jog_cmd.velocities.push_back(1.0); + jog_cmd.header.stamp = servo_test_node_->now(); + joint_jog_publisher->publish(jog_cmd); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + ASSERT_EQ(status_, moveit_servo::StatusCode::INVALID); + + // No additional trajectories were generated with the invalid commands + ASSERT_EQ(traj_count_, traj_count_before); + status = status_; + RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "Status after invalid jointjog: " << static_cast(status)); } TEST_F(ServoRosFixture, testTwist)