Merge pull request #284 from KKalem/alars_cams

Alars robot description

Author: KKalem

messages/dji_msgs/msg/Links.msg

@@ -13,19 +13,26 @@ string MAP = 'map'
 string ODOM = 'odom'
 string HOME_POINT = 'home_point'
 
-# The drone's body
+# The drone's body, defined as the point between the thick parts
+# of the legs.
 string BASE_LINK = 'base_link'
 # Same BASE_LINK, but only has the yaw component of the orientation.
 string BASE_FLAT = 'base_flat_link'
 
 
-# parts of the drone
-string GIMBAL_CAMERA_LINK = 'gimbal_camera_link'
-string WINCH_LINK = 'winch_link' # should be right-leg T junction
+# parts of the alars module that are relevant. in-between parts
+# like the roll/pitch/yaw arms of the gimbal are not included here
+# even though they are published by the z1_pro_driver node
+string ROPE_BASE_LINK = 'rope_base_link'
+
+string GIMBAL_CAMERA_LINK = 'z1_camera_link'
+string FISHEYE_CAMERA_LINK = 'fisheye_camera_link'
+
+# optical frames: z "depth", x "right", y "down" with top-left origin
+string GIMBAL_OPTICAL_FRAME = 'z1_optical_frame'
+string FISHEYE_OPTICAL_FRAME = 'fisheye_optical_frame'
 
 
-# Used by hook_estimator
-string HOOK_LINK = 'hook_link'
 
 # Used by projection/filter
 string ESTIMATED_AUV = 'estimated_auv'

messages/dji_msgs/msg/Topics.msg

@@ -5,14 +5,18 @@
 string MOVE_TO_SETPOINT_TOPIC = 'move_to_setpoint' # PoseStamped
 string LABELED_UTM_TOPIC = 'labeled_utm_topic' # String, formatted: "utm_{zone}_{band}"
 
-# user feedback
-string CONTROLLER_INPUT_TOPIC = 'joy' # Joy
-string CONTROLLER_VIBRATION_TOPIC = 'joy/set_feedback' # JoyFeedback
-string TTS_TOPIC = 'speak' # String
 
 # cameras
 string GIMBAL_CAMERA_RAW_TOPIC = 'gimbal_camera/camera/image_raw' # Image
+string GIMBAL_CAMERA_INFO_TOPIC = 'gimbal_camera/camera/camera_info' #CameraInfo
 string FISHEYE_CAMERA_RAW_TOPIC = 'fisheye_camera/camera/image_raw' # Image
+string FISHEYE_CAMERA_INFO_TOPIC = 'fisheye_camera/camera/camera_info' #CameraInfo
+
+
+# sensors that are not coming from PSDK
+string LOAD_CELL_RAW_TOPIC = 'sensor/load_cell_raw' # Int32
+string LOAD_CELL_WEIGHT_TOPIC = 'sensor/load_cell_weight' # Float32
+
 
 # object detectors in normalized image coordinates [-1,1]
 # (0,0) is the center of the image, +x is right, +y is up!
@@ -44,6 +48,10 @@ string DISABLE_ALARS_DETECTOR_SERVICE_TOPIC = 'disable_alars_detector' # Trigger
 string ENABLE_HOOK_ESTIMATOR_SERVICE_TOPIC = 'enable_hook_estimator' # Trigger
 string DISABLE_HOOK_ESTIMATOR_SERVICE_TOPIC = 'disable_hook_estimator' # Trigger
 
-# sensors that are not coming from PSDK
-string LOAD_CELL_RAW_TOPIC = 'sensor/load_cell_raw' # Int32
-string LOAD_CELL_WEIGHT_TOPIC = 'sensor/load_cell_weight' # Float32
+
+
+
+# user feedback
+string CONTROLLER_INPUT_TOPIC = 'joy' # Joy
+string CONTROLLER_VIBRATION_TOPIC = 'joy/set_feedback' # JoyFeedback
+string TTS_TOPIC = 'speak' # String

perception/alars/auv_state_estimation/auv_state_estimation/projection.py

@@ -1,4 +1,5 @@
 import rclpy
+from rclpy.time import Time, Duration
 import numpy as np
 from rclpy.node import Node
 from sensor_msgs.msg import CameraInfo
