YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
MobileBaseVelocityControl_nws_ros.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: LGPL-2.1-or-later
4 */
5
6#define _USE_MATH_DEFINES
7#include <cmath>
8
11#include <yarp/os/Log.h>
13#include <yarp/os/LogStream.h>
14#include <mutex>
15
18using namespace yarp::dev;
19using namespace yarp::dev::Nav2D;
20using namespace yarp::os;
21using namespace yarp::sig;
22
23namespace {
24 YARP_LOG_COMPONENT(MOBVEL_NWS_ROS, "yarp.device.MobileBaseVelocityControl_nws_ros")
25}
26
27//------------------------------------------------------------------------------------------------------------------------------
28
33
35{
36 m_iNavVel = nullptr;
37}
38
42
47
49{
50 if (m_iNavVel)
51 {
53 }
54 else
55 {
56 yCError(MOBVEL_NWS_ROS, "Subdevice interface not yet initialized");
57 }
58}
59
60//------------------------------------------------------------------------------------------------------------------------------
61
63{
64 if (config.check("node_name") == true)
65 {
66 m_ros_node_name = config.find("node_name").asString();
67 }
68
69 if (config.check("topic_name") == true)
70 {
71 m_ros_topic_name = config.find("topic_name").asString();
72 }
73
74 yCInfo(MOBVEL_NWS_ROS) << "Waiting for device to be attached";
75
76 //open the subscriber
79
81 {
82 yCError(MOBVEL_NWS_ROS) << " opening " << m_ros_topic_name << " Topic, check your yarp-ROS network configuration\n";
83 return false;
84 }
85
86 //m_command_subscriber->setStrict();
88
89 return true;
90}
91
93{
95 if (m_ros_node) {delete m_ros_node;}
96 if (m_subdev.isValid()) { m_subdev.close(); }
97 return true;
98}
99
100
101bool MobileBaseVelocityControl_nws_ros::detach()
102{
104 return true;
105}
106
107bool MobileBaseVelocityControl_nws_ros::attach(PolyDriver* driver)
108{
110
111 if (driver->isValid())
112 {
113 driver->view(iNavVel);
114 }
115
116 if (nullptr == iNavVel)
117 {
118 yCError(MOBVEL_NWS_ROS, "Unable to view INavigation2DVelocityActions interface. Attach failed.");
119 return false;
120 }
121
123
124 return true;
125}
bool close() override
Close the DeviceDriver.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
yarp::dev::Nav2D::INavigation2DVelocityActions * m_iNavVel
virtual void onRead(yarp::rosmsg::geometry_msgs::Twist &v) override
void init(yarp::dev::Nav2D::INavigation2DVelocityActions *m_iNavVel)
bool view(T *&x)
Get an interface to the device driver.
virtual bool applyVelocityCommand(double x_vel, double y_vel, double theta_vel, double timeout=0.1)=0
Apply a velocity command.
A container for a device driver.
Definition PolyDriver.h:23
bool close() override
Close the DeviceDriver.
bool isValid() const
Check if device is valid.
A mini-server for performing network communication in the background.
The Node class.
Definition Node.h:23
A base class for nested structures that can be searched.
Definition Searchable.h:56
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
bool topic(const std::string &name)
Set topic to subscribe to.
Definition Subscriber.h:62
void useCallback(TypedReaderCallback< T > &callback)
Definition Subscriber.h:135
yarp::rosmsg::geometry_msgs::Vector3 angular
Definition Twist.h:33
yarp::rosmsg::geometry_msgs::Vector3 linear
Definition Twist.h:32
yarp::conf::float64_t y
Definition Vector3.h:38
yarp::conf::float64_t z
Definition Vector3.h:39
yarp::conf::float64_t x
Definition Vector3.h:37
#define M_PI
#define yCInfo(component,...)
#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.