diff --git a/coresense_instrumentation/CHANGELOG.rst b/coresense_instrumentation/CHANGELOG.rst new file mode 100644 index 0000000..bf4cd09 --- /dev/null +++ b/coresense_instrumentation/CHANGELOG.rst @@ -0,0 +1,11 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package coresense_instrumentation +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.2.0 (2023-11-30) +------------------ +* Added new type: sensor_msgs/msg/Image with specialiced template + +0.1.0 (2023-11-29) +------------------ +* Driver works correctly diff --git a/coresense_instrumentation/package.xml b/coresense_instrumentation/package.xml index 4d2db93..f185844 100644 --- a/coresense_instrumentation/package.xml +++ b/coresense_instrumentation/package.xml @@ -2,7 +2,7 @@ coresense_instrumentation - 0.1.0 + 0.2.0 Coresense Instrumentation Juan Carlos Manzanares Serrano Apache-2.0 diff --git a/coresense_instrumentation_driver/CHANGELOG.rst b/coresense_instrumentation_driver/CHANGELOG.rst index c5d6b7e..74ee18a 100644 --- a/coresense_instrumentation_driver/CHANGELOG.rst +++ b/coresense_instrumentation_driver/CHANGELOG.rst @@ -1,7 +1,11 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package coresense_instrumentation +Changelog for package coresense_instrumentation_drived ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.0 (2023-11-30) +------------------ +* Added new type: sensor_msgs/msg/Image with specialiced template + 0.1.0 (2023-11-29) ------------------ * Driver works correctly diff --git a/coresense_instrumentation_driver/CMakeLists.txt b/coresense_instrumentation_driver/CMakeLists.txt index 133779e..6df5c95 100644 --- a/coresense_instrumentation_driver/CMakeLists.txt +++ b/coresense_instrumentation_driver/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(rclcpp_components REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(image_transport REQUIRED) set(dependencies rclcpp @@ -19,12 +20,14 @@ set(dependencies rclcpp_lifecycle std_msgs sensor_msgs + image_transport ) include_directories(include) add_library(${PROJECT_NAME} SHARED - src/coresense_instrumentation_driver/InstrumentationLifecycleNode.cpp # + src/coresense_instrumentation_driver/InstrumentationGeneric.cpp + src/coresense_instrumentation_driver/InstrumentationImage.cpp ) ament_target_dependencies(${PROJECT_NAME} ${dependencies}) @@ -32,6 +35,7 @@ ament_target_dependencies(${PROJECT_NAME} ${dependencies}) rclcpp_components_register_nodes(${PROJECT_NAME} "coresense_instrumentation_driver::InstrumentationLifecycleNode" "coresense_instrumentation_driver::InstrumentationLifecycleNode" + "coresense_instrumentation_driver::InstrumentationLifecycleNode" ) install(TARGETS diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp index 0d1350c..1756652 100644 --- a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp +++ b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp @@ -19,10 +19,14 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "std_msgs/msg/string.hpp" #include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "image_transport/image_transport.hpp" namespace coresense_instrumentation_driver { +using std::placeholders::_1; + template class InstrumentationLifecycleNode : public rclcpp_lifecycle::LifecycleNode { @@ -50,6 +54,36 @@ class InstrumentationLifecycleNode : public rclcpp_lifecycle::LifecycleNode std::string topic_type_; }; +template<> +class InstrumentationLifecycleNode: public rclcpp_lifecycle::LifecycleNode +{ +public: + InstrumentationLifecycleNode( + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + virtual ~InstrumentationLifecycleNode(); + + using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + CallbackReturnT on_configure(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_activate(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_deactivate(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_cleanup(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_shutdown(const rclcpp_lifecycle::State &) override; + + std::string get_topic(); + std::string get_topic_type(); + +private: + void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg); + + rclcpp::Node::SharedPtr node_; + image_transport::Publisher pub_; + image_transport::Subscriber sub_; + std::string topic_; + std::string topic_type_; +}; + } // namespace coresense_instrumentation_driver #endif // INSTRUMENTATION_LIFECYCLE_NODE_HPP diff --git a/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py b/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py index c395a6a..34f33c6 100644 --- a/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py +++ b/coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py @@ -19,9 +19,9 @@ def generate_launch_description(): - names = ['scan', 'chatter'] - topics = ['/scan', '/chatter'] - types = ['sensor_msgs::msg::LaserScan', 'std_msgs::msg::String'] + names = ['scan', 'chatter', 'image'] + topics = ['/scan', '/chatter', '/image'] + types = ['sensor_msgs::msg::LaserScan', 'std_msgs::msg::String', 'sensor_msgs::msg::Image'] composable_nodes = [] for topic, topic_type, name in zip(topics, types, names): diff --git a/coresense_instrumentation_driver/package.xml b/coresense_instrumentation_driver/package.xml index 6313f5a..9f45365 100644 --- a/coresense_instrumentation_driver/package.xml +++ b/coresense_instrumentation_driver/package.xml @@ -2,7 +2,7 @@ coresense_instrumentation_driver - 0.1.0 + 0.2.0 Coresense Instrumentation Driver Juan Carlos Manzanares Serrano Apache-2.0 @@ -14,6 +14,7 @@ rclcpp_components std_msgs sensor_msgs + image_transport ament_lint_auto ament_lint_common diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationLifecycleNode.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationGeneric.cpp similarity index 94% rename from coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationLifecycleNode.cpp rename to coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationGeneric.cpp index 9e82ad5..6b52f36 100644 --- a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationLifecycleNode.cpp +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationGeneric.cpp @@ -14,16 +14,17 @@ #include "coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp" -template class coresense_instrumentation_driver::InstrumentationLifecycleNode; -template class coresense_instrumentation_driver::InstrumentationLifecycleNode; namespace coresense_instrumentation_driver { +template class coresense_instrumentation_driver::InstrumentationLifecycleNode; +template class coresense_instrumentation_driver::InstrumentationLifecycleNode; + template InstrumentationLifecycleNode::InstrumentationLifecycleNode( const rclcpp::NodeOptions & options) -: rclcpp_lifecycle::LifecycleNode("InstrumentationLifecycleNode", "", options) +: rclcpp_lifecycle::LifecycleNode("lifecycle_node", "", options) { declare_parameter("topic", std::string("")); declare_parameter("topic_type", std::string("")); @@ -31,13 +32,13 @@ InstrumentationLifecycleNode::InstrumentationLifecycleNode( get_parameter("topic", topic_); get_parameter("topic_type", topic_type_); - RCLCPP_DEBUG(get_logger(), "Creating InstrumentationLifecycleNode"); + RCLCPP_INFO(get_logger(), "Creating InstrumentationGeneric"); } template InstrumentationLifecycleNode::~InstrumentationLifecycleNode() { - RCLCPP_DEBUG(get_logger(), "Destroying InstrumentationLifecycleNode"); + RCLCPP_DEBUG(get_logger(), "Destroying InstrumentationGeneric"); } template diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationImage.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationImage.cpp new file mode 100644 index 0000000..ded88de --- /dev/null +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationImage.cpp @@ -0,0 +1,110 @@ +// Copyright 2023 Intelligent Robotics Lab +// +// 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 "coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp" + +namespace coresense_instrumentation_driver +{ + +template class coresense_instrumentation_driver::InstrumentationLifecycleNode; + +InstrumentationLifecycleNode::InstrumentationLifecycleNode( + const rclcpp::NodeOptions & options) +: rclcpp_lifecycle::LifecycleNode("image_node", "", options) +{ + declare_parameter("topic", std::string("")); + declare_parameter("topic_type", std::string("")); + + get_parameter("topic", topic_); + get_parameter("topic_type", topic_type_); + + node_ = rclcpp::Node::make_shared("subnode"); + + RCLCPP_INFO(get_logger(), "Creating InstrumentationImage"); +} + +InstrumentationLifecycleNode::~InstrumentationLifecycleNode() +{ + RCLCPP_DEBUG(get_logger(), "Destroying InstrumentationImage"); +} + +void InstrumentationLifecycleNode::imageCallback( + const sensor_msgs::msg::Image::ConstSharedPtr & msg) +{ + if (pub_ && pub_.getNumSubscribers() > 0) { + pub_.publish(msg); + } +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationLifecycleNode::on_configure(const rclcpp_lifecycle::State &) +{ + auto subscription_options = rclcpp::SubscriptionOptions(); + + sub_ = image_transport::create_subscription( + node_.get(), + topic_, + std::bind( + &InstrumentationLifecycleNode::imageCallback, this, + std::placeholders::_1), + "raw", + rmw_qos_profile_sensor_data, + subscription_options); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationLifecycleNode::on_activate(const rclcpp_lifecycle::State &) +{ + image_transport::ImageTransport it(node_); + pub_ = it.advertise("/coresense" + topic_, 1); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationLifecycleNode::on_deactivate( + const rclcpp_lifecycle::State &) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationLifecycleNode::on_cleanup(const rclcpp_lifecycle::State &) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +InstrumentationLifecycleNode::on_shutdown(const rclcpp_lifecycle::State &) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +std::string InstrumentationLifecycleNode::get_topic() +{ + return topic_; +} + +std::string InstrumentationLifecycleNode::get_topic_type() +{ + return topic_type_; +} + +} // namespace coresense_instrumentation_driver + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE( + coresense_instrumentation_driver::InstrumentationLifecycleNode) diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp index 625be7b..3dbc20a 100644 --- a/coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp @@ -29,20 +29,23 @@ int main(int argc, char * argv[]) auto executor = std::make_shared(); - if (topic_type == "std_msgs::msg::String") { + if (topic_type == "sensor_msgs/msg/LaserScan") { + auto node = + std::make_shared>(); + executor->add_node(node->get_node_base_interface()); + executor->spin(); + } else if (topic_type == "std_msgs/msg/String") { auto node = std::make_shared>(); - executor->add_node(node->get_node_base_interface()); executor->spin(); - } else if (topic_type == "sensor_msgs::msg::LaserScan") { + } else if (topic_type == "sensor_msgs/msg/Image") { auto node = - std::make_shared>(); - + std::make_shared>(); executor->add_node(node->get_node_base_interface()); executor->spin(); } else { - RCLCPP_INFO(rclcpp::get_logger("main"), "Usage: %s ", argv[0]); + RCLCPP_ERROR(rclcpp::get_logger("main"), "Unknown topic type: %s", topic_type.c_str()); return 1; } diff --git a/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp b/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp index 5560416..e5ad660 100644 --- a/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp +++ b/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include "coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp" @@ -103,6 +104,48 @@ TEST_F(IntegrationTest, StringNodeLifecycle) executor.spin_once(std::chrono::seconds(1)); } +TEST_F(IntegrationTest, ImageNodeLifecycle) +{ + rclcpp::executors::SingleThreadedExecutor executor; + + auto node = + std::make_shared>( + rclcpp::NodeOptions().append_parameter_override( + "topic", + "/test_topic").append_parameter_override( + "topic_type", "sensor_msgs::msg::Image")); + + executor.add_node(node->get_node_base_interface()); + + auto result = node->on_configure(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + result = node->on_activate(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + node->on_activate(rclcpp_lifecycle::State()); + result = node->on_deactivate(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + result = node->on_cleanup(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + result = node->on_shutdown(rclcpp_lifecycle::State()); + ASSERT_EQ( + result, + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + + executor.spin_once(std::chrono::seconds(1)); +} + TEST_F(IntegrationTest, GetTopic) { auto node =