Exception handling for transfrom

moveit · Aug 3, 2023 · 78f5c43 · 78f5c43
1 parent d524172
commit 78f5c43
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 7 deletions.
diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp
@@ -134,8 +134,10 @@ class Servo
 private:
   /**
    * \brief Convert a give twist command to planning frame
+   * (This implementation assumes that source and target frames are stationary)
+   * See issue: https://github.com/ros-planning/moveit2/issues/2150
    * @param command The twist command to be converted
-   * @return The transformed twist command
+   * @return The transformed twist command.
    */
   const TwistCommand toPlanningFrame(const TwistCommand& command);
 

diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp
@@ -327,16 +327,33 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, moveit::
     }
     else if (expected_type == CommandType::TWIST)
     {
-      const TwistCommand command_in_planning_frame = toPlanningFrame(std::get<TwistCommand>(command));
-      delta_result = jointDeltaFromTwist(command_in_planning_frame, robot_state, servo_params_);
-      servo_status_ = delta_result.first;
+      try
+      {
+        const TwistCommand command_in_planning_frame = toPlanningFrame(std::get<TwistCommand>(command));
+        delta_result = jointDeltaFromTwist(command_in_planning_frame, robot_state, servo_params_);
+        servo_status_ = delta_result.first;
+      }
+      catch (tf2::TransformException& ex)
+      {
+        servo_status_ = StatusCode::INVALID;
+        RCLCPP_ERROR_STREAM(LOGGER, "Could not transform twist to planning frame.");
+      }
     }
     else if (expected_type == CommandType::POSE)
     {
-      const PoseCommand command_in_planning_frame = toPlanningFrame(std::get<PoseCommand>(command));
-      delta_result = jointDeltaFromPose(command_in_planning_frame, robot_state, servo_params_);
-      servo_status_ = delta_result.first;
+      try
+      {
+        const PoseCommand command_in_planning_frame = toPlanningFrame(std::get<PoseCommand>(command));
+        delta_result = jointDeltaFromPose(command_in_planning_frame, robot_state, servo_params_);
+        servo_status_ = delta_result.first;
+      }
+      catch (tf2::TransformException& ex)
+      {
+        servo_status_ = StatusCode::INVALID;
+        RCLCPP_ERROR_STREAM(LOGGER, "Could not transform pose to planning frame.");
+      }
     }
+
     if (servo_status_ != StatusCode::INVALID)
     {
       joint_position_deltas = delta_result.second;