Minor updates

moveit · Aug 3, 2023 · b1d7b4a · b1d7b4a
1 parent d8c3be2
commit b1d7b4a
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 12 deletions.
diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp
@@ -298,9 +298,13 @@ void ServoNode::servoLoop()
     if ((servo_->getStatus() != StatusCode::INVALID))
     {
       if (servo_params_.command_out_type == "trajectory_msgs/JointTrajectory")
+      {
         trajectory_publisher_->publish(composeTrajectoryMessage(servo_->getParams(), next_joint_state));
+      }
       else if (servo_params_.command_out_type == "std_msgs/Float64MultiArray")
+      {
         multi_array_publisher_->publish(composeMultiArrayMessage(servo_->getParams(), next_joint_state));
+      }
       last_commanded_state_ = next_joint_state;
     }
 

diff --git a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp
@@ -76,10 +76,7 @@ TEST_F(ServoRosFixture, testJointJog)
   auto response = switch_input_client_->async_send_request(request);
   ASSERT_EQ(response.get()->success, true);
 
-  if (state_count_ > 0)
-    RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "State available");
-  else
-    RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "state not available");
+  ASSERT_NE(state_count_, 0);
 
   sensor_msgs::msg::JointState prev_state, curr_state;
   {
@@ -130,10 +127,7 @@ TEST_F(ServoRosFixture, testTwist)
   auto response = switch_input_client_->async_send_request(request);
   ASSERT_EQ(response.get()->success, true);
 
-  if (state_count_ > 0)
-    RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "State available");
-  else
-    RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "state not available");
+  ASSERT_NE(state_count_, 0);
 
   sensor_msgs::msg::JointState prev_state, curr_state;
   {
@@ -193,10 +187,7 @@ TEST_F(ServoRosFixture, testPose)
   pose_cmd.pose.orientation.y = 0.0;
   pose_cmd.pose.orientation.z = 0.0;
 
-  if (state_count_ > 0)
-    RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "State available");
-  else
-    RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "state not available");
+  ASSERT_NE(state_count_, 0);
 
   sensor_msgs::msg::JointState prev_state, curr_state;
   {