Moveit2 - 7. Scenario planning ROS API

Scenario Planning ROS API

Introduce the use of planning scenario difference to perform , Add and delete objects in the robot world , Connect and separate objects on the robot

Run code

Open two terminal windows , Start in the first terminal window RViz And wait for all projects to load ：

ros2 launch moveit2_tutorials move_group.launch.py

Run the startup file of this demo in the second terminal window ：

ros2 launch moveit2_tutorials planning_scene_ros_api_tutorial.launch.py

A moment later ,RViz The window should appear , It looks like “ stay RViz Medium and fast start MoveIt2” tutorial “RViz Visualization tools ” Partial result diagram . To complete each demonstration step , stay RViz Active （ focusing ） Press the bottom of the screen RvizVisualToolsGui In the panel “Next” Button , Or select the top of the screen “Tools” In the panel “Key Tool” Then press... On the keyboard “0” Alphabet Key .

Expected output

stay RViz in , You should see the following ：

• Objects appear in the planning scene .
• Objects are connected to robots .
• Objects separate from robots .
• The object is removed from the planning scene .

Complete code

The complete code of this demo can be found in Here MoveIt GitHub project see .

visualization

MoveItVisualTools The software package provides many functions for RViz Visualize objects in 、 Functions of robots and trajectories , At the same time, it also provides debugging tools such as script step-by-step self-test .

rviz_visual_tools::RvizVisualTools visual_tools("panda_link0", "planning_scene_ros_api_tutorial", node);
visual_tools.loadRemoteControl();
visual_tools.deleteAllMarkers();

ROS API（ Application program interface ）

To the planning scenario publisher node ROS API By using “ Difference （diffs）” Topic interface implementation . The planning scenario difference is the current planning scenario （ from move_group Node to maintain ） Differences with the new planning scenarios expected by users .

Topics needed for broadcasting

Here, a publisher node will be created and waiting for the subscription of the subscriber node . Please note that , This topic may need to be remapped in the startup file .

rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr planning_scene_diff_publisher =
node->create_publisher<moveit_msgs::msg::PlanningScene>("planning_scene", 1);
while (planning_scene_diff_publisher->get_subscription_count() < 1)
{
    
  rclcpp::sleep_for(std::chrono::milliseconds(500));
}
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

Define the object message to connect

This message will be used here to add or delete the object in the robot world , And connect the object to the robot .

moveit_msgs::msg::AttachedCollisionObject attached_object;
attached_object.link_name = "panda_hand";
/* The header must contain a valid TF frame*/
attached_object.object.header.frame_id = "panda_hand";
/* The id of the object */
attached_object.object.id = "box";

/* A default pose */
geometry_msgs::msg::Pose pose;
pose.position.z = 0.11;
pose.orientation.w = 1.0;

/* Define a box to be attached */
shape_msgs::msg::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.075;
primitive.dimensions[1] = 0.075;
primitive.dimensions[2] = 0.075;

attached_object.object.primitives.push_back(primitive);
attached_object.object.primitive_poses.push_back(pose);

Please note that , Connecting an object to a robot requires that the corresponding operation be specified as ADD operation .

attached_object.object.operation = attached_object.object.ADD;

Because the object will be connected to the robot hand to simulate the object picking process , Therefore, the collision detector is expected to ignore the collision between the object and the robot hand .

attached_object.touch_links = std::vector<std::string>{
     "panda_hand", "panda_leftfinger", "panda_rightfinger" };

Add objects to the environment

By adding an object to the planning scene “world” Part to add the object to the environment . Be careful , Only... Is used here attached_object News “object” Field .

RCLCPP_INFO(LOGGER, "Adding the object into the world at the location of the hand.");
moveit_msgs::msg::PlanningScene planning_scene;
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene.is_diff = true;
planning_scene_diff_publisher->publish(planning_scene);
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

episode ： Synchronous and asynchronous updates

There are two different ways to use “ Difference （diffs）” And move_group Nodes interact ：

