From b1d7b4a18eb3f260be546657c692ab6e0b9fc41f Mon Sep 17 00:00:00 2001 From: ibrahiminfinite Date: Thu, 3 Aug 2023 12:10:22 +0530 Subject: [PATCH] Minor updates --- moveit_ros/moveit_servo/src/servo_node.cpp | 4 ++++ .../moveit_servo/tests/test_ros_integration.cpp | 15 +++------------ 2 files changed, 7 insertions(+), 12 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 01951a1511b..c28426e44d1 100644 --- 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 index 45bfa678fb9..12ccb600d73 100644 --- 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; {