39 #include <moveit_msgs/srv/get_planning_scene.hpp>
40 #include <moveit_msgs/srv/apply_planning_scene.hpp>
42 #include <rclcpp/executors.hpp>
43 #include <rclcpp/future_return_code.hpp>
57 options.arguments({
"--ros-args",
"-r",
58 "__node:=" + std::string(
"planning_scene_interface_") +
59 std::to_string(
reinterpret_cast<std::size_t
>(
this)) });
60 node_ = rclcpp::Node::make_shared(
"_", ns,
options);
62 planning_scene_diff_publisher_ = node_->create_publisher<moveit_msgs::msg::PlanningScene>(
"planning_scene", 1);
63 planning_scene_service_ =
64 node_->create_client<moveit_msgs::srv::GetPlanningScene>(move_group::GET_PLANNING_SCENE_SERVICE_NAME);
65 apply_planning_scene_service_ =
66 node_->create_client<moveit_msgs::srv::ApplyPlanningScene>(move_group::APPLY_PLANNING_SCENE_SERVICE_NAME);
70 waitForService(std::static_pointer_cast<rclcpp::ClientBase>(planning_scene_service_));
71 waitForService(std::static_pointer_cast<rclcpp::ClientBase>(apply_planning_scene_service_));
77 auto request = std::make_shared<moveit_msgs::srv::GetPlanningScene::Request>();
78 moveit_msgs::srv::GetPlanningScene::Response::SharedPtr response;
79 std::vector<std::string> result;
80 request->components.components = request->components.WORLD_OBJECT_NAMES;
82 auto res = planning_scene_service_->async_send_request(request);
83 if (rclcpp::spin_until_future_complete(node_, res) != rclcpp::FutureReturnCode::SUCCESS)
91 for (
const moveit_msgs::msg::CollisionObject& collision_object : response->scene.world.collision_objects)
93 if (!collision_object.type.key.empty())
94 result.push_back(collision_object.id);
99 for (
const moveit_msgs::msg::CollisionObject& collision_object : response->scene.world.collision_objects)
100 result.push_back(collision_object.id);
106 double maxz,
bool with_type, std::vector<std::string>& types)
108 auto request = std::make_shared<moveit_msgs::srv::GetPlanningScene::Request>();
109 moveit_msgs::srv::GetPlanningScene::Response::SharedPtr response;
110 std::vector<std::string> result;
111 request->components.components = request->components.WORLD_OBJECT_GEOMETRY;
113 auto res = planning_scene_service_->async_send_request(request);
114 if (rclcpp::spin_until_future_complete(node_, res) != rclcpp::FutureReturnCode::SUCCESS)
116 RCLCPP_WARN(node_->get_logger(),
"Could not call planning scene service to get object names");
119 response = res.get();
121 for (
const moveit_msgs::msg::CollisionObject& collision_object : response->scene.world.collision_objects)
123 if (with_type && collision_object.type.key.empty())
125 if (collision_object.mesh_poses.empty() && collision_object.primitive_poses.empty())
128 for (
const geometry_msgs::msg::Pose& mesh_pose : collision_object.mesh_poses)
130 if (!(mesh_pose.position.x >= minx && mesh_pose.position.x <= maxx && mesh_pose.position.y >= miny &&
131 mesh_pose.position.y <= maxy && mesh_pose.position.z >= minz && mesh_pose.position.z <= maxz))
137 for (
const geometry_msgs::msg::Pose& primitive_pose : collision_object.primitive_poses)
139 if (!(primitive_pose.position.x >= minx && primitive_pose.position.x <= maxx &&
140 primitive_pose.position.y >= miny && primitive_pose.position.y <= maxy &&
141 primitive_pose.position.z >= minz && primitive_pose.position.z <= maxz))
149 result.push_back(collision_object.id);
151 types.push_back(collision_object.type.key);
157 std::map<std::string, geometry_msgs::msg::Pose>
getObjectPoses(
const std::vector<std::string>& object_ids)
159 auto request = std::make_shared<moveit_msgs::srv::GetPlanningScene::Request>();
160 moveit_msgs::srv::GetPlanningScene::Response::SharedPtr response;
161 std::map<std::string, geometry_msgs::msg::Pose> result;
162 request->components.components = request->components.WORLD_OBJECT_GEOMETRY;
164 auto res = planning_scene_service_->async_send_request(request);
165 if (rclcpp::spin_until_future_complete(node_, res) == rclcpp::FutureReturnCode::SUCCESS)
167 response = res.get();
168 for (
const moveit_msgs::msg::CollisionObject& collision_object : response->scene.world.collision_objects)
170 if (std::find(object_ids.begin(), object_ids.end(), collision_object.id) != object_ids.end())
172 result[collision_object.id] = collision_object.pose;
179 std::map<std::string, moveit_msgs::msg::CollisionObject>
getObjects(
const std::vector<std::string>& object_ids)
181 auto request = std::make_shared<moveit_msgs::srv::GetPlanningScene::Request>();
182 moveit_msgs::srv::GetPlanningScene::Response::SharedPtr response;
183 std::map<std::string, moveit_msgs::msg::CollisionObject> result;
184 request->components.components = request->components.WORLD_OBJECT_GEOMETRY;
186 auto res = planning_scene_service_->async_send_request(request);
187 if (rclcpp::spin_until_future_complete(node_, res) != rclcpp::FutureReturnCode::SUCCESS)
189 RCLCPP_WARN(node_->get_logger(),
"Could not call planning scene service to get object geometries");
192 response = res.get();
194 for (
const moveit_msgs::msg::CollisionObject& collision_object : response->scene.world.collision_objects)
196 if (object_ids.empty() || std::find(object_ids.begin(), object_ids.end(), collision_object.id) != object_ids.end())
198 result[collision_object.id] = collision_object;
204 std::map<std::string, moveit_msgs::msg::AttachedCollisionObject>
207 auto request = std::make_shared<moveit_msgs::srv::GetPlanningScene::Request>();
208 moveit_msgs::srv::GetPlanningScene::Response::SharedPtr response;
209 std::map<std::string, moveit_msgs::msg::AttachedCollisionObject> result;
210 request->components.components = request->components.ROBOT_STATE_ATTACHED_OBJECTS;
212 auto res = planning_scene_service_->async_send_request(request);
213 if (rclcpp::spin_until_future_complete(node_, res) != rclcpp::FutureReturnCode::SUCCESS)
215 RCLCPP_WARN(node_->get_logger(),
"Could not call planning scene service to get attached object geometries");
218 response = res.get();
220 for (
const moveit_msgs::msg::AttachedCollisionObject& attached_collision_object :
221 response->scene.robot_state.attached_collision_objects)
223 if (object_ids.empty() ||
224 std::find(object_ids.begin(), object_ids.end(), attached_collision_object.object.id) != object_ids.end())
226 result[attached_collision_object.object.id] = attached_collision_object;
234 auto request = std::make_shared<moveit_msgs::srv::ApplyPlanningScene::Request>();
235 moveit_msgs::srv::ApplyPlanningScene::Response::SharedPtr response;
238 auto res = apply_planning_scene_service_->async_send_request(request);
239 if (rclcpp::spin_until_future_complete(node_, res) != rclcpp::FutureReturnCode::SUCCESS)
241 RCLCPP_WARN(node_->get_logger(),
"Failed to call ApplyPlanningScene service");
243 response = res.get();
244 return response->success;
248 const std::vector<moveit_msgs::msg::ObjectColor>& object_colors)
const
256 if (
planning_scene.object_colors[i].id.empty() && i < collision_objects.size())
273 moveit_msgs::msg::CollisionObject object;
274 for (
const std::string& object_id : object_ids)
276 object.id = object_id;
277 object.operation =
object.REMOVE;
285 void waitForService(
const std::shared_ptr<rclcpp::ClientBase>& srv)
289 std::chrono::duration<double> d(5.0);
290 srv->wait_for_service(std::chrono::duration_cast<std::chrono::nanoseconds>(d));
291 if (!srv->service_is_ready())
293 RCLCPP_WARN_STREAM(node_->get_logger(),
294 "service '" << srv->get_service_name() <<
"' not advertised yet. Continue waiting...");
295 srv->wait_for_service();
299 rclcpp::Node::SharedPtr node_;
300 rclcpp::Client<moveit_msgs::srv::GetPlanningScene>::SharedPtr planning_scene_service_;
301 rclcpp::Client<moveit_msgs::srv::ApplyPlanningScene>::SharedPtr apply_planning_scene_service_;
302 rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr planning_scene_diff_publisher_;
303 moveit::core::RobotModelConstPtr robot_model_;
322 double maxx,
double maxy,
double maxz,
324 std::vector<std::string>& types)
329 std::map<std::string, geometry_msgs::msg::Pose>
335 std::map<std::string, moveit_msgs::msg::CollisionObject>
341 std::map<std::string, moveit_msgs::msg::AttachedCollisionObject>
349 moveit_msgs::msg::PlanningScene ps;
350 ps.robot_state.is_diff =
true;
352 ps.world.collision_objects.reserve(1);
353 ps.world.collision_objects.push_back(collision_object);
358 const std_msgs::msg::ColorRGBA& object_color)
360 moveit_msgs::msg::PlanningScene ps;
361 ps.robot_state.is_diff =
true;
363 ps.world.collision_objects.reserve(1);
364 ps.world.collision_objects.push_back(collision_object);
365 moveit_msgs::msg::ObjectColor oc;
366 oc.id = collision_object.id;
367 oc.color = object_color;
368 ps.object_colors.push_back(oc);
373 const std::vector<moveit_msgs::msg::CollisionObject>& collision_objects,
374 const std::vector<moveit_msgs::msg::ObjectColor>& object_colors)
376 moveit_msgs::msg::PlanningScene ps;
377 ps.robot_state.is_diff =
true;
379 ps.world.collision_objects = collision_objects;
380 ps.object_colors = object_colors;
382 for (
size_t i = 0; i < ps.object_colors.size(); ++i)
384 if (ps.object_colors[i].id.empty() && i < collision_objects.size())
386 ps.object_colors[i].id = collision_objects[i].id;
398 const moveit_msgs::msg::AttachedCollisionObject& collision_object)
400 moveit_msgs::msg::PlanningScene ps;
401 ps.robot_state.is_diff =
true;
403 ps.robot_state.attached_collision_objects.reserve(1);
404 ps.robot_state.attached_collision_objects.push_back(collision_object);
409 const std::vector<moveit_msgs::msg::AttachedCollisionObject>& collision_objects)
411 moveit_msgs::msg::PlanningScene ps;
412 ps.robot_state.is_diff =
true;
414 ps.robot_state.attached_collision_objects = collision_objects;
424 const std::vector<moveit_msgs::msg::ObjectColor>& object_colors)
const
std::map< std::string, moveit_msgs::msg::AttachedCollisionObject > getAttachedObjects(const std::vector< std::string > &object_ids)
std::vector< std::string > getKnownObjectNames(bool with_type)
std::vector< std::string > getKnownObjectNamesInROI(double minx, double miny, double minz, double maxx, double maxy, double maxz, bool with_type, std::vector< std::string > &types)
std::map< std::string, geometry_msgs::msg::Pose > getObjectPoses(const std::vector< std::string > &object_ids)
std::map< std::string, moveit_msgs::msg::CollisionObject > getObjects(const std::vector< std::string > &object_ids)
bool applyPlanningScene(const moveit_msgs::msg::PlanningScene &planning_scene)
void removeCollisionObjects(const std::vector< std::string > &object_ids) const
void addCollisionObjects(const std::vector< moveit_msgs::msg::CollisionObject > &collision_objects, const std::vector< moveit_msgs::msg::ObjectColor > &object_colors) const
PlanningSceneInterfaceImpl(const std::string &ns="", bool wait=true)
bool applyAttachedCollisionObject(const moveit_msgs::msg::AttachedCollisionObject &attached_collision_object)
Apply attached collision object to the planning scene of the move_group node synchronously....
std::map< std::string, moveit_msgs::msg::AttachedCollisionObject > getAttachedObjects(const std::vector< std::string > &object_ids=std::vector< std::string >())
Get the attached objects identified by the given object ids list. If no ids are provided,...
bool applyPlanningScene(const moveit_msgs::msg::PlanningScene &ps)
Update the planning_scene of the move_group node with the given ps synchronously. Other PlanningScene...
std::vector< std::string > getKnownObjectNames(bool with_type=false)
Get the names of all known objects in the world. If with_type is set to true, only return objects tha...
bool applyAttachedCollisionObjects(const std::vector< moveit_msgs::msg::AttachedCollisionObject > &attached_collision_objects)
Apply attached collision objects to the planning scene of the move_group node synchronously....
std::map< std::string, moveit_msgs::msg::CollisionObject > getObjects(const std::vector< std::string > &object_ids=std::vector< std::string >())
Get the objects identified by the given object ids list. If no ids are provided, return all the known...
std::vector< std::string > getKnownObjectNamesInROI(double minx, double miny, double minz, double maxx, double maxy, double maxz, bool with_type, std::vector< std::string > &types)
Get the names of known objects in the world that are located within a bounding region (specified in t...
PlanningSceneInterface(const std::string &ns="", bool wait=true)
void removeCollisionObjects(const std::vector< std::string > &object_ids) const
Remove collision objects from the world via /planning_scene.
std::map< std::string, geometry_msgs::msg::Pose > getObjectPoses(const std::vector< std::string > &object_ids)
Get the poses from the objects identified by the given object ids list.
~PlanningSceneInterface()
bool applyCollisionObject(const moveit_msgs::msg::CollisionObject &collision_object)
Apply collision object to the planning scene of the move_group node synchronously....
bool applyCollisionObjects(const std::vector< moveit_msgs::msg::CollisionObject > &collision_objects, const std::vector< moveit_msgs::msg::ObjectColor > &object_colors=std::vector< moveit_msgs::msg::ObjectColor >())
Apply collision objects to the planning scene of the move_group node synchronously....
void addCollisionObjects(const std::vector< moveit_msgs::msg::CollisionObject > &collision_objects, const std::vector< moveit_msgs::msg::ObjectColor > &object_colors=std::vector< moveit_msgs::msg::ObjectColor >()) const
Add collision objects to the world via /planning_scene. Make sure object.operation is set to object....
Main namespace for MoveIt.
void setNodeLoggerName(const std::string &name)
Call once after creating a node to initialize logging namespaces.
This namespace includes the base class for MoveIt planners.
This namespace includes the central class for representing planning contexts.