From fd6d5dbb3b354a6e241535f8f78ee4593680a3e0 Mon Sep 17 00:00:00 2001 From: RUFFY-369 Date: Tue, 13 Sep 2022 13:37:28 +0530 Subject: [PATCH] add depth cam functionality to rotors driver and drone wrapper --- .../src/drone_wrapper/drone_wrapper_class.py | 35 ++++++++++++++++-- rotors_driver/src/rotors_driver_node.py | 36 +++++++++++++++---- 2 files changed, 62 insertions(+), 9 deletions(-) diff --git a/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py b/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py index 20e8995..cdd01be 100755 --- a/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py +++ b/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py @@ -263,6 +263,18 @@ def cam_ventral_cb(self, msg): rospy.logdebug('Ventral image updated') self.rqt_cam_ventral_publisher.publish(self.ventral_image) + + def depth_cam_frontal_cb(self, msg): + self.depth_frontal_image = msg + rospy.logdebug('Frontal depth image updated') + + self.rqt_depth_cam_frontal_publisher.publish(self.depth_frontal_image) + + def depth_cam_ventral_cb(self, msg): + self.depth_ventral_image = msg + rospy.logdebug('Ventral depth image updated') + + self.rqt_depth_cam_ventral_publisher.publish(self.depth_ventral_image) def stay_armed_stay_offboard_cb(self, event): try: @@ -280,6 +292,12 @@ def get_frontal_image(self): def get_ventral_image(self): return self.bridge.imgmsg_to_cv2(self.ventral_image) + + def get_depth_frontal_image(self): + return self.bridge.imgmsg_to_cv2(self.depth_frontal_image) + + def get_depth_ventral_image(self): + return self.bridge.imgmsg_to_cv2(self.depth_ventral_image) def get_position(self, frame="map"): ps = self.frames_tf.transform(self.pose_stamped.header.frame_id, frame, @@ -657,7 +675,8 @@ def __init__(self, name='drone', ns='', verbose=False): # cam_frontal_topic = rospy.get_param('cam_frontal_topic', None) # cam_frontal_topic = rospy.get_param('cam_frontal_topic', '/iris/cam_frontal/image_raw') - drone_model = rospy.get_param('drone_model', 'iris') # default --> iris + drone_model = rospy.get_param(self.ns +'drone_model', 'iris') # default --> iris + enable_depth = rospy.get_param(self.ns +'enable_depth', False) self.frontal_image = Image() self.ventral_image = Image() @@ -666,10 +685,22 @@ def __init__(self, name='drone', ns='', verbose=False): rospy.Subscriber(cam_frontal_topic, Image, self.cam_frontal_cb) rospy.Subscriber(cam_ventral_topic, Image, self.cam_ventral_cb) + if enable_depth: + self.rqt_depth_cam_frontal_publisher = rospy.Publisher(self.ns + 'drone_wrapper/depth_cam_frontal/image_raw', Image, + queue_size=1) + self.rqt_depth_cam_ventral_publisher = rospy.Publisher(self.ns + 'drone_wrapper/depth_cam_ventral/image_raw', Image, + queue_size=1) + self.depth_frontal_image = Image() + self.depth_ventral_image = Image() + depth_cam_frontal_topic = '/' + drone_model + '/depth_cam_frontal/image_raw' + depth_cam_ventral_topic = '/' + drone_model + '/depth_cam_ventral/image_raw' + rospy.Subscriber(depth_cam_frontal_topic, Image, self.depth_cam_frontal_cb) + rospy.Subscriber(depth_cam_ventral_topic, Image, self.depth_cam_ventral_cb) + self.setpoint_raw_publisher = rospy.Publisher(self.ns + 'mavros/setpoint_raw/local', PositionTarget, queue_size=1) if __name__ == "__main__": drone = DroneWrapper() - rospy.spin() \ No newline at end of file + rospy.spin() diff --git a/rotors_driver/src/rotors_driver_node.py b/rotors_driver/src/rotors_driver_node.py index d2cd1af..622126a 100755 --- a/rotors_driver/src/rotors_driver_node.py +++ b/rotors_driver/src/rotors_driver_node.py @@ -39,10 +39,12 @@ class RotorsDriver(): CONSTRAINTS = RotorsConstraints() def __init__(self): - self.sample_time = rospy.get_param('sample_time', 1.0) - self.mav_name = rospy.get_param('drone_model', 'firefly') - self.drone_state_upd_freq = rospy.get_param('drone_state_timer_frequency', 0.01) - self.misc_state_upd_freq = rospy.get_param('misc_state_timer_frequency', 1.0) + self.ns = rospy.get_namespace() + self.sample_time = rospy.get_param(self.ns +'sample_time', 1.0) + self.mav_name = rospy.get_param(self.ns +'drone_model', 'iris') + self.drone_state_upd_freq = rospy.get_param(self.ns +'drone_state_timer_frequency', 0.01) + self.misc_state_upd_freq = rospy.get_param(self.ns +'misc_state_timer_frequency', 1.0) + self.enable_depth = rospy.get_param(self.ns +'enable_depth', False) self.drone_flight_state = States.LANDED self.current_state = Odometry() self.current_x=self.current_state.pose.pose.position.x @@ -88,8 +90,16 @@ def __init__(self): queue_size=1) - - + if self.enable_depth: + self.depth_frontal_image = Image() + self.depth_ventral_image = Image() + self.depth_cam_frontal_publisher = rospy.Publisher(self.ns + 'depth_cam_frontal/image_raw', Image, + queue_size=1) + self.depth_cam_ventral_publisher = rospy.Publisher(self.ns + 'depth_cam_ventral/image_raw', Image, + queue_size=1) + rospy.Subscriber(self.ns + 'vi_sensor_frontal/camera_depth/camera/image_raw', Image, self.depth_cam_frontal) + rospy.Subscriber(self.ns + 'vi_sensor_ventral/camera_depth/camera/image_raw', Image, self.depth_cam_ventral) + rospy.Subscriber("mavros/setpoint_raw/local", PositionTarget, self.publish_position_desired) @@ -117,6 +127,10 @@ def drone_state_update_callback(self, event=None): self.cam_frontal_publisher.publish(self.frontal_image) self.cam_ventral_publisher.publish(self.ventral_image) + + if self.enable_depth: + self.depth_cam_frontal_publisher.publish(self.depth_frontal_image) + self.depth_cam_ventral_publisher.publish(self.depth_ventral_image) def misc_state_update_callback(self, event=None): landed_state = 2 if self.drone_flight_state == 4 else 1 @@ -148,6 +162,14 @@ def cam_frontal(self, msg): def cam_ventral(self, msg): self.ventral_image = msg rospy.logdebug('Ventral image updated') + + def depth_cam_frontal(self, msg): + self.depth_frontal_image = msg + rospy.logdebug('Frontal depth image updated') + + def depth_cam_ventral(self, msg): + self.depth_ventral_image = msg + rospy.logdebug('Ventral depth image updated') def odom_callback(self,msg): self.current_state = msg @@ -357,4 +379,4 @@ def __del__(self): except rospy.ROSInterruptException: print("ROS Terminated") - del rotors \ No newline at end of file + del rotors