Update CameraROS and RTOCameraNode classes.

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
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}>
+  $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
+)
+
+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()

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<sensor_msgs::msg::Image>::SharedPtr streaming_pub_;
+    rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr caminfor_pub_;
 
-private:
-	rclcpp::Publisher<sensor_msgs::msg::Image>::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_ */

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 */

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 <string>
 
 #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<sensor_msgs::msg::Image>("image_raw", 10);
+    caminfor_pub_ = parent_node_ptr->create_publisher<sensor_msgs::msg::CameraInfo>("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<sensor_msgs::msg::Image>(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_);
 }

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_));
+}

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 <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "RobotinoCameraNode.h"
+
+int main(int argc, char** argv) {
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<RobotinoCameraNode>());
+    rclcpp::shutdown();
+    return 0;
+}