38 #include <geometric_shapes/solid_primitive_dims.h>
39 #include <rclcpp/clock.hpp>
40 #include <rclcpp/executors.hpp>
41 #include <rclcpp/logger.hpp>
42 #include <rclcpp/logging.hpp>
43 #include <rclcpp/node.hpp>
44 #include <rclcpp/publisher.hpp>
45 #include <rclcpp/version.h>
46 #if RCLCPP_VERSION_GTE(20, 0, 0)
47 #include <rclcpp/event_handler.hpp>
49 #include <rclcpp/qos_event.hpp>
51 #include <rclcpp/time.hpp>
52 #include <rclcpp/utilities.hpp>
54 static const std::string ROBOT_DESCRIPTION =
"robot_description";
58 auto pub_aco = node->create_publisher<moveit_msgs::msg::AttachedCollisionObject>(
"attached_collision_object", 10);
59 moveit_msgs::msg::AttachedCollisionObject aco;
60 aco.link_name =
"r_wrist_roll_link";
61 aco.touch_links.push_back(
"r_wrist_roll_link");
62 aco.touch_links.push_back(
"r_gripper_palm_link");
63 aco.touch_links.push_back(
"r_gripper_led_frame");
64 aco.touch_links.push_back(
"r_gripper_motor_accelerometer_link");
65 aco.touch_links.push_back(
"r_gripper_tool_frame");
66 aco.touch_links.push_back(
"r_gripper_motor_slider_link");
67 aco.touch_links.push_back(
"r_gripper_motor_screw_link");
68 aco.touch_links.push_back(
"r_gripper_l_finger_link");
69 aco.touch_links.push_back(
"r_gripper_l_finger_tip_link");
70 aco.touch_links.push_back(
"r_gripper_r_finger_link");
71 aco.touch_links.push_back(
"r_gripper_r_finger_tip_link");
72 aco.touch_links.push_back(
"r_gripper_l_finger_tip_frame");
74 moveit_msgs::msg::CollisionObject& co = aco.object;
76 co.header.stamp = rclcpp::Clock().now();
77 co.header.frame_id = aco.link_name;
78 co.pose.orientation.w = 1.0;
79 co.operation = moveit_msgs::msg::CollisionObject::ADD;
80 co.primitives.resize(1);
81 co.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX;
82 co.primitives[0].dimensions.push_back(0.1);
83 co.primitives[0].dimensions.push_back(0.1);
84 co.primitives[0].dimensions.push_back(0.4);
85 co.primitive_poses.resize(1);
86 co.primitive_poses[0].position.x = 0.1;
87 co.primitive_poses[0].position.y = 0;
88 co.primitive_poses[0].position.z = -0.2;
90 using namespace std::chrono_literals;
91 pub_aco->publish(aco);
92 rclcpp::sleep_for(1s);
93 pub_aco->publish(aco);
94 RCLCPP_INFO(node->get_logger(),
"Object published.");
95 rclcpp::sleep_for(1500ms);
98 int main(
int argc,
char** argv)
100 rclcpp::init(argc, argv);
102 auto node = rclcpp::Node::make_shared(
"demo_scene");
void sendKnife(const rclcpp::Node::SharedPtr &node)
int main(int argc, char **argv)