Skip to content

Commit f9048d6

Browse files
author
Javier Moreno
committed
Refs #3364
1 parent 90ef71a commit f9048d6

21 files changed

Lines changed: 379 additions & 153 deletions

File tree

‎actuator/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -14,4 +14,6 @@ install(TARGETS
1414
DESTINATION lib/${PROJECT_NAME}
1515
)
1616

17+
target_link_libraries(${PROJECT_NAME} micrortps_client microcdr)
18+
1719
ament_package()

‎actuator/main.c‎

Lines changed: 34 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -1,59 +1,61 @@
11
#include <rclc/rclc.h>
22
#include <std_msgs/msg/string.h>
3-
#include <std_msgs/msg/float64.h>
3+
#include <std_msgs/msg/int32.h>
4+
#include <std_msgs/msg/u_int32.h>
45

56
#include <stdio.h>
67

7-
static rclc_publisher_t* publisher;
8+
static rclc_publisher_t* engine_pub;
9+
static uint32_t engine_power = 100;
810

9-
10-
void altitude_on_message(const void* msgin)
11+
void engine_on_message(const void* msgin)
1112
{
12-
const std_msgs__msg__Float64* msg = (const std_msgs__msg__Float64*)msgin;
13-
14-
// Publish warning if value is less than 1000
15-
if (msg->data <= 500)
13+
14+
const std_msgs__msg__Int32* msg = (const std_msgs__msg__Int32*)msgin;
15+
16+
if (msg->data + engine_power > 0)
1617
{
17-
std_msgs__msg__String Warning;
18-
Warning.data.data = "Failure!!";
19-
Warning.data.size = strlen(Warning.data.data);
20-
Warning.data.capacity = strlen(Warning.data.data);
21-
22-
rclc_publish(publisher, (const void*)&Warning);
18+
engine_power += msg->data;
2319
}
24-
else if (msg->data <= 1000)
20+
else
2521
{
26-
std_msgs__msg__String Warning;
27-
Warning.data.data = "Warning!!";
28-
Warning.data.size = strlen(Warning.data.data);
29-
Warning.data.capacity = strlen(Warning.data.data);
30-
31-
rclc_publish(publisher, (const void*)&Warning);
22+
engine_power = 0;
3223
}
33-
}
34-
3524

36-
void engine_on_message(const void* msgin)
37-
{
38-
const std_msgs__msg__Float64* msg = (const std_msgs__msg__Float64*)msgin;
25+
// Publish new altitude
26+
std_msgs__msg__UInt32 msg_out;
27+
msg_out.data = engine_power;
28+
rclc_publish(engine_pub, (const void*)&msg_out);
3929
}
4030

4131
int main(int argc, char* argv[])
4232
{
4333
(void)argc;
4434
(void)argv;
35+
36+
rclc_node_t* node = NULL;
37+
rclc_subscription_t* engine_sub = NULL;
38+
39+
40+
4541
rclc_init(0, NULL);
46-
rclc_node_t* node = rclc_create_node("actuator", "");
47-
rclc_subscription_t* altitude_sub = rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64), "position_sensor", altitude_on_message, 1, false);
48-
rclc_subscription_t* engine_sub = rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64), "engine_power", engine_on_message, 1, false);
49-
publisher = rclc_create_publisher(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String), "status_report", 1);
42+
node = rclc_create_node("actuator", "");
43+
engine_sub = rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "std_msgs_msg_Int32", engine_on_message, 1, false);
44+
engine_pub = rclc_create_publisher(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, UInt32), "std_msgs_msg_UInt32", 1);
45+
46+
// Publish new altitude
47+
std_msgs__msg__UInt32 msg_out;
48+
msg_out.data = engine_power;
49+
rclc_publish(engine_pub, (const void*)&msg_out);
5050

5151
rclc_spin_node(node);
5252

