43#include <control_msgs/msg/joint_jog.hpp>
44#include <geometry_msgs/msg/pose_stamped.hpp>
45#include <geometry_msgs/msg/twist_stamped.hpp>
63const int NUM_COMMANDS = 10;
72 auto joint_jog_publisher = servo_test_node_->create_publisher<control_msgs::msg::JointJog>(
73 "/servo_node/delta_joint_cmds", rclcpp::SystemDefaultsQoS());
76 auto request = std::make_shared<moveit_msgs::srv::ServoCommandType::Request>();
77 request->command_type = moveit_msgs::srv::ServoCommandType::Request::JOINT_JOG;
78 auto response = switch_input_client_->async_send_request(request);
79 ASSERT_EQ(response.get()->success,
true);
81 ASSERT_NE(state_count_, 0);
83 control_msgs::msg::JointJog jog_cmd;
85 jog_cmd.header.frame_id =
"panda_link0";
86 jog_cmd.joint_names.resize(7);
87 jog_cmd.velocities.resize(7);
88 jog_cmd.joint_names = {
"panda_joint1",
"panda_joint2",
"panda_joint3",
"panda_joint4",
89 "panda_joint5",
"panda_joint6",
"panda_joint7" };
91 std::fill(jog_cmd.velocities.begin(), jog_cmd.velocities.end(), 0.0);
93 jog_cmd.velocities[6] = 1.0;
96 while (rclcpp::ok() && count < NUM_COMMANDS)
98 jog_cmd.header.stamp = servo_test_node_->now();
99 joint_jog_publisher->publish(jog_cmd);
101 rclcpp::sleep_for(std::chrono::milliseconds(200));
104 ASSERT_GE(traj_count_, NUM_COMMANDS);
107 RCLCPP_INFO_STREAM(servo_test_node_->get_logger(),
"Status after jointjog: " <<
static_cast<size_t>(status));
113 waitForJointStates();
115 auto twist_publisher = servo_test_node_->create_publisher<geometry_msgs::msg::TwistStamped>(
116 "/servo_node/delta_twist_cmds", rclcpp::SystemDefaultsQoS());
118 auto request = std::make_shared<moveit_msgs::srv::ServoCommandType::Request>();
119 request->command_type = moveit_msgs::srv::ServoCommandType::Request::TWIST;
120 auto response = switch_input_client_->async_send_request(request);
121 ASSERT_EQ(response.get()->success,
true);
123 ASSERT_NE(state_count_, 0);
125 geometry_msgs::msg::TwistStamped twist_cmd;
126 twist_cmd.header.frame_id =
"panda_link0";
127 twist_cmd.twist.linear.x = 0.0;
128 twist_cmd.twist.linear.y = 0.0;
129 twist_cmd.twist.linear.z = 0.0;
130 twist_cmd.twist.angular.x = 0.0;
131 twist_cmd.twist.angular.y = 0.0;
132 twist_cmd.twist.angular.z = 0.5;
135 while (rclcpp::ok() && count < NUM_COMMANDS)
137 twist_cmd.header.stamp = servo_test_node_->now();
138 twist_publisher->publish(twist_cmd);
140 rclcpp::sleep_for(std::chrono::milliseconds(200));
143 ASSERT_GE(traj_count_, NUM_COMMANDS);
146 RCLCPP_INFO_STREAM(servo_test_node_->get_logger(),
"Status after twist: " <<
static_cast<size_t>(status));
152 waitForJointStates();
154 auto pose_publisher = servo_test_node_->create_publisher<geometry_msgs::msg::PoseStamped>(
155 "/servo_node/pose_target_cmds", rclcpp::SystemDefaultsQoS());
157 auto request = std::make_shared<moveit_msgs::srv::ServoCommandType::Request>();
158 request->command_type = moveit_msgs::srv::ServoCommandType::Request::POSE;
159 auto response = switch_input_client_->async_send_request(request);
160 ASSERT_EQ(response.get()->success,
true);
162 geometry_msgs::msg::PoseStamped pose_cmd;
163 pose_cmd.header.frame_id =
"panda_link0";
165 pose_cmd.pose.position.x = 0.3;
166 pose_cmd.pose.position.y = 0.0;
167 pose_cmd.pose.position.z = 0.6;
168 pose_cmd.pose.orientation.x = 0.7;
169 pose_cmd.pose.orientation.y = -0.7;
170 pose_cmd.pose.orientation.z = -0.000014;
171 pose_cmd.pose.orientation.w = -0.0000015;
173 ASSERT_NE(state_count_, 0);
176 while (rclcpp::ok() && count < NUM_COMMANDS)
178 pose_cmd.header.stamp = servo_test_node_->now();
179 pose_publisher->publish(pose_cmd);
181 rclcpp::sleep_for(std::chrono::milliseconds(200));
184 ASSERT_GE(traj_count_, NUM_COMMANDS);
187 RCLCPP_INFO_STREAM(servo_test_node_->get_logger(),
"Status after pose: " <<
static_cast<size_t>(status));
195 rclcpp::init(argc, argv);
196 ::testing::InitGoogleTest(&argc, argv);
197 int result = RUN_ALL_TESTS();
TEST_F(BulletCollisionDetectionTester, DISABLED_ContinuousCollisionSelf)
Continuous self collision checks are not supported yet by the bullet integration.
int main(int argc, char **argv)