YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
MobileBaseVelocityControl_nwc_yarp.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: LGPL-2.1-or-later
4 */
5
8#include <yarp/os/Log.h>
10#include <yarp/os/LogStream.h>
11#include <mutex>
12#include <cmath>
13
16using namespace yarp::dev;
17using namespace yarp::dev::Nav2D;
18using namespace yarp::os;
19using namespace yarp::sig;
20
21namespace {
22YARP_LOG_COMPONENT(MOBVEL_NWC_YARP, "yarp.device.MobileBaseVelocityControl_nwc_yarp")
23}
24
25//------------------------------------------------------------------------------------------------------------------------------
26
28{
29 if (!parseParams(config)) { return false; }
30
31 std::string local_rpc_1 = m_local + "/navigation/rpc:o";
32 std::string remote_rpc_1 = m_server + "/rpc:i";
33
35 {
36 yCError(MOBVEL_NWC_YARP, "open() error could not open rpc port %s, check network", local_rpc_1.c_str());
37 return false;
38 }
39
40 bool ok = true;
41
42 ok = Network::connect(local_rpc_1, remote_rpc_1, m_carrier);
43 if (!ok)
44 {
45 yCError(MOBVEL_NWC_YARP, "open() error could not connect to %s", remote_rpc_1.c_str());
46 return false;
47 }
48
50 {
51 yCError(MOBVEL_NWC_YARP, "Error! Cannot attach the port as a client");
52 return false;
53 }
54
55 return true;
56}
57
59{
61 return true;
62}
63
64bool MobileBaseVelocityControl_nwc_yarp::getLastVelocityCommand(double& x_vel, double& y_vel, double& theta_vel)
65{
66 std::lock_guard <std::mutex> lg(m_mutex);
68 if (!ret.retvalue)
69 {
70 yCError(MOBVEL_NWC_YARP, "Unable to set transformation");
71 return false;
72 }
73 x_vel = ret.x_vel;
74 y_vel = ret.y_vel;
75 theta_vel = ret.theta_vel;
76 return true;
77}
78
79bool MobileBaseVelocityControl_nwc_yarp::applyVelocityCommand(double x_vel, double y_vel, double theta_vel, double timeout)
80{
81 std::lock_guard <std::mutex> lg(m_mutex);
82 return m_RPC.applyVelocityCommandRPC(x_vel,y_vel,theta_vel,timeout);
83}
bool ret
virtual bool applyVelocityCommandRPC(const double x_vel, const double y_vel, const double theta_vel, const double timeout)
virtual return_getLastVelocityCommand getLastVelocityCommandRPC()
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
bool close() override
Close the DeviceDriver.
bool applyVelocityCommand(double x_vel, double y_vel, double theta_vel, double timeout=0.1) override
Apply a velocity command.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
bool getLastVelocityCommand(double &x_vel, double &y_vel, double &theta_vel) override
Returns the last applied velocity command.
A mini-server for performing network communication in the background.
void close() override
Stop port activity.
Definition Port.cpp:363
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
yarp::os::WireLink & yarp()
Get YARP state associated with this object.
Definition Wire.h:28
#define yCError(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.