RGBカメラによるライントレースの実装

CMakeLists.txt

@@ -51,6 +51,23 @@ ament_target_dependencies(object_tracking_component
   cv_bridge)
 rclcpp_components_register_nodes(object_tracking_component "object_tracking::Tracker")
 
+add_library(camera_line_follower_component SHARED
+  src/camera_line_follower_component.cpp)
+target_compile_definitions(camera_line_follower_component
+  PRIVATE "RASPIMOUSE_ROS2_EXAMPLES_BUILDING_DLL")
+ament_target_dependencies(camera_line_follower_component
+  rclcpp
+  rclcpp_components
+  rclcpp_lifecycle
+  std_msgs
+  std_srvs
+  sensor_msgs
+  geometry_msgs
+  OpenCV
+  cv_bridge
+  raspimouse_msgs)
+rclcpp_components_register_nodes(camera_line_follower_component "camera_line_follower::Camera_Follower")
+
 add_library(line_follower_component SHARED
   src/line_follower_component.cpp)
 target_compile_definitions(line_follower_component
@@ -112,6 +129,7 @@ ament_export_dependencies(OpenCV)
 ament_export_include_directories(include)
 ament_export_libraries(
   object_tracking_component
+  camera_line_follower_component
   line_follower_component
   direction_controller_component)
 
@@ -122,6 +140,7 @@ install(
 
 install(TARGETS
   object_tracking_component
+  camera_line_follower_component
   line_follower_component
   direction_controller_component
   EXPORT export_${PROJECT_NAME}

include/raspimouse_ros2_examples/camera_line_follower_component.hpp

@@ -0,0 +1,82 @@
+// Copyright 2020 RT Corporation
+//
+// 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.
+
+#ifndef RASPIMOUSE_ROS2_EXAMPLES__CAMERA_LINE_FOLLOWER_COMPONENT_HPP_
+#define RASPIMOUSE_ROS2_EXAMPLES__CAMERA_LINE_FOLLOWER_COMPONENT_HPP_
+
+#include <memory>
+#include <string>
+
+#include "raspimouse_msgs/msg/switches.hpp"
+#include "raspimouse_ros2_examples/visibility_control.h"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "std_srvs/srv/set_bool.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "opencv2/highgui/highgui.hpp"
+
+namespace camera_line_follower
+{
+
+class Camera_Follower : public rclcpp_lifecycle::LifecycleNode
+{
+public:
+  RASPIMOUSE_ROS2_EXAMPLES_PUBLIC
+  explicit Camera_Follower(const rclcpp::NodeOptions & options);
+
+protected:
+  void image_callback(const sensor_msgs::msg::Image::SharedPtr msg_image);
+  void callback_switches(const raspimouse_msgs::msg::Switches::SharedPtr msg);
+  void on_cmd_vel_timer();
+
+private:
+  cv::VideoCapture cap_;
+  bool object_is_detected_;
+  bool can_publish_cmdvel_;
+  cv::Point2d object_normalized_point_;
+  double object_normalized_area_;
+  geometry_msgs::msg::Twist cmd_vel_;
+  raspimouse_msgs::msg::Switches switches_;
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::Image>> result_image_pub_;
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>> cmd_vel_pub_;
+  std::shared_ptr<rclcpp::Client<std_srvs::srv::SetBool>> motor_power_client_;
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
+  rclcpp::Subscription<raspimouse_msgs::msg::Switches>::SharedPtr switches_sub_;
+  rclcpp::TimerBase::SharedPtr cmd_vel_timer_;
+
+  std::string mat_type2encoding(int mat_type);
+  void convert_frame_to_message(
+    const cv::Mat & frame,
+    sensor_msgs::msg::Image & msg);
+
+  void following(const cv::Mat & input_frame, cv::Mat & result_frame);
+
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+  on_configure(const rclcpp_lifecycle::State &);
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+  on_activate(const rclcpp_lifecycle::State &);
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+  on_deactivate(const rclcpp_lifecycle::State &);
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+  on_cleanup(const rclcpp_lifecycle::State &);
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+  on_shutdown(const rclcpp_lifecycle::State &);
+};
+
+}  // namespace camera_line_follower
+
+#endif  // RASPIMOUSE_ROS2_EXAMPLES__CAMERA_LINE_FOLLOWER_COMPONENT_HPP_

launch/camera_line_follower.launch.py

@@ -0,0 +1,90 @@
+# Copyright 2020 RT Corporation
+#
+# 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.
+
+import launch
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.actions import Node
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+    declare_mouse = DeclareLaunchArgument(
+        'mouse',
+        default_value="true",
+        description='Launch raspimouse node'
+    )
+    declare_use_camera_node = DeclareLaunchArgument(
+        'use_camera_node',
+        default_value='true',
+        description='Use camera node.'
+    )
+    declare_video_device = DeclareLaunchArgument(
+        'video_device',
+        default_value='/dev/video0',
+        description='Set video device.'
+    )
+
+    """Generate launch description with multiple components."""
+    container = ComposableNodeContainer(
+            name='camera_line_follower_container',
+            namespace='',
+            package='rclcpp_components',
+            executable='component_container_mt',
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='raspimouse_ros2_examples',
+                    plugin='camera_line_follower::Camera_Follower',
+                    name='camera_follower',
+                    extra_arguments=[{'use_intra_process_comms': True}]),
+                ComposableNode(
+                    package='raspimouse',
+                    plugin='raspimouse::Raspimouse',
+                    name='raspimouse',
+                    parameters=[{'use_light_sensors': False}],
+                    extra_arguments=[{'use_intra_process_comms': True}],
+                    condition=IfCondition(LaunchConfiguration('mouse'))),
+                ComposableNode(
+                    package='usb_cam',
+                    plugin='usb_cam::UsbCamNode',
+                    name='usb_cam',
+                    remappings=[('image_raw', 'camera/color/image_raw')],
+                    parameters=[
+                        {'video_device': LaunchConfiguration('video_device')},
+                        {'frame_id': 'camera_color_optical_frame'},
+                        {'pixel_format': 'yuyv2rgb'}
+                    ],
+                    extra_arguments=[{'use_intra_process_comms': True}],
+                    condition=IfCondition(LaunchConfiguration('use_camera_node'))),
+            ],
+            output='screen',
+    )
+
+    manager = Node(
+        name='manager',
+        package='raspimouse_ros2_examples',
+        executable='lifecycle_node_manager',
+        output='screen',
+        parameters=[{'components': ['raspimouse', 'camera_follower']}]
+    )
+
+    return launch.LaunchDescription([
+        declare_mouse,
+        declare_use_camera_node,
+        declare_video_device,
+        container,
+        manager
+    ])