diff --git a/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py b/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py index 38564f9..7ad71eb 100644 --- a/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py +++ b/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py @@ -89,6 +89,8 @@ def __init__(self, shared_objects): rospy.Publisher("/cameras/undercarriage/camera_control", CameraControlMessage, queue_size=1).publish(reset_camera_message) rospy.Publisher("/cameras/main_navigation/camera_control", CameraControlMessage, queue_size=1).publish(reset_camera_message) rospy.Publisher("/cameras/end_effector/camera_control", CameraControlMessage, queue_size=1).publish(reset_camera_message) + rospy.Publisher("/cameras/zed_left/camera_control", CameraControlMessage, queue_size=1).publish(reset_camera_message) + rospy.Publisher("/cameras/point_cloud/camera_control", CameraControlMessage, queue_size=1).publish(reset_camera_message) self.msleep(3000) @@ -213,6 +215,7 @@ def __get_cameras(self): names = set(names) + #Remove zed camera from EXCLUDED_CAMERAS above once in use for camera in EXCLUDED_CAMERAS: if camera in names: names.remove(camera) @@ -235,6 +238,16 @@ def __get_cameras(self): if "end_effector" in names: self.valid_cameras.append("end_effector") + #Place two if statements below inside of larger scope + #if statement that checks if zed camera device is + #connected to rover before checking to append + if "zed_left" in names: + self.valid_cameras.append("zed_left") + + if "point_cloud" in names: + self.valid_cameras.append("point_cloud") + + def __setup_video_threads(self): for camera in self.valid_cameras: self.camera_threads[camera] = RoverVideoReceiver.RoverVideoReceiver(camera) diff --git a/software/ros_packages/nimbro_topic_transport/launch/camera_topics4.yaml b/software/ros_packages/nimbro_topic_transport/launch/camera_topics4.yaml new file mode 100644 index 0000000..bf20d67 --- /dev/null +++ b/software/ros_packages/nimbro_topic_transport/launch/camera_topics4.yaml @@ -0,0 +1,4 @@ +topics: + - name: "/cameras/zed_left/image_1280x720/compressed" + compress: false # enable bz2 compression + rate: 30.0 #rate limit at 1Hz diff --git a/software/ros_packages/nimbro_topic_transport/launch/camera_topics5.yaml b/software/ros_packages/nimbro_topic_transport/launch/camera_topics5.yaml new file mode 100644 index 0000000..ca18053 --- /dev/null +++ b/software/ros_packages/nimbro_topic_transport/launch/camera_topics5.yaml @@ -0,0 +1,4 @@ +topics: + - name: "cameras/point_cloud/image_1280x720/compressed" + compress: false # enable bz2 compression + rate: 30.0 # rate limit at 1Hz diff --git a/software/ros_packages/nimbro_topic_transport/launch/udp_camera_receiver.launch b/software/ros_packages/nimbro_topic_transport/launch/udp_camera_receiver.launch index aa952cd..d3297eb 100644 --- a/software/ros_packages/nimbro_topic_transport/launch/udp_camera_receiver.launch +++ b/software/ros_packages/nimbro_topic_transport/launch/udp_camera_receiver.launch @@ -34,4 +34,12 @@ + + + + + + + + diff --git a/software/ros_packages/rover_camera/launch/example.launch b/software/ros_packages/rover_camera/launch/example.launch index d657d4d..8c0d37b 100644 --- a/software/ros_packages/rover_camera/launch/example.launch +++ b/software/ros_packages/rover_camera/launch/example.launch @@ -13,6 +13,12 @@ + + + + + + diff --git a/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch b/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch index 0edc76d..ef77166 100644 --- a/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch +++ b/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch @@ -47,6 +47,14 @@ + + + + + + + + diff --git a/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch b/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch index ab2f6a9..2d8b9c8 100644 --- a/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch +++ b/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch @@ -1,32 +1,176 @@ - + - + - + - [{name: "/rover_control/command_control/ground_station_drive", compress: true, rate: 15.0}, - {name: "/rover_control/tower/pan_tilt/control", compress: true, rate: 30.0}, - {name: "/rover_control/chassis/pan_tilt/control", compress: true, rate: 30.0}, - {name: "/rover_control/mining/control", compress: true, rate: 30.0}, - {name: "/rover_control/mining/drill/control", compress: true, rate: 30.0}, - {name: "/rover_arm/control/relative", compress: true, rate: 30.0}, - {name: "/rover_control/gripper/control", compress: true, rate: 30.0}] + [{name: "/cameras/chassis/image_1280x720/compressed", compress: false, rate: 0.0}] - + - + - [{name: "/cameras/chassis/camera_control", compress: false, rate: 5.0}, - {name: "/cameras/undercarriage/camera_control", compress: false, rate: 5.0}, - {name: "/cameras/main_navigation/camera_control", compress: false, rate: 5.0}, - {name: "/cameras/end_effector/camera_control", compress: false, rate: 5.0}, - {name: "/rover_status/update_requested", compress: false, rate: 5.0}, - {name: "/rover_arm/control/absolute", compress: true, rate: 5.0}, - {name: "/rover_control/camera/control", compress: true, rate: 5.0}] + [{name: "/cameras/undercarriage/image_1280x720/compressed", compress: false, rate: 0.0}] + + + + + + + + [{name: "/cameras/main_navigation/image_1280x720/compressed", compress: false, rate: 0.0}] + + + + + + + + [{name: "/cameras/end_effector/image_1280x720/compressed", compress: false, rate: 0.0}] + + + + + + + + [{name: "/cameras/chassis/image_640x360/compressed", compress: false, rate: 0.0}] + + + + + + + + [{name: "/cameras/undercarriage/image_640x360/compressed", compress: false, rate: 0.0}] + + + + + + + + [{name: "/cameras/main_navigation/image_640x360/compressed", compress: false, rate: 0.0}] + + + + + + + + [{name: "/cameras/end_effector/image_640x360/compressed", compress: false, rate: 0.0}] + + + + + + + + [{name: "/cameras/chassis/image_256x144/compressed", compress: false, rate: 0.0}] + + + + + + + + [{name: "/cameras/undercarriage/image_256x144/compressed", compress: false, rate: 0.0}] + + + + + + + + [{name: "/cameras/main_navigation/image_256x144/compressed", compress: false, rate: 0.0}] + + + + + + + + [{name: "/cameras/end_effector/image_256x144/compressed", compress: false, rate: 0.0}] + + + + + + + + [{name: "/rover_status/wheel_status", compress: false, rate: 1.0}] + + + + + + + + [{name: "/rover_status/camera_status", compress: false, rate: 1.0}] + + + + + + + + [{name: "/rover_status/frsky_status", compress: false, rate: 1.0}] + + + + + + + + [{name: "/rover_status/gps_status", compress: false, rate: 1.0}, + {name: "/rover_odometry/gps/fix", compress: false, rate: 5.0}] + + + + + + + + [{name: "/rover_status/jetson_status", compress: false, rate: 1.0}] + + + + + + + + [{name: "/rover_status/misc_status", compress: false, rate: 1.0}] + + + + + + + + [ + {name: "/rover_status/battery_status", compress: false, rate: 1.0}, + {name: "/rover_control/tower/status/co2", compress: false, rate: 1.0}, + {name: "/rover_odometry/imu/data", compress: false, rate: 10.0}, + {name: "/rover_control/mining/status", compress: false, rate: 5.0}, + {name: "/rover_control/gripper/status", compress: false, rate: 5.0}, + {name: "/rover_science/soil_probe/data", compress: false, rate: 5.0}, + {name: "/rover_science/rdf/data", compress: false, rate: 50.0}, + {name: "/rover_control/scale/measurement", compress: false, rate: 20.0} + ] + + + + + + + + [ + {name: "/rover_arm/status", compress: false, rate: 5.0} + ]