@@ -8,7 +9,7 @@
 from scipy.spatial.transform import Rotation as R
 from geometry_msgs.msg import PolygonStamped, Point32
 
-from dji_msgs.msg import Links
+from dji_msgs.msg import Links, Topics
 
 
 class ProjectionNode(Node):
@@ -19,12 +20,12 @@ def __init__(self, name = 'projection_node'):
             namespace="",
             parameters=[
                 ("z_water", 0.0),
-                ("topics.camera_info", "gimbal_camera/camera/cam_info"),
-                ("topics.input_polygon", "alars_detection/auv_obb"),
-                ("topics.output_projected_polygon", "projection/auv_obb"),
+                ("topics.camera_info", Topics.GIMBAL_CAMERA_INFO_TOPIC),
+                ("topics.input_polygon", Topics.ESTIMATED_AUV_OBB_TOPIC),
+                ("topics.output_projected_polygon", Topics.PROJECTED_AUV_OBB_TOPIC),
                 ("topics.rviz.camera_rays", "rviz/projection_rays"),
                 ("frames.map", Links.MAP),                      
-                ("frames.camera", Links.GIMBAL_CAMERA_LINK), 
+                ("frames.camera", Links.GIMBAL_OPTICAL_FRAME), 
                 ])
 
         self.marker_ns = self.get_name() # ns fro rviz ray markers
@@ -50,7 +51,6 @@ def __init__(self, name = 'projection_node'):
         self.K_inv = None
         self.width = None
         self.height = None
-        self.R_im_cam = np.array([[ 0, -1,  0], [-1,  0,  0], [ 0,  0, -1]])  # Optical to cam link frame
 
         # tf
         self.tf_buffer = tf2_ros.Buffer()
@@ -83,11 +83,11 @@ def project(self, msg: PolygonStamped):
             self.get_logger().warning("No CameraInfo received yet")
             return
 
-        stamp = msg.header.stamp
-        points_im = np.array([(p.x, p.y) for p in msg.polygon.points])
-
+        # use zero time to get the lastest available instead of the exact stamp of the image
+        # image might be coming faster than TF updates for reasons, and using stamp of image
+        # to get TF causes "lookup in the future" errors.
         try:
-            transform = self.tf_buffer.lookup_transform(self.map_frame, self.cam_frame, stamp, timeout=rclpy.duration.Duration(seconds=0.5))
+            transform = self.tf_buffer.lookup_transform(self.map_frame, self.cam_frame, Time(seconds=0), timeout=Duration(seconds=1))
         except Exception as e:
             self.get_logger().info(f"TF lookup failed ({self.cam_frame} -> {self.map_frame}): {e}")
             return
@@ -99,30 +99,31 @@ def project(self, msg: PolygonStamped):
         rot = R.from_quat([q.x, q.y, q.z, q.w]).as_matrix()
 
         # pixelcoords from normalized image coords
+        points_im = np.array([(p.x, p.y) for p in msg.polygon.points])
         u = (points_im[:, 0] + 1.0) * 0.5 * self.width
-        v = (1.0 - (points_im[:, 1] + 1.0) * 0.5) * self.height # y up -> v down (optical frame)
+        v = (points_im[:, 1] + 1.0) * 0.5 * self.height
 
         ray_im = self.K_inv @ np.vstack((u, v, np.ones_like(u))) # projection from pixel to 3D ray in image frame (pc = K_inv @ xs)
-        ray_cam = self.R_im_cam @ ray_im # Convert from image frame to cam link frame
-
-        ray_map = rot @ ray_cam # rotate ray to map frame
+        ray_map = rot @ ray_im # rotate ray to map frame
+        
         dz = ray_map[2, :]
         if np.any(dz > -1e-2):  # if the z component of the norm ray is positive or close to zero, it means the ray is parallel to or pointing away from the water plane
             self.get_logger().info(f"Projection doesn't intersect with water surface")
             return
 
         t_intersect = (self.z_water - cam_pos_map[2]) / dz # translation along ray to intersect with water
         intersection_points_map = cam_pos_map[:, None] + ray_map * t_intersect  # intersection points in map frame
-        intersection_points_cam = ray_cam * t_intersect  # intersection points in cam frame
+        intersection_points_cam = ray_im * t_intersect  # intersection points in cam frame
 
