YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
SimulatedWorld_nws_yarp.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2026-2026 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: LGPL-2.1-or-later
4 */
5
7#include <sstream>
8#include <string>
10#include <yarp/os/LogStream.h>
11#include <time.h>
12#include <stdlib.h>
13
14using namespace yarp::sig;
15using namespace yarp::dev;
16using namespace yarp::os;
17
18namespace {
19YARP_LOG_COMPONENT(SIMWORLDNWS, "yarp.device.SimulatedWorld_nws_yarp")
20}
21
25
30
31bool SimulatedWorld_nws_yarp::attach(PolyDriver* driver)
32{
33 if (driver==nullptr)
34 {
35 yCError(SIMWORLDNWS, "Invalid pointer to device driver received");
36 return false;
37 }
38
39 driver->view(m_Isim_p);
40 if (nullptr == m_Isim_p)
41 {
42 yCError(SIMWORLDNWS, "Unable to view ISimulatedWorld interface");
43 return false;
44 }
45 m_msgsImpl = std::make_unique<ISimulatedWorldMsgsImpl>(m_Isim_p);
46
47 return true;
48}
49
50bool SimulatedWorld_nws_yarp::detach()
51{
52 m_Isim_p = nullptr;
53 return true;
54}
55
56bool SimulatedWorld_nws_yarp::read(yarp::os::ConnectionReader& connection)
57{
58 if (!connection.isValid()) { return false;}
59 if (!m_msgsImpl) { return false;}
60
61 bool b = m_msgsImpl->read(connection);
62 if (b)
63 {
64 return true;
65 }
66 else
67 {
68 yCError(SIMWORLDNWS, "read() Command failed");
69 return false;
70 }
71}
72
74{
75 if (!parseParams(config)) { return false; }
76
77 m_rpcPortName = m_name + "/rpc:i";
78
79 if (!m_rpcPort.open(m_rpcPortName.c_str()))
80 {
81 yCError(SIMWORLDNWS) << "Error opening port" << m_rpcPortName;
82 return false;
83 }
84 m_rpcPort.setReader(*this);
85
86 return true;
87}
88
90{
91 yCTrace(SIMWORLDNWS, "BatteryWrapper::Close");
92 m_rpcPort.close();
93 detachAll();
94 return true;
95}
96
97ReturnValue ISimulatedWorldMsgsImpl::attach(const std::string& id, const std::string& link_name)
98{
99 std::lock_guard <std::mutex> lg(m_mutex);
100 auto ret = m_iSim->attach(id, link_name);
101 if(!ret)
102 {
103 yCError(SIMWORLDNWS) << "Could not attach";
104 }
105 return ret;
106}
107
109{
110 std::lock_guard <std::mutex> lg(m_mutex);
111 auto ret = m_iSim->changeColor(id, color);
112 if(!ret)
113 {
114 yCError(SIMWORLDNWS) << "Could not changeColor";
115 }
116 return ret;
117}
118
120{
121 std::lock_guard <std::mutex> lg(m_mutex);
122 auto ret = m_iSim->deleteObject(id);
123 if(!ret)
124 {
125 yCError(SIMWORLDNWS) << "Could not deleteObject";
126 }
127 return ret;
128}
129
131{
132 std::lock_guard <std::mutex> lg(m_mutex);
133 auto ret = m_iSim->deleteAll();
134 if(!ret)
135 {
136 yCError(SIMWORLDNWS) << "Could not deleteAll";
137 }
138 return ret;
139}
140
141ReturnValue ISimulatedWorldMsgsImpl::enableCollision(const std::string& id, const bool enable)
142{
143 std::lock_guard <std::mutex> lg(m_mutex);
144 auto ret = m_iSim->enableCollision(id, enable);
145 if(!ret)
146 {
147 yCError(SIMWORLDNWS) << "Could not enableCollision";
148 }
149 return ret;
150}
151
152ReturnValue ISimulatedWorldMsgsImpl::enableGravity(const std::string& id, const bool enable)
153{
154 std::lock_guard <std::mutex> lg(m_mutex);
155 auto ret = m_iSim->enableGravity(id, enable);
156 if(!ret)
157 {
158 yCError(SIMWORLDNWS) << "Could not enableGravity";
159 }
160 return ret;
161}
162
163ReturnValue ISimulatedWorldMsgsImpl::setPose(const std::string& id, const Pose6D& pose, const std::string& frame_name)
164{
165 std::lock_guard <std::mutex> lg(m_mutex);
166 auto ret = m_iSim->setPose(id, pose, frame_name);
167 if(!ret)
168 {
169 yCError(SIMWORLDNWS) << "Could not setPose";
170 }
171 return ret;
172}
173
174ReturnValue ISimulatedWorldMsgsImpl::makeSphere(const std::string& id_name, const double radius, const Pose6D& pose, const ColorRGB& color, const std::string& frame_name, const bool gravity_enable, const bool collision_enable)
175{
176 std::lock_guard <std::mutex> lg(m_mutex);
177 auto ret = m_iSim->makeSphere(id_name, radius, pose, color, frame_name, gravity_enable, collision_enable);
178 if(!ret)
179 {
180 yCError(SIMWORLDNWS) << "Could not makeSphere";
181 }
182 return ret;
183}
184
185ReturnValue ISimulatedWorldMsgsImpl::makeBox(const std::string& id_name, const double width, const double height, const double thickness, const Pose6D& pose, const ColorRGB& color, const std::string& frame_name, const bool gravity_enable, const bool collision_enable)
186{
187 std::lock_guard <std::mutex> lg(m_mutex);
188 auto ret = m_iSim->makeBox(id_name, width, height, thickness, pose, color, frame_name, gravity_enable, collision_enable);
189 if(!ret)
190 {
191 yCError(SIMWORLDNWS) << "Could not makeBox";
192 }
193 return ret;
194}
195
196ReturnValue ISimulatedWorldMsgsImpl::makeCylinder(const std::string& id_name, const double radius, const double length, const Pose6D& pose, const ColorRGB& color, const std::string& frame_name, const bool gravity_enable, const bool collision_enable)
197{
198 std::lock_guard <std::mutex> lg(m_mutex);
199 auto ret = m_iSim->makeCylinder(id_name, radius, length, pose, color, frame_name, gravity_enable, collision_enable);
200 if(!ret)
201 {
202 yCError(SIMWORLDNWS) << "Could not makeCylinder";
203 }
204 return ret;
205}
206
207ReturnValue ISimulatedWorldMsgsImpl::makeFrame(const std::string& id_name, const double size, const Pose6D& pose, const ColorRGB& color, const std::string& frame_name, const bool gravity_enable, const bool collision_enable)
208{
209 std::lock_guard <std::mutex> lg(m_mutex);
210 auto ret = m_iSim->makeFrame(id_name, size, pose, color, frame_name, gravity_enable, collision_enable);
211 if(!ret)
212 {
213 yCError(SIMWORLDNWS) << "Could not makeFrame";
214 }
215 return ret;
216}
217
218ReturnValue ISimulatedWorldMsgsImpl::makeModel(const std::string& id_name, const std::string& filename, const Pose6D& pose, const std::string& frame_name, const bool gravity_enable, const bool collision_enable)
219{
220 std::lock_guard <std::mutex> lg(m_mutex);
221 auto ret = m_iSim->makeModel(id_name, filename, pose, frame_name, gravity_enable, collision_enable);
222 if(!ret)
223 {
224 yCError(SIMWORLDNWS) << "Could not makeModel";
225 }
226 return ret;
227}
228
229ReturnValue ISimulatedWorldMsgsImpl::makeActor(const std::string& id_name, const std::string& skin_filename, const std::string& anim_filename, const Pose6D& pose)
230{
231 std::lock_guard <std::mutex> lg(m_mutex);
232 auto ret = m_iSim->makeActor(id_name, skin_filename, anim_filename, pose);
233 if(!ret)
234 {
235 yCError(SIMWORLDNWS) << "Could not makeActor";
236 }
237 return ret;
238}
239
241{
242 std::lock_guard <std::mutex> lg(m_mutex);
243 auto ret = m_iSim->detach(id);
244 if(!ret)
245 {
246 yCError(SIMWORLDNWS) << "Could not detach";
247 }
248 return ret;
249}
250
251ReturnValue ISimulatedWorldMsgsImpl::rename(const std::string& old_name, const std::string& new_name)
252{
253 std::lock_guard <std::mutex> lg(m_mutex);
254 auto ret = m_iSim->rename(old_name, new_name);
255 if(!ret)
256 {
257 yCError(SIMWORLDNWS) << "Could not rename";
258 }
259 return ret;
260}
261
263{
264 std::lock_guard <std::mutex> lg(m_mutex);
265 getListReturnValue response;
266
267 std::vector<std::string> names;
268 auto ret = m_iSim->getList(names);
269 if(!ret)
270 {
271 yCError(SIMWORLDNWS) << "Could not getList";
272 response.retVal = ret;
273 return response;
274 }
275
276 response.retVal = ret;
277 response.listnames = names;
278
279 return response;
280}
281
282getPoseReturnValue ISimulatedWorldMsgsImpl::getPose(const std::string& id, const std::string& frame_name)
283{
284 std::lock_guard <std::mutex> lg(m_mutex);
285 getPoseReturnValue response;
286
287 Pose6D pose;
288 auto ret = m_iSim->getPose(id, pose, frame_name);
289 if(!ret)
290 {
291 yCError(SIMWORLDNWS) << "Could not getPose";
292 response.retVal = ret;
293 return response;
294 }
295
296 response.retVal = ret;
297 response.pose = pose;
298
299 return response;
300}
301
yarp::dev::ReturnValue enableCollision(const std::string &id, const bool enable) override
yarp::dev::ReturnValue makeBox(const std::string &id_name, const double width, const double height, const double thickness, const yarp::sig::Pose6D &pose, const yarp::sig::ColorRGB &color, const std::string &frame_name, const bool gravity_enable, const bool collision_enable) override
getPoseReturnValue getPose(const std::string &id, const std::string &frame_name) override
yarp::dev::ReturnValue makeSphere(const std::string &id_name, const double radius, const yarp::sig::Pose6D &pose, const yarp::sig::ColorRGB &color, const std::string &frame_name, const bool gravity_enable, const bool collision_enable) override
yarp::dev::ReturnValue rename(const std::string &old_name, const std::string &new_name) override
yarp::dev::ReturnValue makeModel(const std::string &id_name, const std::string &filename, const yarp::sig::Pose6D &pose, const std::string &frame_name, const bool gravity_enable, const bool collision_enable) override
yarp::dev::ReturnValue deleteObject(const std::string &id) override
yarp::dev::ReturnValue detach(const std::string &id) override
yarp::dev::ReturnValue setPose(const std::string &id, const yarp::sig::Pose6D &pose, const std::string &frame_name="") override
yarp::dev::ReturnValue changeColor(const std::string &id, const yarp::sig::ColorRGB &color) override
yarp::dev::ReturnValue enableGravity(const std::string &id, const bool enable) override
yarp::dev::ReturnValue makeFrame(const std::string &id_name, const double size, const yarp::sig::Pose6D &pose, const yarp::sig::ColorRGB &color, const std::string &frame_name, const bool gravity_enable, const bool collision_enable) override
yarp::dev::ReturnValue attach(const std::string &id, const std::string &link_name) override
yarp::dev::ReturnValue makeCylinder(const std::string &id_name, const double radius, const double length, const yarp::sig::Pose6D &pose, const yarp::sig::ColorRGB &color, const std::string &frame_name, const bool gravity_enable, const bool collision_enable) override
getListReturnValue getList() override
yarp::dev::ReturnValue makeActor(const std::string &id_name, const std::string &skin_filename, const std::string &actor_filename, const yarp::sig::Pose6D &pose) override
yarp::dev::ReturnValue deleteAll() override
ISimulatedWorldMsgsImpl(yarp::dev::ISimulatedWorld *iSim)
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
bool close() override
Close the DeviceDriver.
bool open(yarp::os::Searchable &params) override
Open the DeviceDriver.
yarp::dev::ReturnValue retVal
std::vector< std::string > listnames
yarp::dev::ReturnValue retVal
yarp::sig::Pose6D pose
bool view(T *&x)
Get an interface to the device driver.
ISimulatedWorld Interface.
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.
Definition PolyDriver.h:23
bool detachAll() final
Detach the object (you must have first called attach).
A mini-server for performing network communication in the background.
An interface for reading from a network connection.
virtual bool isValid() const =0
void setReader(PortReader &reader) override
Set an external reader for port data.
Definition Port.cpp:470
void close() override
Stop port activity.
Definition Port.cpp:330
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
Definition Port.cpp:79
A base class for nested structures that can be searched.
Definition Searchable.h:31
#define yCError(component,...)
#define yCTrace(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.