From 6e52e713af9c49723ec1c2149903e6bb7a07633b Mon Sep 17 00:00:00 2001 From: BrOleg5 Date: Fri, 19 Jul 2024 19:37:42 +0700 Subject: [PATCH] Update CameraROS and RTOCameraNode classes. Add camera_info publisher to CameraROS. Rename RTOCameraNode to RobotinoCameraNode. --- robotino_node/CMakeLists.txt | 20 ++++- .../include/robotino_node/CameraROS.h | 37 +++++---- .../include/robotino_node/RTOCameraNode.h | 36 --------- .../robotino_node/RobotinoCameraNode.h | 30 ++++++++ robotino_node/src/CameraROS.cpp | 76 ++++++------------- robotino_node/src/RTOCameraNode.cpp | 53 ------------- robotino_node/src/RobotinoCameraNode.cpp | 27 +++++++ robotino_node/src/robotino_camera_node.cpp | 18 +++++ robotino_node/src/rto_camera_node.cpp | 17 ----- 9 files changed, 134 insertions(+), 180 deletions(-) delete mode 100644 robotino_node/include/robotino_node/RTOCameraNode.h create mode 100644 robotino_node/include/robotino_node/RobotinoCameraNode.h delete mode 100644 robotino_node/src/RTOCameraNode.cpp create mode 100644 robotino_node/src/RobotinoCameraNode.cpp create mode 100644 robotino_node/src/robotino_camera_node.cpp delete mode 100644 robotino_node/src/rto_camera_node.cpp diff --git a/robotino_node/CMakeLists.txt b/robotino_node/CMakeLists.txt index 85a76af..54d27e3 100644 --- a/robotino_node/CMakeLists.txt +++ b/robotino_node/CMakeLists.txt @@ -59,6 +59,23 @@ target_include_directories( ament_target_dependencies(robotino_node rclcpp rto_msgs std_msgs geometry_msgs sensor_msgs tf2 tf2_ros nav_msgs builtin_interfaces) +add_executable( + robotino_camera_node + src/robotino_camera_node.cpp + src/RobotinoCameraNode.cpp + src/ComROS.cpp + src/CameraROS.cpp) + +target_link_libraries(robotino_camera_node ${REC_ROBOTINO_API2_LIBRARY}) + +target_include_directories( + robotino_camera_node PUBLIC + $ + $ +) + +ament_target_dependencies(robotino_camera_node rclcpp sensor_msgs) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) set(ament_cmake_copyright_FOUND TRUE) @@ -68,8 +85,7 @@ endif() install(TARGETS robotino_node - # rto_camera_node - # rto_laserrangefinder_node + robotino_camera_node DESTINATION lib/${PROJECT_NAME}) ament_package() diff --git a/robotino_node/include/robotino_node/CameraROS.h b/robotino_node/include/robotino_node/CameraROS.h index d760187..98cf68e 100644 --- a/robotino_node/include/robotino_node/CameraROS.h +++ b/robotino_node/include/robotino_node/CameraROS.h @@ -3,6 +3,8 @@ * * Created on: 08.12.2011 * Author: indorewala@servicerobotics.eu + * Edited on: 19.07.2024 + * Author: BrOleg5 */ #ifndef CAMERAROS_H_ @@ -14,29 +16,24 @@ #include "sensor_msgs/msg/image.hpp" #include "sensor_msgs/msg/camera_info.hpp" -class CameraROS : public rec::robotino::api2::Camera -{ -public: - CameraROS(); - ~CameraROS(); +class CameraROS : public rec::robotino::api2::Camera { + public: + CameraROS(rclcpp::Node* parent_node_ptr); + ~CameraROS() {} - void setNumber( int number, rclcpp::Node * parent_node ); - void setTimeStamp(rclcpp::Time stamp); + private: + rclcpp::Clock::SharedPtr clock_ptr_; + rclcpp::Publisher::SharedPtr streaming_pub_; + rclcpp::Publisher::SharedPtr caminfor_pub_; -private: - rclcpp::Publisher::SharedPtr streaming_pub_; - - sensor_msgs::msg::Image img_msg_; - - rclcpp::Time stamp_; - - void imageReceivedEvent( - const unsigned char* data, - unsigned int dataSize, - unsigned int width, - unsigned int height, - unsigned int step ); + sensor_msgs::msg::Image img_msg_; + sensor_msgs::msg::CameraInfo caminfo_msg_; + void imageReceivedEvent(const unsigned char* data, + unsigned int dataSize, + unsigned int width, + unsigned int height, + unsigned int step); }; #endif /* CAMERAROS_H_ */ diff --git a/robotino_node/include/robotino_node/RTOCameraNode.h b/robotino_node/include/robotino_node/RTOCameraNode.h deleted file mode 100644 index f83f4a0..0000000 --- a/robotino_node/include/robotino_node/RTOCameraNode.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - * RTONode.h - * - * Created on: 09.12.2011 - * Author: indorewala@servicerobotics.eu - */ - -#ifndef RTOCameraNode_H -#define RTOCameraNode_H - -#include "ComROS.h" -#include "CameraROS.h" - -#include "rclcpp/rclcpp.hpp" - -class RTOCameraNode : public rclcpp::Node -{ -public: - RTOCameraNode(); - ~RTOCameraNode(); - - -private: - void spin(); - rclcpp::TimerBase::SharedPtr timer_; - std::string hostname_; - int cameraNumber_; - - ComROS com_; - CameraROS camera_; - - void initModules(); - -}; - -#endif /* RTOCameraNode_H */ diff --git a/robotino_node/include/robotino_node/RobotinoCameraNode.h b/robotino_node/include/robotino_node/RobotinoCameraNode.h new file mode 100644 index 0000000..f8fcee4 --- /dev/null +++ b/robotino_node/include/robotino_node/RobotinoCameraNode.h @@ -0,0 +1,30 @@ +/* + * RobotinoCameraNode.h + * + * Created on: 09.12.2011 + * Author: indorewala@servicerobotics.eu + * Edited on: 19.07.2024 + * Author: BrOleg5 + */ + +#ifndef ROBOTINOCAMERANODE_H +#define ROBOTINOCAMERANODE_H + +#include "rclcpp/rclcpp.hpp" + +#include "ComROS.h" +#include "CameraROS.h" + +class RobotinoCameraNode : public rclcpp::Node { + public: + RobotinoCameraNode(); + ~RobotinoCameraNode() {} + + private: + rclcpp::TimerBase::SharedPtr timer_; + + ComROS com_; + CameraROS camera_; +}; + +#endif /* ROBOTINOCAMERANODE_H */ diff --git a/robotino_node/src/CameraROS.cpp b/robotino_node/src/CameraROS.cpp index e95916f..9c9754f 100644 --- a/robotino_node/src/CameraROS.cpp +++ b/robotino_node/src/CameraROS.cpp @@ -3,64 +3,36 @@ * * Created on: 08.12.2011 * Author: indorewala@servicerobotics.eu + * Edited on: 19.07.2024 + * Author: BrOleg5 */ +#include #include "CameraROS.h" #include "sensor_msgs/fill_image.hpp" -namespace sensor_msgs -{ -extern bool fillImage( - msg::Image & image, - const std::string & encoding_arg, - uint32_t rows_arg, - uint32_t cols_arg, - uint32_t step_arg, - const void * data_arg); -}; +CameraROS::CameraROS(rclcpp::Node* parent_node_ptr) : img_msg_(), caminfo_msg_() { + streaming_pub_ = parent_node_ptr->create_publisher("image_raw", 10); + caminfor_pub_ = parent_node_ptr->create_publisher("camera_info", 10); + clock_ptr_ = parent_node_ptr->get_clock(); - -CameraROS::CameraROS() -{ - img_msg_ = sensor_msgs::msg::Image(); -} - -CameraROS::~CameraROS() -{ + img_msg_.header.frame_id = "camera_link"; + caminfo_msg_.header.frame_id = "camera_link"; } - -void CameraROS::setNumber( int number, rclcpp::Node * parent_node ) -{ - std::stringstream topic; - - if( number == 0) - topic << "image_raw"; - else - topic << "image_raw" << number; - - streaming_pub_ = parent_node->create_publisher(topic.str(),10); - - setCameraNumber( number ); -} - -void CameraROS::setTimeStamp(rclcpp::Time stamp) -{ - stamp_ = stamp; -} - -void CameraROS::imageReceivedEvent( - const unsigned char* data, - unsigned int dataSize, - unsigned int width, - unsigned int height, - unsigned int step ) -{ - // Build the Image msg - img_msg_.header.stamp = stamp_; - sensor_msgs::fillImage(img_msg_, "bgr8", height, width, step, data); - - // Publish the Image & CameraInfo msgs - streaming_pub_->publish(img_msg_); - +void CameraROS::imageReceivedEvent(const unsigned char* data, + unsigned int dataSize, + unsigned int width, + unsigned int height, + unsigned int step) { + // Build the Image msg + img_msg_.header.stamp = clock_ptr_->now(); + sensor_msgs::fillImage(img_msg_, sensor_msgs::image_encodings::RGB8, height, width, step, data); + + caminfo_msg_.height = height; + caminfo_msg_.width = width; + + // Publish the Image & CameraInfo msgs + streaming_pub_->publish(img_msg_); + caminfor_pub_->publish(caminfo_msg_); } diff --git a/robotino_node/src/RTOCameraNode.cpp b/robotino_node/src/RTOCameraNode.cpp deleted file mode 100644 index b57a1ff..0000000 --- a/robotino_node/src/RTOCameraNode.cpp +++ /dev/null @@ -1,53 +0,0 @@ -/* - * RTONode.cpp - * - * Created on: 09.12.2011 - * Author: indorewala@servicerobotics.eu - */ - -#include "RTOCameraNode.h" -#include - -using namespace std::chrono_literals; - -RTOCameraNode::RTOCameraNode(): - Node("rto_camera_node"), - com_(this) -{ - this->declare_parameter("hostname", "172.26.1.1"); - this->declare_parameter("cameraNumber", 0); - hostname_ = this->get_parameter("hostname").as_string(); - cameraNumber_ = this->get_parameter("cameraNumber").as_int(); - std::ostringstream os; - os << "Camera" << cameraNumber_; - com_.setName( os.str() ); - - initModules(); - timer_ = this->create_wall_timer(200ms, std::bind(&RTOCameraNode::spin, this)); -} - -RTOCameraNode::~RTOCameraNode() -{ -} - -void RTOCameraNode::initModules() -{ - com_.setAddress( hostname_.c_str() ); - - // Set the ComIds - camera_.setComId( com_.id() ); - - // Set the camera numbers - camera_.setNumber( cameraNumber_ , this); - - com_.connectToServer( false ); -} - -void RTOCameraNode::spin() -{ - rclcpp::Time curr_time = rclcpp::Clock().now(); - camera_.setTimeStamp(curr_time); - - com_.processEvents(); -} - diff --git a/robotino_node/src/RobotinoCameraNode.cpp b/robotino_node/src/RobotinoCameraNode.cpp new file mode 100644 index 0000000..5d5802d --- /dev/null +++ b/robotino_node/src/RobotinoCameraNode.cpp @@ -0,0 +1,27 @@ +/* + * RobotinoCameraNode.cpp + * + * Created on: 09.12.2011 + * Author: indorewala@servicerobotics.eu + * Edited on: 19.07.2024 + * Author: BrOleg5 + */ + +#include "RobotinoCameraNode.h" + +RobotinoCameraNode::RobotinoCameraNode() : Node("camera_node"), com_(this), camera_(this) { + this->declare_parameter("hostname", "192.168.0.1"); + this->declare_parameter("timer_period_ms", 50); + this->declare_parameter("camera_number", 0); + + std::string hostname = this->get_parameter("hostname").as_string(); + com_.setAddress(hostname.c_str()); + com_.connectToServer(true); + RCLCPP_INFO(this->get_logger(), "Connecting to Robotino with host IP %s\n", hostname.c_str()); + + int camera_number = this->get_parameter("camera_number").as_int(); + camera_.setCameraNumber(camera_number); + + auto timer_period = std::chrono::milliseconds(this->get_parameter("timer_period_ms").as_int()); + timer_ = this->create_wall_timer(timer_period, std::bind(&CameraROS::processEvents, &camera_)); +} diff --git a/robotino_node/src/robotino_camera_node.cpp b/robotino_node/src/robotino_camera_node.cpp new file mode 100644 index 0000000..437247b --- /dev/null +++ b/robotino_node/src/robotino_camera_node.cpp @@ -0,0 +1,18 @@ +/* + * main.cpp + * + * Created on: 09.12.2011 + * Author: indorewala@servicerobotics.eu + * Edited on: 19.07.2024 + * Author: BrOleg5 + */ +#include +#include "rclcpp/rclcpp.hpp" +#include "RobotinoCameraNode.h" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/robotino_node/src/rto_camera_node.cpp b/robotino_node/src/rto_camera_node.cpp deleted file mode 100644 index 0d9b83b..0000000 --- a/robotino_node/src/rto_camera_node.cpp +++ /dev/null @@ -1,17 +0,0 @@ -/* - * main.cpp - * - * Created on: 09.12.2011 - * Author: indorewala@servicerobotics.eu - */ - - -#include "RTOCameraNode.h" - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - - return 0; -}