-        self.publish_poly(stamp, intersection_points_map.T)
-        self.publish_ray_marker(stamp, np.mean(intersection_points_cam, axis=1))
+        now: Time = self.get_clock().now()
+        self.publish_poly(now, intersection_points_map.T)
+        self.publish_ray_marker(now, np.mean(intersection_points_cam, axis=1))
 
-    def publish_ray_marker(self, stamp, intersection_point):
+    def publish_ray_marker(self, stamp:Time, intersection_point):
         # publish projected line (ray) for rviz
         marker = Marker()
         marker.header.frame_id = self.cam_frame
-        marker.header.stamp = stamp
+        marker.header.stamp = stamp.to_msg()
         marker.ns = self.marker_ns
         marker.id = 0
         marker.type = Marker.LINE_STRIP
@@ -136,11 +137,11 @@ def publish_ray_marker(self, stamp, intersection_point):
         marker.points.append(p1)
         self.ray_pub.publish(marker)
 
-    def publish_poly(self, stamp, points): 
+    def publish_poly(self, stamp:Time, points): 
         # Publish head pose and obb corners (may be used for filter)
         obb_marker = PolygonStamped()
         obb_marker.header.frame_id = self.map_frame
-        obb_marker.header.stamp = stamp
+        obb_marker.header.stamp = stamp.to_msg()
         for px, py, pz in points:
             point32 = Point32()
             point32.x = px

perception/alars/auv_state_estimation/config/params.yaml

@@ -5,7 +5,7 @@
     z_water: 0.0
 
     topics:
-      camera_info: "gimbal_camera/camera/cam_info"
+      camera_info: "gimbal_camera/camera/camera_info"
       input_polygon: "alars_detection/auv_obb"          
       output_projected_polygon: "alars_projection/auv_obb"   
       rviz:

perception/alars/auv_yolo_detector/auv_yolo_detector/auv_yolo_detector.py

