Verify inputs

Author: urfeex

ur_controllers/src/force_mode_controller.cpp

@@ -236,6 +236,14 @@ bool ForceModeController::setForceMode(const ur_msgs::srv::SetForceMode::Request
 
   // transform task frame into base
   const std::string tf_prefix = params_.tf_prefix;
+  if (std::abs(req->task_frame.pose.orientation.x) < 1e-6 && std::abs(req->task_frame.pose.orientation.y) < 1e-6 &&
+      std::abs(req->task_frame.pose.orientation.z) < 1e-6 && std::abs(req->task_frame.pose.orientation.w) < 1e-6) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Received task frame with all-zeros quaternion. It should have at least one "
+                                           "non-zero entry.");
+    resp->success = false;
+    return false;
+  }
+
   try {
     auto task_frame_transformed = tf_buffer_->transform(req->task_frame, tf_prefix + "base");
 
@@ -251,6 +259,7 @@ bool ForceModeController::setForceMode(const ur_msgs::srv::SetForceMode::Request
   } catch (const tf2::TransformException& ex) {
     RCLCPP_ERROR(get_node()->get_logger(), "Could not transform %s to robot base: %s",
                  req->task_frame.header.frame_id.c_str(), ex.what());
+    resp->success = false;
     return false;
   }
 
@@ -275,6 +284,12 @@ bool ForceModeController::setForceMode(const ur_msgs::srv::SetForceMode::Request
   force_mode_parameters.limits[4] = req->selection_vector_ry ? req->speed_limits.angular.y : req->deviation_limits[4];
   force_mode_parameters.limits[5] = req->selection_vector_rz ? req->speed_limits.angular.z : req->deviation_limits[5];
 
+  if (req->type < 1 || req->type > 3) {
+    RCLCPP_ERROR(get_node()->get_logger(), "The force mode type has to be 1, 2, or 3. Received %u", req->type);
+    resp->success = false;
+    return false;
+  }
+
   /* The type decides how the robot interprets the force frame (the one defined in task_frame). See ur_script manual
    * for explanation, under force_mode. */
   force_mode_parameters.type = static_cast<double>(req->type);

ur_robot_driver/test/integration_test_force_mode.py

@@ -241,3 +241,88 @@ def test_force_mode_controller(self, tf_prefix):
                 deactivate_controllers=["force_mode_controller"],
             ).ok
         )
+
+    def test_illegal_force_mode_types(self, tf_prefix):
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=[
+                    "force_mode_controller",
+                ],
+                deactivate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "joint_trajectory_controller",
+                ],
+            ).ok
+        )
+        self._force_mode_controller_interface = ForceModeInterface(self.node)
+
+        # Create task frame for force mode
+        point = Point(x=0.0, y=0.0, z=0.0)
+        orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
+        task_frame_pose = Pose()
+        task_frame_pose.position = point
+        task_frame_pose.orientation = orientation
+        header = std_msgs.msg.Header(seq=1, frame_id=tf_prefix + "base")
+        header.stamp.sec = int(time.time()) + 1
+        header.stamp.nanosec = 0
+        frame_stamp = PoseStamped()
+        frame_stamp.header = header
+        frame_stamp.pose = task_frame_pose
+
+        res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=0)
+        self.assertFalse(res.success)
+        res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=4)
+        self.assertFalse(res.success)
+        res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=1)
+        self.assertTrue(res.success)
+        res = self._force_mode_controller_interface.stop_force_mode()
+        res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=2)
+        self.assertTrue(res.success)
+        res = self._force_mode_controller_interface.stop_force_mode()
+        res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=3)
+        self.assertTrue(res.success)
+        res = self._force_mode_controller_interface.stop_force_mode()
+
+    def test_illegal_task_frame(self, tf_prefix):
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=[
+                    "force_mode_controller",
+                ],
+                deactivate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "joint_trajectory_controller",
+                ],
+            ).ok
+        )
+        self._force_mode_controller_interface = ForceModeInterface(self.node)
+
+        # Create task frame for force mode
+        point = Point(x=0.0, y=0.0, z=0.0)
+        orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
+        task_frame_pose = Pose()
+        task_frame_pose.position = point
+        task_frame_pose.orientation = orientation
+        header = std_msgs.msg.Header(seq=1, frame_id=tf_prefix + "base")
+        header.stamp.sec = int(time.time()) + 1
+        header.stamp.nanosec = 0
+        frame_stamp = PoseStamped()
+        frame_stamp.header = header
+        frame_stamp.pose = task_frame_pose
+
+        # Illegal frame name produces error
+        header.frame_id = "nonexisting6t54"
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp,
+        )
+        self.assertFalse(res.success)
+        header.frame_id = "base"
+
+        # Illegal quaternion produces error
+        task_frame_pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=0.0)
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp,
+        )
+        self.assertFalse(res.success)