YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
MobileBaseVelocityControl_nws_ros2.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2023 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6#define _USE_MATH_DEFINES
8
9#include <yarp/os/Log.h>
11#include <yarp/os/LogStream.h>
12
14#include <Ros2Utils.h>
15
16#include <cmath>
17#include <mutex>
20using std::placeholders::_1;
21using namespace yarp::dev;
22using namespace yarp::dev::Nav2D;
23using namespace yarp::os;
24using namespace yarp::sig;
25
26namespace {
27 YARP_LOG_COMPONENT(MOBVEL_NWS_ROS2, "yarp.device.MobileBaseVelocityControl_nws_ros2")
28}
29
31{
32 parseParams(config);
33 if(m_namespace.empty()) {
35 } else {
37 }
38 if (m_node == nullptr) {
39 yCError(MOBVEL_NWS_ROS2) << " opening " << m_node_name << " Node, check your yarp-ROS2 network configuration\n";
40 return false;
41 }
42
43 m_ros2_subscriber = m_node->create_subscription<geometry_msgs::msg::Twist>(m_topic_name,
44 10,
45 std::bind(&MobileBaseVelocityControl_nws_ros2::twist_callback, this, _1));
46
47 if (!m_ros2_subscriber)
48 {
49 yCError(MOBVEL_NWS_ROS2) << " opening " << m_topic_name << " Topic, check your ROS2 network configuration\n";
50 return false;
51 }
52
53 m_spinner = new Ros2Spinner(m_node);
54 m_spinner->start();
55
56 yCInfo(MOBVEL_NWS_ROS2) << "Waiting for device to attach";
57
58 return true;
59}
60
61void MobileBaseVelocityControl_nws_ros2::twist_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
62{
63 this->m_iNavVel->applyVelocityCommand(msg->linear.x,
64 msg->linear.y,
65 msg->angular.z * 180 / M_PI);
66
67}
68
70{
71 m_iNavVel = nullptr;
72 return true;
73}
74
76{
77 if (driver->isValid())
78 {
79 driver->view(m_iNavVel);
80 }
81
82 if (nullptr == m_iNavVel)
83 {
84 yCError(MOBVEL_NWS_ROS2, "Subdevice passed to attach method is invalid");
85 return false;
86 }
87
88 yCInfo(MOBVEL_NWS_ROS2) << "Device attached";
89
90 return true;
91}
92
93
95{
96 yCInfo(MOBVEL_NWS_ROS2, "closing...");
97 delete m_spinner;
98 yCInfo(MOBVEL_NWS_ROS2, "closed");
99 return true;
100}
#define M_PI
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
bool close() override
Close the DeviceDriver.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
bool detach() override
Detach the object (you must have first called attach).
bool attach(yarp::dev::PolyDriver *driver) override
Attach to another object.
static rclcpp::Node::SharedPtr createNode(std::string name)
Definition Ros2Utils.cpp:9
bool view(T *&x)
Get an interface to the device driver.
virtual yarp::dev::ReturnValue 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 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.
Definition Searchable.h:31
bool start()
Start the new thread running.
Definition Thread.cpp:93
#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.