53-
if (altitude_sub) rclc_destroy_subscription(altitude_sub);
53+
//if (altitude_sub) rclc_destroy_subscription(altitude_sub);
5454
if (engine_sub) rclc_destroy_subscription(engine_sub);
55-
if (publisher) rclc_destroy_publisher(publisher);
55+
if (engine_pub) rclc_destroy_publisher(engine_pub);
5656
if (node) rclc_destroy_node(node);
5757

58+
printf("Actuator node closed.\n");
59+
5860
return 0;
5961
}

position_sensor/CMakeLists.txt renamed to altitude_sensor/CMakeLists.txt

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,19 +1,21 @@
11
cmake_minimum_required(VERSION 3.5)
22
set(CMAKE_CXX_CLANG_TIDY clang-tidy -checks=*)
3-
project(position_sensor)
3+
project(altitude_sensor)
44

55
find_package(ament_cmake REQUIRED)
66
find_package(rclc REQUIRED)
77
find_package(std_msgs REQUIRED)
88

9-
add_executable(position_sensor main.c)
10-
ament_target_dependencies(position_sensor rclc std_msgs)
9+
add_executable(altitude_sensor main.c)
10+
ament_target_dependencies(altitude_sensor rclc std_msgs)
1111

12-
target_link_libraries(position_sensor m)
12+
target_link_libraries(altitude_sensor m)
1313

1414
install(TARGETS
15-
position_sensor
15+
altitude_sensor
1616
DESTINATION lib/${PROJECT_NAME}
1717
)
1818

19+
target_link_libraries(${PROJECT_NAME} micrortps_client microcdr)
20+
1921
ament_package()

‎altitude_sensor/main.c‎

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
#include <rclc/rclc.h>
2+
#include <std_msgs/msg/float64.h>
3+
#include <std_msgs/msg/u_int32.h>
4+
5+
#include <stdio.h>
6+
#include <math.h>
7+
#include <time.h>
8+
9+
10+
static uint32_t Offset = 0;
11+
12+
/**
13+
* @brief
14+
*
15+
* @param msgin
16+
*/
17+
void on_power_message(const void* msgin)
18+
{
19+
std_msgs__msg__UInt32* msg = (std_msgs__msg__UInt32*)msgin;
20+
21+
Offset = msg->data;
22+
}
23+
24+
25+
/**
26+
* @brief
27+
*
28+
* @param argc
29+
* @param argv
30+
* @return int
31+
*/
32+
int main(int argc, char *argv[])
33+
{
34+
(void)argc;
35+
(void)argv;
36+
rclc_init(0, NULL);
37+
38+
rclc_node_t* node = NULL;
39+
rclc_publisher_t* publisher = NULL;
40+
rclc_subscription_t* power_subscription = NULL;
41+
42+
43+
node = rclc_create_node("altitude_sensor", "");
44+
publisher = rclc_create_publisher(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64), "std_msgs_msg_Float64", 1);
45+
//power_subscription = rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, UInt32), "std_msgs_msg_UInt32", on_power_message, 1, false);
46+
47+
double Altitude = 1000;
48+
double Count = 0;
49+
time_t t;
50+
srand((unsigned) time(&t));
51+
52+
while (rclc_ok())
53+
{
54+
Count += 0.1;
55+
Altitude += (rand() % 10) * cos(Count);
56+
57+
58+
// Publish new altitude
59+
std_msgs__msg__Float64 msg;
60+
msg.data = Altitude + Offset;
61+
rclc_publish(publisher, (const void*)&msg);
62+
63+
64+
// Spin node
65+
rclc_spin_node_once(node, 1);
66+
}
67+
68+
if (publisher) rclc_destroy_publisher(publisher);
69+
if (power_subscription) rclc_destroy_publisher(power_subscription);
70+
if (node) rclc_destroy_node(node);
71+
72+
printf("altitude sendor closed.\n");
73+
return 0;
74+
}

