add collision box subscriber

kuka_driver_examples/eci_demo/CMakeLists.txt

@@ -18,6 +18,7 @@ find_package(rclcpp REQUIRED)
 find_package(moveit_visual_tools REQUIRED)
 find_package(rviz_visual_tools REQUIRED)
 find_package(moveit_msgs REQUIRED)
+find_package(kuka_driver_interfaces REQUIRED)
 
 include_directories(include)
 
@@ -27,6 +28,7 @@ ament_target_dependencies(moveit_basic_planners_example
   rclcpp
   rviz_visual_tools
   moveit_visual_tools
+  kuka_driver_interfaces
 )
 
 add_executable(moveit_depalletizing_example src/MoveitDepalletizingExample.cpp)
@@ -35,6 +37,7 @@ ament_target_dependencies(moveit_depalletizing_example
   rclcpp
   rviz_visual_tools
   moveit_visual_tools
+  kuka_driver_interfaces
 )
 
 install(TARGETS moveit_basic_planners_example moveit_depalletizing_example

kuka_driver_examples/eci_demo/include/moveit_example.h

@@ -21,6 +21,7 @@
 #include "moveit/planning_scene_interface/planning_scene_interface.h"
 #include "moveit_msgs/msg/collision_object.hpp"
 #include "moveit_visual_tools/moveit_visual_tools.h"
+#include "kuka_driver_interfaces/msg/collision_box.hpp"
 
 class MoveitExample : public rclcpp::Node
 {
@@ -60,6 +61,11 @@ class MoveitExample : public rclcpp::Node
 
     move_group_interface_->setMaxVelocityScalingFactor(1.0);
     move_group_interface_->setMaxAccelerationScalingFactor(1.0);
+
+    // Create collision box subscriber
+    collision_box_subscriber_ = 
+      this->create_subscription<kuka_driver_interfaces::msg::CollisionBox>("collision_box", 10, std::bind(&MoveitExample::addCollisionBox, this, std::placeholders::_1));
+
   }
 
   moveit_msgs::msg::RobotTrajectory::SharedPtr drawCircle()
@@ -191,24 +197,24 @@ class MoveitExample : public rclcpp::Node
     AddObject(collision_object);
   }
 
-  void addCollisionBox(const Eigen::Vector3d & pose, const Eigen::Vector3d & size)
+  void addCollisionBox(const kuka_driver_interfaces::msg::CollisionBox::SharedPtr box_msg)
   {
     moveit_msgs::msg::CollisionObject collision_object;
     collision_object.header.frame_id = move_group_interface_->getPlanningFrame();
     collision_object.id = "collision_box";
     shape_msgs::msg::SolidPrimitive primitive;
     primitive.type = primitive.BOX;
     primitive.dimensions.resize(3);
-    primitive.dimensions[primitive.BOX_X] = size.x();
-    primitive.dimensions[primitive.BOX_Y] = size.y();
-    primitive.dimensions[primitive.BOX_Z] = size.z();
+    primitive.dimensions[primitive.BOX_X] = box_msg->size.x;
+    primitive.dimensions[primitive.BOX_Y] = box_msg->size.y;
+    primitive.dimensions[primitive.BOX_Z] = box_msg->size.z;
 
     // Define a pose for the box (specified relative to frame_id).
     geometry_msgs::msg::Pose stand_pose;
     stand_pose.orientation.w = 1.0;
-    stand_pose.position.x = pose.x();
-    stand_pose.position.y = pose.y();
-    stand_pose.position.z = pose.z();
+    stand_pose.position.x = box_msg->position.x;
+    stand_pose.position.y = box_msg->position.y;
+    stand_pose.position.z = box_msg->position.z;
 
     collision_object.primitives.push_back(primitive);
     collision_object.primitive_poses.push_back(stand_pose);
@@ -343,6 +349,7 @@ class MoveitExample : public rclcpp::Node
 protected:
   std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_interface_;
   rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr planning_scene_diff_publisher_;
+  rclcpp::Subscription<kuka_driver_interfaces::msg::CollisionBox>::SharedPtr collision_box_subscriber_;
   std::shared_ptr<moveit_visual_tools::MoveItVisualTools> moveit_visual_tools_;
   const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_basic_plan");
   const std::string PLANNING_GROUP = "lbr_iisy_arm";

kuka_driver_examples/eci_demo/package.xml

@@ -15,6 +15,7 @@
   <depend>rclcpp</depend>
   <depend>moveit_visual_tools</depend>
   <depend>moveit_configs_utils</depend>
+  <depend>kuka_driver_interfaces</depend>
 
   <exec_depend>ros2_controllers</exec_depend>
   <exec_depend>moveit</exec_depend>

kuka_driver_examples/eci_demo/src/MoveitBasicPlannersExample.cpp

@@ -14,6 +14,7 @@
 
 #include <math.h>
 
+#include <kuka_driver_interfaces/msg/collision_box.hpp>
 #include <memory>
 
 #include "moveit_example.h"
@@ -71,9 +72,16 @@ int main(int argc, char * argv[])
   }
 
   // Add collision object
-  example_node->addCollisionBox(
-    Eigen::Vector3d(0.25, -0.075, 0.675),
-    Eigen::Vector3d(0.1, 0.4, 0.1));
+  kuka_driver_interfaces::msg::CollisionBox box_msg;
+  box_msg.position.x = 0.25;
+  box_msg.position.y = -0.075;
+  box_msg.position.z = 0.675;
+
+  box_msg.size.x = 0.1;
+  box_msg.size.y = 0.4;
+  box_msg.size.z = 0.1;
+
+  example_node->addCollisionBox(std::make_shared<kuka_driver_interfaces::msg::CollisionBox>(box_msg));
   example_node->addBreakPoint();
 
   // Try moving back with Pilz LIN

kuka_driver_interfaces/CMakeLists.txt

@@ -18,13 +18,16 @@ endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
+find_package(geometry_msgs REQUIRED)
 
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/SetInt.srv"
   "srv/GetInt.srv"
   "srv/SetDouble.srv"
   "msg/RobotState.msg"
+  "msg/CollisionBox.msg"
+  DEPENDENCIES geometry_msgs
 )
 
 

kuka_driver_interfaces/msg/CollisionBox.msg

@@ -0,0 +1,4 @@
+# Message describing a collision box
+
+geometry_msgs/Vector3 position
+geometry_msgs/Vector3 size

kuka_driver_interfaces/package.xml

@@ -13,6 +13,8 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
+  <depend>geometry_msgs</depend>
+
   <exec_depend>rosidl_default_runtime</exec_depend>
 
   <member_of_group>rosidl_interface_packages</member_of_group>