Skip to content

Commit 05a5864

Browse files
ste93ste93
committed
added simple publisher and subscriber device for test
1 parent 6c2edf7 commit 05a5864

File tree

8 files changed

+432
-0
lines changed

8 files changed

+432
-0
lines changed

‎src/devices/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,3 +13,5 @@ add_subdirectory(map2D_nws_ros2)
1313
add_subdirectory(frameGrabber_nws_ros2)
1414
add_subdirectory(rgbdToPointCloudSensor_nws_ros2)
1515
add_subdirectory(mobileBaseVelocityControl_nws_ros2)
16+
add_subdirectory(ros2SubscriptionTest)
17+
add_subdirectory(ros2PublishTest)

‎src/devices/ros2PublishTest/CMakeLists.txt‎

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
# Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
2+
# All rights reserved.
3+
#
4+
# This software may be modified and distributed under the terms of the
5+
# BSD-3-Clause license. See the accompanying LICENSE file for details.
6+
7+
yarp_prepare_plugin(ros2PublishTest
8+
CATEGORY device
9+
TYPE Ros2PublishTest
10+
INCLUDE Ros2PublishTest.h
11+
INTERNAL ON
12+
)
13+
14+
yarp_add_plugin(yarp_ros2PublishTest)
15+
16+
target_sources(yarp_ros2PublishTest
17+
PRIVATE
18+
Ros2PublishTest.cpp
19+
Ros2PublishTest.h
20+
)
21+
22+
target_link_libraries(yarp_ros2PublishTest
23+
PRIVATE
24+
YARP::YARP_os
25+
YARP::YARP_sig
26+
YARP::YARP_dev
27+
rclcpp::rclcpp
28+
std_msgs::std_msgs__rosidl_typesupport_cpp
29+
)
30+
31+
yarp_install(
32+
TARGETS yarp_ros2PublishTest
33+
EXPORT yarp-device-ros2PublishTest
34+
COMPONENT yarp-device-ros2PublishTest
35+
LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR}
36+
ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}
37+
YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}
38+
)
39+
40+
set_property(TARGET yarp_ros2PublishTest PROPERTY FOLDER "Plugins/Device")

‎src/devices/ros2PublishTest/Ros2PublishTest.cpp‎

Lines changed: 81 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
/*
2+
* Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
3+
* All rights reserved.
4+
*
5+
* This software may be modified and distributed under the terms of the
6+
* BSD-3-Clause license. See the accompanying LICENSE file for details.
7+
*/
8+
9+
#include "Ros2PublishTest.h"
10+
11+
#include <yarp/os/LogComponent.h>
12+
#include <yarp/os/LogStream.h>
13+
14+
#include <chrono>
15+
#include <functional>
16+
#include <memory>
17+
#include <string>
18+
19+
using namespace std::chrono_literals;
20+
21+
22+
YARP_LOG_COMPONENT(ROS2PUBLISHTEST, "yarp.ros2.ros2PublishTest", yarp::os::Log::TraceType);
23+
24+
MinimalPublisher::MinimalPublisher(std::string name, std::string topic)
25+
: Node(name)
26+
{
27+
publisher_ = this->create_publisher<std_msgs::msg::String>(topic, 10);
28+
}
29+
30+
void MinimalPublisher::publish(std::string messageString) {
31+
auto message = std_msgs::msg::String();
32+
message.data = messageString;
33+
publisher_->publish(message);
34+
}
35+
36+
Ros2PublishTest::Ros2PublishTest():
37+
PeriodicThread(0.033)
38+
{
39+
}
40+
41+
bool Ros2PublishTest::open(yarp::os::Searchable& config)
42+
{
43+
yCTrace(ROS2PUBLISHTEST);
44+
if (config.check("node_name"))
45+
{
46+
m_node_name = config.find("node_name").asString();
47+
} else {
48+
yCError(ROS2PUBLISHTEST) << "missing node_name parameter";
49+
return false;
50+
}
51+
52+
if (config.check("topic_name"))
53+
{
54+
m_topic = config.find("topic_name").asString();
55+
} else {
56+
yCError(ROS2PUBLISHTEST) << "missing topic_name parameter";
57+
return false;
58+
}
59+
60+
61+
if(!rclcpp::ok())
62+
{
63+
rclcpp::init(/*argc*/ 0, /*argv*/ nullptr);
64+
}
65+
minimalPublisher = new MinimalPublisher(m_node_name, m_topic) ;
66+
67+
start();
68+
return true;
69+
}
70+
71+
bool Ros2PublishTest::close()
72+
{
73+
yCInfo(ROS2PUBLISHTEST, "Ros2PublishTest::close");
74+
rclcpp::shutdown();
75+
return true;
76+
}
77+
78+
void Ros2PublishTest::run()
79+
{
80+
minimalPublisher->publish("Hello from " + m_topic + "! " + m_node_name);
81+
}

