diff --git a/src/moveit_interface.cpp b/src/moveit_interface.cpp index dca3ca0..147b7ff 100644 --- a/src/moveit_interface.cpp +++ b/src/moveit_interface.cpp @@ -45,6 +45,8 @@ class MoveitInterface : public rclcpp::Node move_group_ = std::make_shared(std::shared_ptr(shared_this), PLANNING_GROUP); move_group_->setStartStateToCurrentState(); planning_scene_interface_ = std::make_shared(); + move_group_->setMaxVelocityScalingFactor(0.5); + move_group_->setMaxAccelerationScalingFactor(0.5); // Connection to Rviz visual_tools_ = std::make_shared(std::shared_ptr(shared_this), "panda_link0", "move_group", move_group_->getRobotModel()); @@ -121,33 +123,55 @@ class MoveitInterface : public rclcpp::Node //Just for the IML experiment void box_callback(const std_msgs::msg::Empty::SharedPtr /*msg*/) { - moveit_msgs::msg::CollisionObject visual_object; + /*moveit_msgs::msg::CollisionObject visual_object; visual_object.header.frame_id = move_group_->getPlanningFrame(); - visual_object.id = "box1"; + visual_object.id = "obstacle"; shape_msgs::msg::SolidPrimitive primitive; primitive.type = primitive.BOX; primitive.dimensions.resize(3); primitive.dimensions[primitive.BOX_X] = 0.2; primitive.dimensions[primitive.BOX_Y] = 0.2; - primitive.dimensions[primitive.BOX_Z] = 0.4; + primitive.dimensions[primitive.BOX_Z] = 0.05; geometry_msgs::msg::Pose box_pose; box_pose.orientation.w = 1.0; box_pose.position.x = 0.5; box_pose.position.y = 0.0; - box_pose.position.z = 0.2; + box_pose.position.z = 0.0; visual_object.primitives.push_back(primitive); visual_object.primitive_poses.push_back(box_pose); visual_object.operation = visual_object.ADD; + */ + moveit_msgs::msg::CollisionObject table_object; + table_object.header.frame_id = move_group_->getPlanningFrame(); + + table_object.id = "table"; + + shape_msgs::msg::SolidPrimitive primitive2; + primitive2.type = primitive2.BOX; + primitive2.dimensions.resize(3); + primitive2.dimensions[primitive2.BOX_X] = 0.8; + primitive2.dimensions[primitive2.BOX_Y] = 1.4; + primitive2.dimensions[primitive2.BOX_Z] = 0.02; + + geometry_msgs::msg::Pose table_pose; + table_pose.orientation.w = 1.0; + table_pose.position.x = 0.5; + table_pose.position.y = 0.0; + table_pose.position.z = -0.02; + + table_object.primitives.push_back(primitive2); + table_object.primitive_poses.push_back(table_pose); + table_object.operation = table_object.ADD; std::vector visual_objects; - visual_objects.push_back(visual_object); + // visual_objects.push_back(visual_object); + visual_objects.push_back(table_object); planning_scene_interface_->addCollisionObjects(visual_objects); - } rclcpp::Publisher::SharedPtr publisher_; @@ -177,4 +201,4 @@ int main(int argc, char * argv[]) rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; -} \ No newline at end of file +}