• Through one rosservice Call to send a “ Difference （diffs）” And block until the “ Difference ”（ Synchronize updates ）
• Send a message through a topic “ Difference （diffs）”, Even if it's time to “ Difference ” The follow-up process may not be applied yet （ Asynchronous update ）

Although the latter method is used most of the time in this tutorial （ Consider the long sleep inserted for visualization purposes , Asynchronous updates do not cause problems ）, But will planning_scene_diff_publisher It is entirely reasonable to replace with the following service clients ：

rclcpp::Client<moveit_msgs::srv::ApplyPlanningScene>::SharedPtr planning_scene_diff_client =
node->create_client<moveit_msgs::srv::ApplyPlanningScene>("apply_planning_scene");
planning_scene_diff_client->wait_for_service();

And through a service call, the “ Difference ” Send to planning scenario ：

auto request = std::make_shared<moveit_msgs::srv::ApplyPlanningScene::Request>();
request->scene = planning_scene;
std::shared_future<std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene_Response>> response_future;
response_future = planning_scene_diff_client->async_send_request(request);

Then wait for the service to respond ：

std::chrono::seconds wait_time(1);
std::future_status fs = response_future.wait_for(wait_time);
if (fs == std::future_status::timeout)
{
    
  RCLCPP_ERROR(LOGGER, "Service timed out.");
}
else
{
    
  std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene_Response> planning_response;
  planning_response = response_future.get();
  if (planning_response->success)
  {
    
    RCLCPP_INFO(LOGGER, "Service successfully added object.");
  }
  else
  {
    
    RCLCPP_ERROR(LOGGER, "Service failed to add object.");
  }
}

Please note that , Here, it is confirmed that the “ Difference ” The follow-up process will not continue before .

Connect objects to robots

When a robot picks up an object from the environment , The object needs to be “ additional ” To the robot , So that any component dealing with the robot model knows to consider this connected object , For example, collision detection .

Connecting an object to a robot requires the following two operations ：

• Remove the original object from the environment

• Connect the object to the robot

* First, define the REMOVE object message*/
moveit_msgs::msg::CollisionObject remove_object;
remove_object.id = "box";
remove_object.header.frame_id = "panda_hand";
remove_object.operation = remove_object.REMOVE;

Notice how you can ensure that by clearing these fields first diff The message does not contain other connected objects or collision objects .

/* Carry out the REMOVE + ATTACH operation */
RCLCPP_INFO(LOGGER, "Attaching the object to the hand and removing it from the world.");
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(remove_object);
planning_scene.robot_state.attached_collision_objects.push_back(attached_object);
planning_scene.robot_state.is_diff = true;
planning_scene_diff_publisher->publish(planning_scene);
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

Separate an object from the robot

Separating an object from a robot requires the following two operations ：

• Separate the object from the robot
• Reinsert the object into the environment

/* First, define the DETACH object message*/
moveit_msgs::msg::AttachedCollisionObject detach_object;
detach_object.object.id = "box";
detach_object.link_name = "panda_hand";
detach_object.object.operation = attached_object.object.REMOVE;

Notice how you can ensure that by clearing these fields first diff The message does not contain other connected objects or collision objects .

/* Carry out the DETACH + ADD operation */
RCLCPP_INFO(LOGGER, "Detaching the object from the robot and returning it to the world.");
planning_scene.robot_state.attached_collision_objects.clear();
planning_scene.robot_state.attached_collision_objects.push_back(detach_object);
planning_scene.robot_state.is_diff = true;
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene.is_diff = true;
planning_scene_diff_publisher->publish(planning_scene);
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

Remove objects from the collision world

To remove an object from the collision world, you only need to use the previously defined remove object message . Also note how you can ensure that by clearing these fields first diff The message does not contain other connected objects or collision objects .

RCLCPP_INFO(LOGGER, "Removing the object from the world.");
planning_scene.robot_state.attached_collision_objects.clear();
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(remove_object);
planning_scene_diff_publisher->publish(planning_scene);