Niko/franka_moveit_interface
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

move_square tested

Browse Source
This commit is contained in:
Niko Feith 2024-01-26 17:26:11 +01:00
parent 14f1bc45fc
commit b524818c74
1 changed files with 31 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
src/moveit_interface.cpp
View File

@ -45,6 +45,8 @@ class MoveitInterface : public rclcpp::Node
move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(std::shared_ptr<rclcpp::Node>(shared_this), PLANNING_GROUP);
move_group_->setStartStateToCurrentState();
planning_scene_interface_ = std::make_shared<moveit::planning_interface::PlanningSceneInterface>();
move_group_->setMaxVelocityScalingFactor(0.5);
move_group_->setMaxAccelerationScalingFactor(0.5);
// Connection to Rviz
visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(std::shared_ptr<rclcpp::Node>(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<moveit_msgs::msg::CollisionObject> 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<moveit_msgs::msg::RobotTrajectory>::SharedPtr publisher_;