@@ -323,7 +323,7 @@ def filter_detections(self, obb: OBB) -> Tuple[Results, torch.Tensor, Union[list
     def published_normalized_position(self, p: tuple, wh: tuple, label: str) -> tuple:
         """
         Normalize pixels coordinates so the origin is the center of the image, the x axis is horizontal
-        from left to right and y axis is vertical from bottom to top
+        from left to right and y axis is vertical from top to bottom
         Publishes normalized pixel position in corresponding topics, depending on label
         Args:
             p: (horizontal position, vertical position)  (both in pixels)
@@ -335,7 +335,7 @@ def published_normalized_position(self, p: tuple, wh: tuple, label: str) -> tupl
         point.header.stamp = self.get_clock().now().to_msg()
         point.header.frame_id = self.model_params["frames.camera"]
         point.point.x = float((p[0]-wh[0]/2)/(wh[0]/2))
-        point.point.y = float(-(p[1]-wh[1]/2)/(wh[1]/2))
+        point.point.y = float((p[1]-wh[1]/2)/(wh[1]/2))
         if label == 'sam': self.sam_position_pub.publish(point)
         elif label == 'buoy': self.buoy_position_pub.publish(point)
         else: self.get_logger().error('Position not published, label argument should be "sam" or "buoy"')
@@ -351,7 +351,7 @@ def publish_normalized_points(self, points: np.ndarray, wh: tuple, label=None):
         w, h = wh
         for px, py in points:
             xn = float((px - w/2) / (w/2))
-            yn = float(-(py - h/2) / (h/2))
+            yn = float((py - h/2) / (h/2))
             p = Point32()
             p.x = xn
             p.y = yn
@@ -425,7 +425,7 @@ def get_params(self):
 
             "frames.map": namespace.removeprefix("/") + "/" + Links.MAP,
             "frames.quadrotor_odom": namespace.removeprefix("/") + "/" + Links.ODOM,
-            "frames.camera": namespace.removeprefix("/") + "/" + Links.GIMBAL_CAMERA_LINK,
+            "frames.camera": namespace.removeprefix("/") + "/" + Links.GIMBAL_OPTICAL_FRAME,
 
         }
         self.model_params = {

scripts/smarc_bringups/scripts/dji_bringup.sh

@@ -87,7 +87,8 @@ if [[ $USE_SIM_TIME = "False" ]]; then
     tmux send-keys "ros2 launch psdk_wrapper wrapper.launch.py namespace:=/$ROBOT_NAME/wrapper" C-m
 
     tmux select-pane -t $SESSION:0.1
-    tmux send-keys "ros2 run dji_captain dji_captain --ros-args -p use_sim_time:=$USE_SIM_TIME  -p home_altitude_above_water:=$HOME_ABOVE_WATER -r __ns:=/$ROBOT_NAME " C-m
+    # tmux send-keys "ros2 run dji_captain dji_captain --ros-args -p use_sim_time:=$USE_SIM_TIME  -p home_altitude_above_water:=$HOME_ABOVE_WATER -r __ns:=/$ROBOT_NAME " C-m
+    tmux send-keys "ros2 launch dji_captain alars_captain.launch robot_name:=$ROBOT_NAME use_sim_time:=$USE_SIM_TIME home_altitude_above_water:=$HOME_ABOVE_WATER" C-m
 
     tmux select-pane -t $SESSION:0.2
     tmux send-keys "fast-discovery-server -i 0" C-m
@@ -99,7 +100,7 @@ else
     tmux select-window -t $SESSION:0
     tmux split-window -h -t $SESSION:0.0
     tmux select-pane -t $SESSION:0.0
-    tmux send-keys "ros2 run dji_captain dji_captain --ros-args -p use_sim_time:=$USE_SIM_TIME  -p home_altitude_above_water:=$HOME_ABOVE_WATER -r __ns:=/$ROBOT_NAME " C-m
+    tmux send-keys "ros2 launch dji_captain alars_captain.launch robot_name:=$ROBOT_NAME use_sim_time:=$USE_SIM_TIME home_altitude_above_water:=$HOME_ABOVE_WATER" C-m
 
     tmux select-pane -t $SESSION:0.1
     tmux send-keys "ros2 topic echo /$ROBOT_NAME/captain_status std_msgs/msg/String --field data" C-m
@@ -188,13 +189,15 @@ tmux send-keys "ros2 topic echo ${ROBOT_NAME}/alars_bt/status std_msgs/msg/Strin
 # 4 Camera and detection
 ############
 # camera and detection node
-tmux new-window -t $SESSION:4 -n 'Camera'
-tmux rename-window "Cam"
+tmux new-window -t $SESSION:4 -n 'CamProc'
+tmux rename-window "CamProc"
 tmux select-window -t $SESSION:4
+
 tmux split-window -h -t $SESSION:4.0
+tmux select-layout -t $SESSION:4 even-vertical
 
-tmux select-pane -t $SESSION:4.0
 # Fransisco's YOLO detector
+tmux select-pane -t $SESSION:4.0
 YOLO_DEVICE=0
 if [[ $USE_SIM_TIME = "True" ]]; then
     YOLO_DEVICE=cpu
@@ -204,33 +207,11 @@ namespace:=$ROBOT_NAME \
 device:=$YOLO_DEVICE \
 use_sim_time:=$USE_SIM_TIME" C-m
 
-# the cam driver is needed just for the real thing
-# for basic usb webcam
-#tmux send-keys "ros2 run usb_cam usb_cam_node_exe --ros-args -r __ns:=/$ROBOT_NAME/gimbal_camera" C-m
-if [[ $USE_SIM_TIME = "False" ]]; then
-    tmux split-window -v -t $SESSION:4.1
-    tmux select-pane -t $SESSION:4.1
-    # for the dji gimbal cam
-    # requires ros-humble-gscam gstreamer1.0-tools gstreamer1.0-plugins-good
-    GSCAM_CONFIG_DJI="v4l2src device=/dev/djipocket3 ! image/jpeg,width=1920,height=1080,framerate=30/1 ! jpegdec ! videoconvert ! video/x-raw,format=BGR"
-    tmux send-keys "ros2 run gscam gscam_node --ros-args \
-    -p gscam_config:=\"$GSCAM_CONFIG_DJI\" \
-    -p frame_id:=osmo3_optical_frame \
-    -p image_encoding:=rgb8 \
-    -p sync_sink:=false \
-    -r __ns:=/$ROBOT_NAME/gimbal_camera" C-m
+# Sebastian's projection node
+tmux select-pane -t $SESSION:4.1
+tmux send-keys "ros2 launch auv_state_estimation projection_launch.py namespace:=$ROBOT_NAME use_sim_time:=$USE_SIM_TIME" C-m
+
 
-    # for the 360 cam
-    tmux select-pane -t $SESSION:4.2
-    # for the dji gimbal cam
-    GSCAM_CONFIG_FISH="v4l2src device=/dev/insta360x4 ! image/jpeg,width=1920,height=1080,framerate=30/1 ! jpegdec ! videoconvert ! video/x-raw,format=BGR"
-    tmux send-keys "ros2 run gscam gscam_node --ros-args \
-    -p gscam_config:=\"$GSCAM_CONFIG_FISH\" \
-    -p frame_id:=fisheye_optical_frame \
-    -p image_encoding:=rgb8 \
-    -p sync_sink:=false \
-    -r __ns:=/$ROBOT_NAME/fisheye_camera" C-m
-fi
 
 ############
 # 5 AUX Nodes like geofence etc.
@@ -246,10 +227,41 @@ tmux send-keys "ros2 run actionable_geofence geofence_node --ros-args -r __ns:=/
 ############
 if [[ $USE_SIM_TIME = "False" ]]; then
     # new window for load_cell_driver
-    tmux new-window -t $SESSION:7 -n 'LoadCell'
-    tmux rename-window "LoadCell"
+    tmux new-window -t $SESSION:7 -n 'Drivers'
+    tmux rename-window "Drivers"
     tmux select-window -t $SESSION:7
+
+    tmux split-window -v -t $SESSION:7.0
+    tmux split-window -v -t $SESSION:7.1
+    tmux select-layout -t $SESSION:7 even-vertical
+
+    tmux select-pane -t $SESSION:7.0
     tmux send-keys "ros2 run nau7802_ros2_driver nau7802_ros2_driver --ros-args -r __ns:=/$ROBOT_NAME" C-m
+
+    tmux select-pane -t $SESSION:7.1
+    # for basic usb webcam
+    #tmux send-keys "ros2 run usb_cam usb_cam_node_exe --ros-args -r __ns:=/$ROBOT_NAME/gimbal_camera" C-m
+    # TODO: replace with the z1 pro driver
+    # for the dji gimbal cam
+    # requires ros-humble-gscam gstreamer1.0-tools gstreamer1.0-plugins-good
+    # GSCAM_CONFIG_DJI="v4l2src device=/dev/djipocket3 ! image/jpeg,width=1920,height=1080,framerate=30/1 ! jpegdec ! videoconvert ! video/x-raw,format=BGR"
+    # tmux send-keys "ros2 run gscam gscam_node --ros-args \
+    # -p gscam_config:=\"$GSCAM_CONFIG_DJI\" \
+    # -p frame_id:=osmo3_optical_frame \
+    # -p image_encoding:=rgb8 \
+    # -p sync_sink:=false \
+    # -r __ns:=/$ROBOT_NAME/gimbal_camera" C-m
+
+    # for the 360 cam
+    tmux select-pane -t $SESSION:7.2
+    # for the dji gimbal cam
+    GSCAM_CONFIG_FISH="v4l2src device=/dev/insta360x4 ! image/jpeg,width=1920,height=1080,framerate=30/1 ! jpegdec ! videoconvert ! video/x-raw,format=BGR"
+    tmux send-keys "ros2 run gscam gscam_node --ros-args \
+    -p gscam_config:=\"$GSCAM_CONFIG_FISH\" \
+    -p frame_id:=fisheye_optical_frame \
+    -p image_encoding:=rgb8 \
+    -p sync_sink:=false \
+    -r __ns:=/$ROBOT_NAME/fisheye_camera" C-m
 fi
 
 

vehicles/descriptions/alars_description/CMakeLists.txt

@@ -0,0 +1,52 @@
+cmake_minimum_required(VERSION 3.8)
+project(alars_description)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+install(
+  DIRECTORY launch/
+  DESTINATION share/${PROJECT_NAME}/launch/
+)
+
+install(
+  DIRECTORY mesh/
+  DESTINATION share/${PROJECT_NAME}/mesh/
+)
+
+#install(
+#  DIRECTORY robots/
+#  DESTINATION share/${PROJECT_NAME}/robots/
+#)
+
+install(
+  DIRECTORY urdf/
+  DESTINATION share/${PROJECT_NAME}/urdf/
+)
+
+install(
+  PROGRAMS
+    # scripts/joint_state_converter.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()