Skip to content
This repository has been archived by the owner on Jun 10, 2021. It is now read-only.

Add DummyMessage talker node and launch file for it #119

Merged
merged 3 commits into from Apr 3, 2020
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
13 changes: 12 additions & 1 deletion system_metrics_collector/CMakeLists.txt
Expand Up @@ -87,6 +87,10 @@ add_executable(linux_memory_collector src/system_metrics_collector/linux_memory_
target_link_libraries(linux_memory_collector ${PROJECT_NAME})
ament_target_dependencies(linux_memory_collector)

prajakta-gokhale marked this conversation as resolved.
Show resolved Hide resolved
add_executable(dummy_talker src/topic_statistics_collector/dummy_talker.cpp)
target_link_libraries(dummy_talker system_metrics_collector)
ament_target_dependencies(dummy_talker)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
find_package(ament_cmake_gtest REQUIRED)
Expand Down Expand Up @@ -140,8 +144,9 @@ if(BUILD_TESTING)
ament_target_dependencies(test_subscriber_topic_statistics rcl rclcpp)
endif()

# To enable use of dummy_message.hpp in test_subscriber_topic_statistics
# To enable use of dummy_message.hpp in executables
rosidl_target_interfaces(test_subscriber_topic_statistics system_metrics_collector_test_msgs "rosidl_typesupport_cpp")
rosidl_target_interfaces(dummy_talker system_metrics_collector_test_msgs "rosidl_typesupport_cpp")

# Install launch files
install(DIRECTORY
Expand All @@ -168,6 +173,12 @@ install(TARGETS
lib/${PROJECT_NAME}
)

install(TARGETS
dummy_talker
DESTINATION
lib/${PROJECT_NAME}
)

install(TARGETS
${PROJECT_NAME}
DESTINATION
Expand Down
@@ -0,0 +1,27 @@
# Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

"""Launch file to launch a DummyMessage publisher node."""

from launch import LaunchDescription
import launch_ros.actions


def generate_launch_description():
return LaunchDescription([
launch_ros.actions.Node(
package='system_metrics_collector',
node_executable='dummy_talker',
output='screen'),
])
@@ -0,0 +1,76 @@
// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <memory>
#include <mutex>
#include <utility>

#include "system_metrics_collector/msg/dummy_message.hpp"

#include "rclcpp/rclcpp.hpp"

/**
* A class that publishes DummyMessages every second with the Header set to current time.
*/
class DummyTalker : public rclcpp::Node
{
public:
DummyTalker()
: Node("dummy_talker")
{
using namespace std::chrono_literals;
auto publish_lambda =
[this]() -> void
{
std::unique_lock<std::mutex> ulock{mutex_};

msg_ = std::make_unique<system_metrics_collector::msg::DummyMessage>();
msg_->header = std_msgs::msg::Header{};
msg_->header.stamp = this->now();
RCLCPP_INFO(
this->get_logger(),
"Publishing header: %lu",
msg_->header.stamp.nanosec);

publisher_->publish(std::move(msg_));
};

publisher_ = this->create_publisher<system_metrics_collector::msg::DummyMessage>(
"dummy_data",
10 /* QoS history_depth */);
timer_ = this->create_wall_timer(1s, publish_lambda);
}

private:
std::unique_ptr<system_metrics_collector::msg::DummyMessage> msg_;
rclcpp::Publisher<system_metrics_collector::msg::DummyMessage>::SharedPtr publisher_;
rclcpp::TimerBase::SharedPtr timer_;
mutable std::mutex mutex_;
};

int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);

const auto dummy_talker =
std::make_shared<DummyTalker>();

rclcpp::executors::SingleThreadedExecutor ex;
ex.add_node(dummy_talker->get_node_base_interface());
ex.spin();

rclcpp::shutdown();

return 0;
}