robossembler/runtime
7
0
Fork 0
Code Issues 5 Pull requests Projects Releases Packages Wiki Activity Actions

clang-format

Browse source
This commit is contained in:
Ilya Uraev 2023-11-09 15:39:19 +03:00
parent 18d2772cef
commit 6eee02bacc
17 changed files with 1299 additions and 1346 deletions
Download patch file Download diff file Expand all files Collapse all files

59
rbs_skill_servers/src/add_planning_scene_objects_service.cpp
View file

@ -1,24 +1,25 @@
#include "moveit/move_group_interface/move_group_interface.h"
#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp"
#include <algorithm>
#include <cinttypes>
#include <memory>
#include <algorithm>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp"
#include "moveit/move_group_interface/move_group_interface.h"
#include "rclcpp/rclcpp.hpp"
using AddPlanningSceneObject = rbs_skill_interfaces::srv::AddPlanningSceneObject;
using AddPlanningSceneObject =
rbs_skill_interfaces::srv::AddPlanningSceneObject;
rclcpp::Node::SharedPtr g_node = nullptr;
void handle_service(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<AddPlanningSceneObject::Request> request,
const std::shared_ptr<AddPlanningSceneObject::Response> response)
{
(void)request_header;
// Create collision object for the robot to avoid
auto const collision_object = [frame_id =
"world", object_name=request->object_id, object_pose=request->object_pose] {
const std::shared_ptr<AddPlanningSceneObject::Response> response) {
(void)request_header;
// Create collision object for the robot to avoid
auto const collision_object = [frame_id = "world",
object_name = request->object_id,
object_pose = request->object_pose] {
moveit_msgs::msg::CollisionObject collision_object;
collision_object.header.frame_id = frame_id;
collision_object.id = object_name;
@ -40,32 +41,32 @@ void handle_service(
box_pose.orientation.y = object_pose[4];
box_pose.orientation.z = object_pose[5];
box_pose.orientation.w = object_pose[6];
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
return collision_object;
}();
}();
// Add the collision object to the scene
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
planning_scene_interface.applyCollisionObject(collision_object);
// Add the collision object to the scene
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
planning_scene_interface.applyCollisionObject(collision_object);
response->success = true;
response->success = true;
}
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true);
g_node = rclcpp::Node::make_shared("add_planing_scene_object", "", node_options);
auto server = g_node->create_service<AddPlanningSceneObject>("add_planing_scene_object_service", handle_service);
rclcpp::spin(g_node);
rclcpp::shutdown();
g_node = nullptr;
return 0;
}
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.allow_undeclared_parameters(true)
.automatically_declare_parameters_from_overrides(true);
g_node =
rclcpp::Node::make_shared("add_planing_scene_object", "", node_options);
auto server = g_node->create_service<AddPlanningSceneObject>(
"add_planing_scene_object_service", handle_service);
rclcpp::spin(g_node);
rclcpp::shutdown();
g_node = nullptr;
return 0;
}