position_sensor/package.xml renamed to altitude_sensor/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<?xml version="1.0"?>
22
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
4-
<name>position_sensor</name>
4+
<name>altitude_sensor</name>
55
<version>0.0.1</version>
66
<description>Demo. Position sensor node</description>
77
<maintainer email="javiermoreno@eprosima.com">Javier Moreno</maintainer>

external_node/CMakeLists.txt renamed to control/CMakeLists.txt

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
cmake_minimum_required(VERSION 3.5)
2-
project(external_node)
2+
project(control)
33

44
# Default to C++14
55
if(NOT CMAKE_CXX_STANDARD)
@@ -14,11 +14,11 @@ find_package(ament_cmake REQUIRED)
1414
find_package(rclcpp REQUIRED)
1515
find_package(std_msgs REQUIRED)
1616

17-
add_executable(external_node main.cpp)
18-
ament_target_dependencies(external_node rclcpp std_msgs)
17+
add_executable(control main.cpp)
18+
ament_target_dependencies(control rclcpp std_msgs)
1919

2020
install(TARGETS
21-
external_node
21+
control
2222
DESTINATION lib/${PROJECT_NAME}
2323
)
2424

external_node/COLCON_IGNORE renamed to control/COLCON_IGNORE

File renamed without changes.

external_node/README.md renamed to control/README.md

File renamed without changes.

‎control/main.cpp‎

Lines changed: 83 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,83 @@
1+
// Copyright 2016 Open Source Robotics Foundation, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "rclcpp/rclcpp.hpp"
16+
#include "std_msgs/msg/float64.hpp"
17+
#include "std_msgs/msg/int32.hpp"
18+
#include "std_msgs/msg/string.hpp"
19+
using std::placeholders::_1;
20+
21+
class ExternalNode : public rclcpp::Node
22+
{
23+
public:
24+
ExternalNode() : Node("control")
25+
{
26+
engine_power_publisher_ = this->create_publisher<std_msgs::msg::Int32>("std_msgs_msg_Int32");
27+
warn_publisher_ = this->create_publisher<std_msgs::msg::String>("std_msgs_msg_String");
28+
29+
altitude_subscription_ = this->create_subscription<std_msgs::msg::Float64>("std_msgs_msg_Float64", std::bind(&ExternalNode::topic_callback, this, _1));
30+
}
31+
32+
private:
33+
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr engine_power_publisher_;
34+
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr warn_publisher_;
35+
rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr altitude_subscription_;
36+
37+
void topic_callback(const std_msgs::msg::Float64::SharedPtr msg)
38+
{
39+
// Warning
40+
if (msg->data <= 500)
41+
{
42+
{
43+
auto message = std_msgs::msg::String();
44+
message.data = "Failure!!";
45+
RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str())
46+
this->warn_publisher_->publish(message);
47+
}
48+
49+
{
50+
auto message = std_msgs::msg::Int32();
51+
message.data = 10;
52+
this->engine_power_publisher_->publish(message);
53+
}
54+
55+
}
56+
57+
// Failure
58+
else if (msg->data <= 1000)
59+
{
60+
{
61+
auto message = std_msgs::msg::String();
62+
message.data = "Warning!!";
63+
RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str())
64+
this->warn_publisher_->publish(message);
65+
}
66+
67+
{
68+
auto message = std_msgs::msg::Int32();
69+
message.data = 5;
70+
this->engine_power_publisher_->publish(message);
71+
}
72+
73+
}
74+
}
75+
};
76+
77+
int main(int argc, char * argv[])
78+
{
79+
rclcpp::init(argc, argv);
80+
rclcpp::spin(std::make_shared<ExternalNode>());
81+
rclcpp::shutdown();
82+
return 0;
83+
}

external_node/package.xml renamed to control/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<?xml version="1.0"?>
22
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="2">
4-
<name>external_node</name>
4+
<name>control</name>
55
<version>0.0.1</version>
66
<description>Demo. Node running on ros2</description>
77
<maintainer email="javiermoreno@eprosima.com">Javier Moreno</maintainer>

0 commit comments

Comments
 (0)