‎src/devices/ros2PublishTest/Ros2PublishTest.h‎

Lines changed: 56 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,56 @@
1+
/*
2+
* Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
3+
* All rights reserved.
4+
*
5+
* This software may be modified and distributed under the terms of the
6+
* BSD-3-Clause license. See the accompanying LICENSE file for details.
7+
*/
8+
9+
#ifndef YARP_ROS2_ROS2PUBLISHTEST_H
10+
#define YARP_ROS2_ROS2PUBLISHTEST_H
11+
12+
#include <yarp/dev/DeviceDriver.h>
13+
#include <yarp/os/PeriodicThread.h>
14+
15+
#include <rclcpp/rclcpp.hpp>
16+
#include <std_msgs/msg/string.hpp>
17+
18+
#include <mutex>
19+
20+
class MinimalPublisher : public rclcpp::Node
21+
{
22+
public:
23+
MinimalPublisher(std::string name, std::string topic);
24+
void publish(std::string messageString);
25+
26+
private:
27+
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
28+
};
29+
30+
class Ros2PublishTest :
31+
public yarp::dev::DeviceDriver,
32+
public yarp::os::PeriodicThread
33+
{
34+
public:
35+
Ros2PublishTest();
36+
Ros2PublishTest(const Ros2PublishTest&) = delete;
37+
Ros2PublishTest(Ros2PublishTest&&) noexcept = delete;
38+
Ros2PublishTest& operator=(const Ros2PublishTest&) = delete;
39+
Ros2PublishTest& operator=(Ros2PublishTest&&) noexcept = delete;
40+
~Ros2PublishTest() override = default;
41+
42+
// DeviceDriver
43+
bool open(yarp::os::Searchable& config) override;
44+
bool close() override;
45+
46+
// PeriodicThread
47+
void run() override;
48+
49+
private:
50+
51+
MinimalPublisher* minimalPublisher;
52+
std::string m_node_name;
53+
std::string m_topic;
54+
};
55+
56+
#endif // YARP_ROS2_ROS2PUBLISHTEST_H

‎src/devices/ros2PublishTest/aaa.xml‎

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
3+
<robot name="CER-SIM-DEPTH" prefix="CER-SIM-DEPTH" xmlns:xi="http://www.w3.org/2001/XInclude">
4+
<devices>
5+
6+
<device xmlns:xi="http://www.w3.org/2001/XInclude" name="subscriber1" type="ros2SubscriptionTest">
7+
<param name="node_name">node1</param>
8+
<param name="topic_name">topic_node1</param>
9+
10+
</device>
11+
12+
<device xmlns:xi="http://www.w3.org/2001/XInclude" name="subscriber21" type="ros2SubscriptionTest">
13+
<param name="node_name">node2</param>
14+
<param name="topic_name">topic_node2</param>
15+
16+
</device>
17+
<device xmlns:xi="http://www.w3.org/2001/XInclude" name="publisher1" type="ros2PublishTest">
18+
<param name="node_name">node3</param>
19+
<param name="topic_name">topic_node1</param>
20+
21+
</device>
22+
<device xmlns:xi="http://www.w3.org/2001/XInclude" name="publisher1" type="ros2PublishTest">
23+
<param name="node_name">node4</param>
24+
<param name="topic_name">topic_node2</param>
25+
26+
</device>
27+
28+
29+
</devices>
30+
</robot>
31+

‎src/devices/ros2SubscriptionTest/CMakeLists.txt‎

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
# Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
2+
# All rights reserved.
3+
#
4+
# This software may be modified and distributed under the terms of the
5+
# BSD-3-Clause license. See the accompanying LICENSE file for details.
6+
7+
yarp_prepare_plugin(ros2SubscriptionTest
8+
CATEGORY device
9+
TYPE Ros2SubscriptionTest
10+
INCLUDE Ros2SubscriptionTest.h
11+
INTERNAL ON
12+
)
13+
14+
yarp_add_plugin(yarp_ros2SubscriptionTest)
15+
16+
target_sources(yarp_ros2SubscriptionTest
17+
PRIVATE
18+
Ros2SubscriptionTest.cpp
19+
Ros2SubscriptionTest.h
20+
)
21+
22+
target_link_libraries(yarp_ros2SubscriptionTest
23+
PRIVATE
24+
YARP::YARP_os
25+
YARP::YARP_sig
26+
YARP::YARP_dev
27+
rclcpp::rclcpp
28+
std_msgs::std_msgs__rosidl_typesupport_cpp
29+
)
30+
31+
yarp_install(
32+
TARGETS yarp_ros2SubscriptionTest
33+
EXPORT yarp-device-ros2SubscriptionTest
34+
COMPONENT yarp-device-ros2SubscriptionTest
35+
LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR}
36+
ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}
37+
YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}
38+
)
39+
40+
set_property(TARGET yarp_ros2SubscriptionTest PROPERTY FOLDER "Plugins/Device")

