20using namespace std::placeholders;
39 simulated_world_nws_ros2_msgs::msg::Pose6D
msg;
52 color.
r =
static_cast<unsigned char>(
msg.r);
53 color.
g =
static_cast<unsigned char>(
msg.g);
54 color.
b =
static_cast<unsigned char>(
msg.b);
64 if (driver && driver->
isValid())
69 if (m_iSim ==
nullptr)
97 node_options.automatically_declare_parameters_from_overrides(
true);
103 if (m_node ==
nullptr) {
108 m_srv_makeSphere = m_node->create_service<simulated_world_nws_ros2_msgs::srv::MakeSphere>(
110 m_srv_makeBox = m_node->create_service<simulated_world_nws_ros2_msgs::srv::MakeBox>(
112 m_srv_makeCylinder = m_node->create_service<simulated_world_nws_ros2_msgs::srv::MakeCylinder>(
114 m_srv_makeFrame = m_node->create_service<simulated_world_nws_ros2_msgs::srv::MakeFrame>(
116 m_srv_makeModel = m_node->create_service<simulated_world_nws_ros2_msgs::srv::MakeModel>(
118 m_srv_makeActor = m_node->create_service<simulated_world_nws_ros2_msgs::srv::MakeActor>(
120 m_srv_changeColor = m_node->create_service<simulated_world_nws_ros2_msgs::srv::ChangeColor>(
122 m_srv_setPose = m_node->create_service<simulated_world_nws_ros2_msgs::srv::SetPose>(
124 m_srv_enableGravity = m_node->create_service<simulated_world_nws_ros2_msgs::srv::EnableGravity>(
126 m_srv_enableCollision = m_node->create_service<simulated_world_nws_ros2_msgs::srv::EnableCollision>(
128 m_srv_getPose = m_node->create_service<simulated_world_nws_ros2_msgs::srv::GetPose>(
130 m_srv_deleteObject = m_node->create_service<simulated_world_nws_ros2_msgs::srv::DeleteObject>(
132 m_srv_deleteAll = m_node->create_service<simulated_world_nws_ros2_msgs::srv::DeleteAll>(
134 m_srv_getList = m_node->create_service<simulated_world_nws_ros2_msgs::srv::GetList>(
136 m_srv_attach = m_node->create_service<simulated_world_nws_ros2_msgs::srv::Attach>(
138 m_srv_detach = m_node->create_service<simulated_world_nws_ros2_msgs::srv::Detach>(
140 m_srv_rename = m_node->create_service<simulated_world_nws_ros2_msgs::srv::Rename>(
169 const std::shared_ptr<simulated_world_nws_ros2_msgs::srv::MakeSphere::Request>
request,
170 std::shared_ptr<simulated_world_nws_ros2_msgs::srv::MakeSphere::Response> response)
173 std::lock_guard<std::mutex> lock(m_mutex);
176 response->success =
false;
179 if (
request->frame_name ==
"None")
188 response->success =
false;
195 response->success =
static_cast<bool>(ret);
200 const std::shared_ptr<simulated_world_nws_ros2_msgs::srv::MakeBox::Request>
request,
201 std::shared_ptr<simulated_world_nws_ros2_msgs::srv::MakeBox::Response> response)
205 std::lock_guard<std::mutex> lock(m_mutex);
208 response->success =
false;
212 if (
request->frame_name ==
"None")
221 response->success =
false;
228 response->success =
static_cast<bool>(ret);
235 const std::shared_ptr<simulated_world_nws_ros2_msgs::srv::MakeCylinder::Request>
request,
236 std::shared_ptr<simulated_world_nws_ros2_msgs::srv::MakeCylinder::Response> response)
239 std::lock_guard<std::mutex> lock(m_mutex);
242 response->success =
false;
246 if (
request->frame_name ==
"None")
255 response->success =
false;
262 response->success =
static_cast<bool>(ret);
267 const std::shared_ptr<simulated_world_nws_ros2_msgs::srv::MakeFrame::Request>
request,
268 std::shared_ptr<simulated_world_nws_ros2_msgs::srv::MakeFrame::Response> response)
271 std::lock_guard<std::mutex> lock(m_mutex);
274 response->success =
false;
278 if (
request->frame_name ==
"None")
287 response->success =
false;
294 response->success =
static_cast<bool>(ret);
299 const std::shared_ptr<simulated_world_nws_ros2_msgs::srv::MakeModel::Request>
request,
300 std::shared_ptr<simulated_world_nws_ros2_msgs::srv::MakeModel::Response> response)
303 std::lock_guard<std::mutex> lock(m_mutex);
306 response->success =
false;
309 if (
request->frame_name ==
"None")
318 response->success =
false;
324 response->success =
static_cast<bool>(ret);
329 const std::shared_ptr<simulated_world_nws_ros2_msgs::srv::MakeActor::Request>
request,
330 std::shared_ptr<simulated_world_nws_ros2_msgs::srv::MakeActor::Response> response)
333 std::lock_guard<std::mutex> lock(m_mutex);
336 response->success =
false;
341 response->success =
static_cast<bool>(ret);
346 const std::shared_ptr<simulated_world_nws_ros2_msgs::srv::ChangeColor::Request>
request,
347 std::shared_ptr<simulated_world_nws_ros2_msgs::srv::ChangeColor::Response> response)
350 std::lock_guard<std::mutex> lock(m_mutex);
353 response->success =
false;
360 response->success =
false;
365 response->success =
static_cast<bool>(ret);
370 const std::shared_ptr<simulated_world_nws_ros2_msgs::srv::SetPose::Request>
request,
371 std::shared_ptr<simulated_world_nws_ros2_msgs::srv::SetPose::Response> response)
374 std::lock_guard<std::mutex> lock(m_mutex);
377 response->success =
false;
381 if (
request->frame_name ==
"None")
390 response->success =
false;
395 response->success =
static_cast<bool>(ret);
400 const std::shared_ptr<simulated_world_nws_ros2_msgs::srv::EnableGravity::Request>
request,
401 std::shared_ptr<simulated_world_nws_ros2_msgs::srv::EnableGravity::Response> response)
404 std::lock_guard<std::mutex> lock(m_mutex);
407 response->success =
false;
414 response->success =
false;
418 response->success =
static_cast<bool>(ret);
423 const std::shared_ptr<simulated_world_nws_ros2_msgs::srv::EnableCollision::Request>
request,
424 std::shared_ptr<simulated_world_nws_ros2_msgs::srv::EnableCollision::Response> response)
427 std::lock_guard<std::mutex> lock(m_mutex);
430 response->success =
false;
436 response->success =
false;
441 response->success =
static_cast<bool>(ret);
446 const std::shared_ptr<simulated_world_nws_ros2_msgs::srv::GetPose::Request>
request,
447 std::shared_ptr<simulated_world_nws_ros2_msgs::srv::GetPose::Response> response)
450 std::lock_guard<std::mutex> lock(m_mutex);
453 response->success =
false;
457 if (
request->frame_name ==
"None")
466 response->success =
false;
472 response->success =
static_cast<bool>(ret);
481 const std::shared_ptr<simulated_world_nws_ros2_msgs::srv::DeleteObject::Request>
request,
482 std::shared_ptr<simulated_world_nws_ros2_msgs::srv::DeleteObject::Response> response)
485 std::lock_guard<std::mutex> lock(m_mutex);
488 response->success =
false;
494 response->success =
false;
499 response->success =
static_cast<bool>(ret);
504 const std::shared_ptr<simulated_world_nws_ros2_msgs::srv::DeleteAll::Request>
request,
505 std::shared_ptr<simulated_world_nws_ros2_msgs::srv::DeleteAll::Response> response)
509 std::lock_guard<std::mutex> lock(m_mutex);
512 response->success =
false;
517 response->success =
static_cast<bool>(ret);
522 const std::shared_ptr<simulated_world_nws_ros2_msgs::srv::GetList::Request>
request,
523 std::shared_ptr<simulated_world_nws_ros2_msgs::srv::GetList::Response> response)
527 std::lock_guard<std::mutex> lock(m_mutex);
530 response->success =
false;
534 std::vector<std::string> names;
535 auto ret = m_iSim->
getList(names);
536 response->success =
static_cast<bool>(ret);
539 response->names = names;
545 const std::shared_ptr<simulated_world_nws_ros2_msgs::srv::Attach::Request>
request,
546 std::shared_ptr<simulated_world_nws_ros2_msgs::srv::Attach::Response> response)
549 std::lock_guard<std::mutex> lock(m_mutex);
552 response->success =
false;
556 if (
request->link_name ==
"None")
565 response->success =
false;
570 response->success =
static_cast<bool>(ret);
577 const std::shared_ptr<simulated_world_nws_ros2_msgs::srv::Detach::Request>
request,
578 std::shared_ptr<simulated_world_nws_ros2_msgs::srv::Detach::Response> response)
581 std::lock_guard<std::mutex> lock(m_mutex);
584 response->success =
false;
591 response->success =
false;
596 response->success =
static_cast<bool>(ret);
601 const std::shared_ptr<simulated_world_nws_ros2_msgs::srv::Rename::Request>
request,
602 std::shared_ptr<simulated_world_nws_ros2_msgs::srv::Rename::Response> response)
605 std::lock_guard<std::mutex> lock(m_mutex);
608 response->success =
false;
614 response->success =
false;
619 response->success =
static_cast<bool>(ret);
static rclcpp::Node::SharedPtr createNode(std::string name)
std::string m_setpose_servicename
std::string m_enablecollision_servicename
std::string m_enablegravity_servicename
std::string m_rename_servicename
std::string m_makesphere_servicename
std::string m_makebox_servicename
std::string m_getlist_servicename
std::string m_deleteobject_servicename
std::string m_getpose_servicename
std::string m_makemodel_servicename
std::string m_makeactor_servicename
std::string m_attach_servicename
std::string m_changecolor_servicename
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
std::string m_makeframe_servicename
std::string m_makecylinder_servicename
std::string m_deleteall_servicename
std::string m_detach_servicename
void getListCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< simulated_world_nws_ros2_msgs::srv::GetList::Request > request, std::shared_ptr< simulated_world_nws_ros2_msgs::srv::GetList::Response > response)
void attachCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< simulated_world_nws_ros2_msgs::srv::Attach::Request > request, std::shared_ptr< simulated_world_nws_ros2_msgs::srv::Attach::Response > response)
void deleteAllCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< simulated_world_nws_ros2_msgs::srv::DeleteAll::Request > request, std::shared_ptr< simulated_world_nws_ros2_msgs::srv::DeleteAll::Response > response)
void deleteObjectCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< simulated_world_nws_ros2_msgs::srv::DeleteObject::Request > request, std::shared_ptr< simulated_world_nws_ros2_msgs::srv::DeleteObject::Response > response)
void getPoseCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< simulated_world_nws_ros2_msgs::srv::GetPose::Request > request, std::shared_ptr< simulated_world_nws_ros2_msgs::srv::GetPose::Response > response)
void makeSphereCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< simulated_world_nws_ros2_msgs::srv::MakeSphere::Request > request, std::shared_ptr< simulated_world_nws_ros2_msgs::srv::MakeSphere::Response > response)
void makeModelCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< simulated_world_nws_ros2_msgs::srv::MakeModel::Request > request, std::shared_ptr< simulated_world_nws_ros2_msgs::srv::MakeModel::Response > response)
void makeActorCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< simulated_world_nws_ros2_msgs::srv::MakeActor::Request > request, std::shared_ptr< simulated_world_nws_ros2_msgs::srv::MakeActor::Response > response)
void setPoseCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< simulated_world_nws_ros2_msgs::srv::SetPose::Request > request, std::shared_ptr< simulated_world_nws_ros2_msgs::srv::SetPose::Response > response)
bool detach() override
Detach the object (you must have first called attach).
void renameCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< simulated_world_nws_ros2_msgs::srv::Rename::Request > request, std::shared_ptr< simulated_world_nws_ros2_msgs::srv::Rename::Response > response)
void makeBoxCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< simulated_world_nws_ros2_msgs::srv::MakeBox::Request > request, std::shared_ptr< simulated_world_nws_ros2_msgs::srv::MakeBox::Response > response)
void makeCylinderCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< simulated_world_nws_ros2_msgs::srv::MakeCylinder::Request > request, std::shared_ptr< simulated_world_nws_ros2_msgs::srv::MakeCylinder::Response > response)
void changeColorCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< simulated_world_nws_ros2_msgs::srv::ChangeColor::Request > request, std::shared_ptr< simulated_world_nws_ros2_msgs::srv::ChangeColor::Response > response)
void enableGravityCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< simulated_world_nws_ros2_msgs::srv::EnableGravity::Request > request, std::shared_ptr< simulated_world_nws_ros2_msgs::srv::EnableGravity::Response > response)
void makeFrameCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< simulated_world_nws_ros2_msgs::srv::MakeFrame::Request > request, std::shared_ptr< simulated_world_nws_ros2_msgs::srv::MakeFrame::Response > response)
void detachCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< simulated_world_nws_ros2_msgs::srv::Detach::Request > request, std::shared_ptr< simulated_world_nws_ros2_msgs::srv::Detach::Response > response)
void enableCollisionCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< simulated_world_nws_ros2_msgs::srv::EnableCollision::Request > request, std::shared_ptr< simulated_world_nws_ros2_msgs::srv::EnableCollision::Response > response)
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
SimulatedWorld_nws_ros2()
bool attach(yarp::dev::PolyDriver *driver) override
Attach to another object.
bool close() override
Close the DeviceDriver.
bool view(T *&x)
Get an interface to the device driver.
virtual yarp::dev::ReturnValue getPose(std::string id, yarp::sig::Pose6D &pose, std::string frame_name="")=0
Get object pose.
virtual yarp::dev::ReturnValue enableGravity(std::string id, bool enable)=0
Enable/disables gravity for an object.
virtual yarp::dev::ReturnValue enableCollision(std::string id, bool enable)=0
Enable/disables collision detection for an object.
virtual yarp::dev::ReturnValue getList(std::vector< std::string > &names)=0
List id of all objects that have been added to the world.
virtual yarp::dev::ReturnValue setPose(std::string id, yarp::sig::Pose6D pose, std::string frame_name="")=0
Set new object pose.
virtual yarp::dev::ReturnValue attach(std::string id, std::string link_name)=0
Attach an object to a link of another object.
virtual yarp::dev::ReturnValue makeCylinder(std::string id, double radius, double length, yarp::sig::Pose6D pose, yarp::sig::ColorRGB color, std::string frame_name="", bool gravity_enable=0, bool collision_enable=1)=0
Make a cylinder.
virtual yarp::dev::ReturnValue changeColor(std::string id, yarp::sig::ColorRGB color)=0
Change the color of an object.
virtual yarp::dev::ReturnValue makeFrame(std::string id, double size, yarp::sig::Pose6D pose, yarp::sig::ColorRGB color, std::string frame_name="", bool gravity_enable=0, bool collision_enable=1)=0
Make a reference frame (a 3D model with 3 arrows representing the axes)
virtual yarp::dev::ReturnValue deleteObject(std::string id)=0
Delete an object.
virtual yarp::dev::ReturnValue makeActor(std::string id, std::string skin_filename, std::string animation_filename, yarp::sig::Pose6D pose)=0
Create an actor.
virtual yarp::dev::ReturnValue makeModel(std::string id, std::string filename, yarp::sig::Pose6D pose, std::string frame_name="", bool gravity_enable=0, bool collision_enable=1)=0
Create a model from a file.
virtual yarp::dev::ReturnValue makeSphere(std::string id, double radius, yarp::sig::Pose6D pose, yarp::sig::ColorRGB color, std::string frame_name="", bool gravity_enable=0, bool collision_enable=1)=0
Make a sphere.
virtual yarp::dev::ReturnValue rename(std::string old_name, std::string new_name)=0
Change the names of an object.
virtual yarp::dev::ReturnValue makeBox(std::string id, double width, double height, double thickness, yarp::sig::Pose6D pose, yarp::sig::ColorRGB color, std::string frame_name="", bool gravity_enable=0, bool collision_enable=1)=0
Make a box.
virtual yarp::dev::ReturnValue deleteAll()=0
Delete all objects in the world.
virtual yarp::dev::ReturnValue detach(std::string id)=0
Detach a previously attached object.
A container for a device driver.
bool isValid() const
Check if device is valid.
A mini-server for performing network communication in the background.
A base class for nested structures that can be searched.
bool stop()
Stop the thread.
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCWarning(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.
The main, catch-all namespace for YARP.