Ros2 clean up

.gitignore

@@ -1,3 +1,4 @@
 .cproject
 .project
 .settings
+**__pycache__

CMakeLists.txt

@@ -38,6 +38,18 @@ target_link_libraries(${PROJECT_NAME}_node
   #avutil
 )
 
-install(PROGRAMS scripts/show_image.py DESTINATION lib/${PROJECT_NAME})
+install(
+  PROGRAMS scripts/show_image.py
+  DESTINATION lib/${PROJECT_NAME})
+
+install(
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY config
+  DESTINATION share/${PROJECT_NAME}
+)
 
 ament_auto_package()

config/params.yaml

@@ -1,10 +1,10 @@
-usb_cam:
-  ros__parameters:
-    video_device: "/dev/video0"
-    framerate: 15
-    io_method: "mmap"
-    frame_id: "camera"
-    pixel_format: "yuyv"
-    image_width: 640
-    image_height: 480
-    camera_name: "test"
+/**:
+    ros__parameters:
+      video_device: "/dev/video0"
+      framerate: 30.0
+      io_method: "mmap"
+      frame_id: "camera"
+      pixel_format: "yuyv"
+      image_width: 640
+      image_height: 480
+      camera_name: "test_cam"

include/usb_cam/usb_cam_node.hpp

@@ -0,0 +1,69 @@
+#include "usb_cam/usb_cam.h"
+
+#include <rclcpp/rclcpp.hpp>
+// #include <image_transport/image_transport.h>
+#include "camera_info_manager/camera_info_manager.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "std_srvs/srv/set_bool.hpp"
+
+std::ostream& operator<<(std::ostream& ostr, const rclcpp::Time& tm) {
+  ostr << tm.nanoseconds();
+  return ostr;
+}
+
+using namespace std::chrono_literals;
+
+
+namespace usb_cam {
+
+
+class UsbCamNode : public rclcpp::Node
+{
+public:
+  UsbCamNode();
+  virtual ~UsbCamNode() override;
+
+  void init();
+  void get_params();
+  void update();
+  bool take_and_send_image();
+
+  void service_capture(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<std_srvs::srv::SetBool::Request>  request,
+    std::shared_ptr<std_srvs::srv::SetBool::Response> response);
+
+  UsbCam cam_;
+
+  // shared image message
+  sensor_msgs::msg::Image::UniquePtr img_;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
+  // parameters
+  std::string video_device_name_;
+  std::string frame_id_;
+
+  std::string io_method_name_;
+  // these parameters all have to be a combination supported by the device
+  // Use
+  // v4l2-ctl --device=0 --list-formats-ext
+  // to discover them,
+  // or guvcview
+  std::string pixel_format_name_;
+  int image_width_;
+  int image_height_;
+  int framerate_;
+
+  // TODO(lucasw) use v4l2ucp for these?
+  // int exposure_, brightness_, contrast_, saturation_, sharpness_, focus_,
+  //    white_balance_, gain_;
+  // bool autofocus_, autoexposure_, auto_white_balance_;
+
+  std::string camera_name_;
+  std::string camera_info_url_;
+  std::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_;
+
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr service_capture_;
+};
+}  // namespace usb_cam

launch/demo_launch.py

@@ -1,8 +1,9 @@
 # Copyright 2018 Lucas Walter
 
 import argparse
-import launch
-import launch_ros.actions
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
 import os
 import sys
 import time
@@ -12,66 +13,75 @@
 
 
 def generate_launch_description():
+    ld = LaunchDescription()
+
     parser = argparse.ArgumentParser(description='usb_cam demo')
-    parser.add_argument('-d', '--device', dest='device', type=str,
-            help='video device', default='/dev/video0')
-    parser.add_argument('-wd', '--width', dest='width', type=int,
-            help='image width', default=640)
-    parser.add_argument('-ht', '--height', dest='height', type=int,
-            help='image height', default=480)
-    parser.add_argument('-f', '--fps', dest='frame_rate', type=float,
-            help='frame rate', default=5)
+    parser.add_argument('-n', '--node-name', dest='node_name', type=str,
+            help='name for device', default='usb_cam')
+    # parser.add_argument('-d', '--device', dest='device', type=str,
+            # help='video device', default='/dev/video0')
+    # parser.add_argument('-wd', '--width', dest='width', type=int,
+            # help='image width', default=640)
+    # parser.add_argument('-ht', '--height', dest='height', type=int,
+            # help='image height', default=480)
+    # parser.add_argument('-f', '--fps', dest='frame_rate', type=float,
+            # help='frame rate', default=5)
     args, unknown = parser.parse_known_args(sys.argv[4:])
 
     usb_cam_dir = get_package_share_directory('usb_cam')
-    print('usb_cam dir ' + usb_cam_dir)
+    # print('usb_cam dir ' + usb_cam_dir)
     launches = []
 
-    prefix = "/tmp/ros2/" + str(int(time.time())) + "/"
-    if not os.path.exists(prefix):
-        os.makedirs(prefix)
+    # get path to params file
+    params_path= os.path.join(
+        usb_cam_dir,
+        'config',
+        'params.yaml'
+    )
+
+    # if not os.path.exists(params_path):
+        # os.makedirs(params_path)
 
     # TODO(lucasw) get from commandline
     # TODO(lucasw) if these are an invalid combination usb_cam just dies-
     # need a more helpfull error message.
-    device = args.device
-    framerate = int(args.frame_rate)
-    width = args.width
-    height = args.height
+    # device = args.device
+    node_name = args.node_name
+    # frame_rate = int(args.frame_rate)
+    # width = args.width
+    # height = args.height
 
     # usb camera
-    params = prefix + "demo_usb_cam.yaml"
-    ns = 'demo'
-    node_name = 'usb_cam'
-    with open(params, 'w') as outfile:
-        print("opened " + params + " for yaml writing")
-        data = {}
-        data[ns] = {}
-        data[ns][node_name] = {}
-        data[ns][node_name]['ros__parameters'] = dict(
-                video_device = device,
-                framerate = framerate,
-                io_method = "mmap",
-                frame_id = "camera",
-                pixel_format = "yuyv",
-                image_width = width,
-                image_height = height,
-                camera_name = "camera",
-                )
-        yaml.dump(data, outfile, default_flow_style=False)
+    # path = prefix + "demo_usb_cam.yaml"
+    # ns = 'demo'
+    # params = {}
 
-    launches.append(launch_ros.actions.Node(
-            package='usb_cam', node_executable='usb_cam_node', output='screen',
-            node_name=node_name,
-            node_namespace=ns,
-            arguments=["__params:=" + params],
-            # arguments=["__params:=" + usb_cam_dir + "/config/params.yaml"]
+    # with open(path, 'w') as outfile:
+        # print("opened " + path + " for yaml writing")
+        # params[node_name] = {}
+        # params[node_name]['ros__parameters'] = dict(
+                # video_device = device,
+                # framerate = frame_rate,
+                # io_method = "mmap",
+                # frame_id = "camera",
+                # pixel_format = "yuyv",
+                # image_width = width,
+                # image_height = height,
+                # camera_name = name,
+                # )
+        # yaml.dump(params, outfile, default_flow_style=False)
+    print(params_path)
+    ld.add_action(Node(
+            package='usb_cam', executable='usb_cam_node', output='screen',
+            name=node_name,
+            # namespace=ns,
+            parameters=[params_path]
             ))
-    launches.append(launch_ros.actions.Node(
-            package='usb_cam', node_executable='show_image.py', output='screen',
-            node_namespace=ns,
+    ld.add_action(Node(
+            package='usb_cam', executable='show_image.py', output='screen',
+            # namespace=ns,
             # arguments=[image_manip_dir + "/data/mosaic.jpg"])
             # remappings=[('image_in', 'image_raw')]
             ))
 
-    return launch.LaunchDescription(launches)
+    return ld

package.xml

@@ -28,7 +28,7 @@
 
   <depend>ffmpeg</depend>
   <depend>libboost-dev</depend>
-  <depend>libboost-lexical-cast-dev</depend>
+  <!-- <depend>libboost-lexical-cast-dev</depend> -->
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

scripts/show_image.py

@@ -17,8 +17,8 @@ def __init__(self):
         self.sub = self.create_subscription(
             Image,
             'image_raw',
-            self.image_callback)
-        # print("subscribed to " + self.sub.getTopic())
+            self.image_callback,
+            100)
 
     def image_callback(self, msg):
         sz = (msg.height, msg.width)
@@ -53,12 +53,11 @@ def main(args=None):
     rclpy.init(args=args)
 
     examine_image = ExamineImage()
-    # TODO(lucasw) this is taking 100% cpu
-    # rclpy.spin(examine_image)
-    # this is also taking 100% cpu
-    while rclpy.ok():
-        rclpy.spin_once(examine_image)
-        time.sleep(0.1)
+
+    try:
+      rclpy.spin(examine_image)
+    except KeyboardInterrupt:
+        pass
 
     examine_image.destroy_node()
     rclpy.shutdown()