‎src/devices/ros2SubscriptionTest/Ros2SubscriptionTest.cpp‎

Lines changed: 106 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,106 @@
1+
/*
2+
* Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
3+
* All rights reserved.
4+
*
5+
* This software may be modified and distributed under the terms of the
6+
* BSD-3-Clause license. See the accompanying LICENSE file for details.
7+
*/
8+
9+
#include "Ros2SubscriptionTest.h"
10+
11+
#include <yarp/os/LogComponent.h>
12+
#include <yarp/os/LogStream.h>
13+
14+
#include <chrono>
15+
#include <functional>
16+
#include <memory>
17+
#include <string>
18+
19+
using namespace std::chrono_literals;
20+
using std::placeholders::_1;
21+
22+
YARP_LOG_COMPONENT(ROS2SUBSCRIPTIONTEST, "yarp.ros2.ros2SubscriptionTest", yarp::os::Log::TraceType);
23+
24+
class MinimalSubscriber : public rclcpp::Node
25+
{
26+
public:
27+
MinimalSubscriber(std::string name, Ros2SubscriptionTest *subscriptionTest, std::string topic)
28+
: Node(name)
29+
{
30+
subscription_ = this->create_subscription<std_msgs::msg::String>(
31+
topic, 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
32+
m_subscriptionTest = subscriptionTest;
33+
}
34+
35+
private:
36+
void topic_callback(const std_msgs::msg::String::SharedPtr msg) const
37+
{
38+
m_subscriptionTest->local_callback(msg);
39+
}
40+
Ros2SubscriptionTest *m_subscriptionTest;
41+
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
42+
};
43+
44+
45+
46+
47+
48+
//Ros2Init::Ros2Init()
49+
//{
50+
// rclcpp::init(/*argc*/ 0, /*argv*/ nullptr);
51+
// node = std::make_shared<rclcpp::Node>("yarprobotinterface_node");
52+
//}
53+
//
54+
//Ros2Init& Ros2Init::get()
55+
//{
56+
// static Ros2Init instance;
57+
// return instance;
58+
//}
59+
60+
61+
Ros2SubscriptionTest::Ros2SubscriptionTest()
62+
{
63+
}
64+
65+
bool Ros2SubscriptionTest::open(yarp::os::Searchable& config)
66+
{
67+
if (config.check("node_name"))
68+
{
69+
m_node_name = config.find("node_name").asString();
70+
} else {
71+
yCError(ROS2SUBSCRIPTIONTEST) << "missing node_name parameter";
72+
return false;
73+
}
74+
75+
if (config.check("topic_name"))
76+
{
77+
m_topic = config.find("topic_name").asString();
78+
} else {
79+
yCError(ROS2SUBSCRIPTIONTEST) << "missing topic_name parameter";
80+
return false;
81+
}
82+
start();
83+
return true;
84+
}
85+
86+
bool Ros2SubscriptionTest::close()
87+
{
88+
yCTrace(ROS2SUBSCRIPTIONTEST);
89+
yCInfo(ROS2SUBSCRIPTIONTEST, "Ros2SubscriptionTest::close");
90+
return true;
91+
}
92+
93+
void Ros2SubscriptionTest::local_callback(const std_msgs::msg::String::SharedPtr msg) const
94+
{
95+
yInfo() << "hello, I'm " + m_node_name + m_topic + ", I heard: '%s'" << msg->data.c_str();
96+
}
97+
98+
void Ros2SubscriptionTest::run()
99+
{
100+
if(!rclcpp::ok())
101+
{
102+
rclcpp::init(/*argc*/ 0, /*argv*/ nullptr);
103+
}
104+
rclcpp::spin(std::make_shared<MinimalSubscriber>(m_node_name, this, m_topic));
105+
rclcpp::shutdown();
106+
}

0 commit comments

Comments
 (0)