From 944422beb1a5515dcceab3732dcde4ef3da63e65 Mon Sep 17 00:00:00 2001 From: RUFFY-369 Date: Fri, 2 Sep 2022 15:04:23 +0530 Subject: [PATCH 1/5] bugfix for rotors cam topic issue --- drone_wrapper/src/drone_wrapper/drone_wrapper_class.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py b/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py index 20e8995..336a6fc 100755 --- a/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py +++ b/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py @@ -657,7 +657,7 @@ 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('drone_model', 'firefly') # default --> iris self.frontal_image = Image() self.ventral_image = Image() From de4654f349e4f9ad980734240cef7f84ece4e726 Mon Sep 17 00:00:00 2001 From: RUFFY-369 Date: Sat, 3 Sep 2022 19:40:05 +0530 Subject: [PATCH 2/5] changed model to iris for optimization in docker container --- .../src/drone_wrapper/drone_wrapper_class.py | 2 +- rotors_driver/src/rotors_driver_node.py | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py b/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py index 336a6fc..20e8995 100755 --- a/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py +++ b/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py @@ -657,7 +657,7 @@ 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', 'firefly') # default --> iris + drone_model = rospy.get_param('drone_model', 'iris') # default --> iris self.frontal_image = Image() self.ventral_image = Image() diff --git a/rotors_driver/src/rotors_driver_node.py b/rotors_driver/src/rotors_driver_node.py index d2cd1af..a84abc1 100755 --- a/rotors_driver/src/rotors_driver_node.py +++ b/rotors_driver/src/rotors_driver_node.py @@ -40,7 +40,7 @@ class RotorsDriver(): def __init__(self): self.sample_time = rospy.get_param('sample_time', 1.0) - self.mav_name = rospy.get_param('drone_model', 'firefly') + self.mav_name = rospy.get_param('drone_model', 'iris') 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.drone_flight_state = States.LANDED @@ -97,7 +97,7 @@ def __init__(self): rospy.Subscriber('/' + self.mav_name +"/ground_truth/odometry", Odometry, self.odom_callback) rospy.Subscriber('/' + self.mav_name +"/frontal_cam/camera_nadir/image_raw", Image, self.cam_frontal) rospy.Subscriber('/' + self.mav_name +"/ventral_cam/camera_nadir/image_raw", Image, self.cam_ventral) - self.firefly_command_publisher = rospy.Publisher('/firefly/command/trajectory', MultiDOFJointTrajectory, queue_size=10) + self.iris_command_publisher = rospy.Publisher('/iris/command/trajectory', MultiDOFJointTrajectory, queue_size=10) time.sleep(1.0) rospy.Timer(rospy.Duration(self.drone_state_upd_freq), self.drone_state_update_callback) rospy.Timer(rospy.Duration(self.misc_state_upd_freq), self.misc_state_update_callback) @@ -198,7 +198,7 @@ def rotors_landing(self,req): while not(0.0<=self.current_z<0.1): - self.firefly_command_publisher.publish(traj) + self.iris_command_publisher.publish(traj) if not req : return True, 0 @@ -213,13 +213,13 @@ def rotors_arm(self, req): # bool success # uint8 result if req.value: - rospy.loginfo("Firefly Arming") + rospy.loginfo("iris Arming") self.drone_flight_state = States.ARMING time.sleep(0.01) self.drone_flight_state = States.ARMED return True, 1 else: - rospy.loginfo("Firefly Disarming") + rospy.loginfo("iris Disarming") self.drone_flight_state = States.DISARMING time.sleep(0.01) self.drone_flight_state = States.DISARMED @@ -335,7 +335,7 @@ def publish_position_desired(self, msg): traj.points.append(point) # time.sleep(0.1) #commented out for vel control - self.firefly_command_publisher.publish(traj) + self.iris_command_publisher.publish(traj) # Deleting (Calling destructor) def __del__(self): From e3f1643d32a052ae07d2858f9f141149fdfefb34 Mon Sep 17 00:00:00 2001 From: RUFFY-369 Date: Mon, 5 Sep 2022 13:51:11 +0530 Subject: [PATCH 3/5] changes made to fix drone_cat_mouse ex bug --- .../src/drone_wrapper/drone_wrapper_class.py | 2 +- rotors_driver/src/rotors_driver_node.py | 21 ++++++++++--------- 2 files changed, 12 insertions(+), 11 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..908a872 100755 --- a/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py +++ b/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py @@ -657,7 +657,7 @@ 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 self.frontal_image = Image() self.ventral_image = Image() diff --git a/rotors_driver/src/rotors_driver_node.py b/rotors_driver/src/rotors_driver_node.py index a84abc1..e2f05cb 100755 --- a/rotors_driver/src/rotors_driver_node.py +++ b/rotors_driver/src/rotors_driver_node.py @@ -39,10 +39,11 @@ 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', 'iris') - 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.drone_flight_state = States.LANDED self.current_state = Odometry() self.current_x=self.current_state.pose.pose.position.x @@ -82,9 +83,9 @@ def __init__(self): queue_size=1) self.velocity_body_publisher = rospy.Publisher('mavros/local_position/velocity_body', TwistStamped, queue_size=1) - self.cam_frontal_publisher = rospy.Publisher('/' + self.mav_name + '/cam_frontal/image_raw', Image, + self.cam_frontal_publisher = rospy.Publisher(self.ns + 'cam_frontal/image_raw', Image, queue_size=1) - self.cam_ventral_publisher = rospy.Publisher('/' + self.mav_name + '/cam_ventral/image_raw', Image, + self.cam_ventral_publisher = rospy.Publisher(self.ns + 'cam_ventral/image_raw', Image, queue_size=1) @@ -94,10 +95,10 @@ def __init__(self): rospy.Subscriber("mavros/setpoint_raw/local", PositionTarget, self.publish_position_desired) - rospy.Subscriber('/' + self.mav_name +"/ground_truth/odometry", Odometry, self.odom_callback) - rospy.Subscriber('/' + self.mav_name +"/frontal_cam/camera_nadir/image_raw", Image, self.cam_frontal) - rospy.Subscriber('/' + self.mav_name +"/ventral_cam/camera_nadir/image_raw", Image, self.cam_ventral) - self.iris_command_publisher = rospy.Publisher('/iris/command/trajectory', MultiDOFJointTrajectory, queue_size=10) + rospy.Subscriber(self.ns + 'ground_truth/odometry', Odometry, self.odom_callback) + rospy.Subscriber(self.ns + 'frontal_cam/camera_nadir/image_raw', Image, self.cam_frontal) + rospy.Subscriber(self.ns + 'ventral_cam/camera_nadir/image_raw', Image, self.cam_ventral) + self.iris_command_publisher = rospy.Publisher(self.ns + 'command/trajectory', MultiDOFJointTrajectory, queue_size=10) time.sleep(1.0) rospy.Timer(rospy.Duration(self.drone_state_upd_freq), self.drone_state_update_callback) rospy.Timer(rospy.Duration(self.misc_state_upd_freq), self.misc_state_update_callback) From 8f20e413552457c12c70f1def5abe9117cc0f411 Mon Sep 17 00:00:00 2001 From: RUFFY-369 Date: Fri, 16 Sep 2022 19:12:55 +0530 Subject: [PATCH 4/5] removes initial irrelevant error --- rotors_driver/src/rotors_driver_node.py | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/rotors_driver/src/rotors_driver_node.py b/rotors_driver/src/rotors_driver_node.py index e2f05cb..17f2a62 100755 --- a/rotors_driver/src/rotors_driver_node.py +++ b/rotors_driver/src/rotors_driver_node.py @@ -88,20 +88,17 @@ def __init__(self): self.cam_ventral_publisher = rospy.Publisher(self.ns + 'cam_ventral/image_raw', Image, queue_size=1) - - - - rospy.Subscriber("mavros/setpoint_raw/local", PositionTarget, self.publish_position_desired) - rospy.Subscriber(self.ns + 'ground_truth/odometry', Odometry, self.odom_callback) rospy.Subscriber(self.ns + 'frontal_cam/camera_nadir/image_raw', Image, self.cam_frontal) rospy.Subscriber(self.ns + 'ventral_cam/camera_nadir/image_raw', Image, self.cam_ventral) - self.iris_command_publisher = rospy.Publisher(self.ns + 'command/trajectory', MultiDOFJointTrajectory, queue_size=10) + time.sleep(1.0) rospy.Timer(rospy.Duration(self.drone_state_upd_freq), self.drone_state_update_callback) rospy.Timer(rospy.Duration(self.misc_state_upd_freq), self.misc_state_update_callback) + self.iris_command_publisher = rospy.Publisher(self.ns + 'command/trajectory', MultiDOFJointTrajectory, queue_size=10) + rospy.Subscriber("mavros/setpoint_raw/local", PositionTarget, self.publish_position_desired) def drone_state_update_callback(self, event=None): @@ -358,4 +355,4 @@ def __del__(self): except rospy.ROSInterruptException: print("ROS Terminated") - del rotors \ No newline at end of file + del rotors From d0d1cc540afdfef93915b1ebe002a98bb52bd8b3 Mon Sep 17 00:00:00 2001 From: RUFFY-369 Date: Mon, 19 Sep 2022 13:06:13 +0530 Subject: [PATCH 5/5] minor pos control bugfix occuring with some input values --- .../src/drone_wrapper/drone_wrapper_class.py | 87 ++++++++++++++----- rotors_driver/src/rotors_driver_node.py | 4 +- 2 files changed, 65 insertions(+), 26 deletions(-) diff --git a/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py b/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py index 908a872..66e3bec 100755 --- a/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py +++ b/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py @@ -42,7 +42,7 @@ def get_frame(self, frame): elif str(frame).lower() in ["frd", "body_frd", "base_link_frd"]: return "base_link_frd" else: - raise self.UnknownFrameError + return def get_frame_id(self, frame): if str(frame).lower() in ["enu", "local_enu", "map", "world"]: @@ -54,7 +54,7 @@ def get_frame_id(self, frame): elif str(frame).lower() in ["frd", "body_frd", "base_link_frd"]: return 12 else: - raise self.UnknownFrameError + return def id2frame(self, id_): if id_ == self.LOCAL_ENU: @@ -66,7 +66,7 @@ def id2frame(self, id_): elif id_ == self.BODY_FRD: return "base_link_frd" else: - raise self.UnknownFrameError + return def __transform_pose(self, from_, to, pose): if from_ == to: @@ -532,22 +532,61 @@ def takeoff(self, h=3, precision=0.05): rospy.loginfo('Drone is already flying!') return - self.set_cmd_pos(0, 0, 0, 0) - self.hold_setpoint_raw() - self.arm(True) - self.stay_armed_stay_offboard_timer = rospy.Timer(rospy.Duration(3), self.stay_armed_stay_offboard_cb) - while True: - while not (self.state.armed and self.state.mode == 'OFFBOARD'): - self.rate.sleep() - rospy.loginfo('Sleeping 1 secs to confirm change') - rospy.sleep(1) - if self.state.mode == 'OFFBOARD': - break - self.set_cmd_mix(z=h) - rospy.loginfo('Taking off!!!') - while True: - if abs(self.pose_stamped.pose.position.z - h) < precision: - break + if self.drone_model == 'iris' or self.drone_model == 'iris1': #This condition is specifically for solving Rotors sim bug so dev + #who are using drone_model name as iris, please try to change it or vary the script accordingly for rotors + + self.arm(True) + self.stay_armed_stay_offboard_timer = rospy.Timer(rospy.Duration(3), self.stay_armed_stay_offboard_cb) + pose = Pose(position=Point(x=self.pose_stamped.pose.position.x, + y=self.pose_stamped.pose.position.y, z=h)) + pose = self.frames_tf.transform("map", "map", pose).pose + + twist = Twist(linear=Vector3(x=0.0, y=0.0, z=0), + angular=Vector3(x=0, y=0, z=0.0)) + twist = self.frames_tf.transform("flu", "flu", twist).twist + + self.coord_frame = 8 + self.setpoint_raw.header.frame_id = self.frames_tf.get_frame("flu") + self.setpoint_raw.coordinate_frame = self.coord_frame + + self.posx = self.pose_stamped.pose.position.x + self.posy = self.pose_stamped.pose.position.y + self.height = pose.position.z + self.vx = twist.linear.x + self.vy = twist.linear.y + self.vz = 0 + + self.setpoint_raw.position.z = pose.position.z + self.setpoint_raw.velocity.x = twist.linear.x + self.setpoint_raw.velocity.y = twist.linear.y + self.setpoint_raw.yaw_rate = twist.angular.z + + global CMD + CMD = 2 # MIX + self.setpoint_raw.type_mask = 1987 # vx vy vz z yaw_rate + + self.setpoint_raw_publisher.publish(self.setpoint_raw) + + rospy.loginfo('Taking off!!!') + + + else: + self.set_cmd_pos(0, 0, 0, 0) + self.hold_setpoint_raw() + self.arm(True) + self.stay_armed_stay_offboard_timer = rospy.Timer(rospy.Duration(3), self.stay_armed_stay_offboard_cb) + while True: + while not (self.state.armed and self.state.mode == 'OFFBOARD'): + self.rate.sleep() + rospy.loginfo('Sleeping 1 secs to confirm change') + rospy.sleep(1) + if self.state.mode == 'OFFBOARD': + break + self.set_cmd_mix(z=h) + rospy.loginfo('Taking off!!!') + while True: + if abs(self.pose_stamped.pose.position.z - h) < precision: + break self.set_cmd_vel() # NOT USED @@ -591,7 +630,7 @@ def __init__(self, name='drone', ns='', verbose=False): rospy.on_shutdown(self.shutdown) - drone_model = rospy.get_param('drone_model', 'iris') # default --> iris + self.drone_model = rospy.get_param('drone_model', 'iris') # default --> iris self.state = State() self.extended_state = ExtendedState() @@ -657,12 +696,12 @@ 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(self.ns +'drone_model', 'iris') # default --> iris + self.drone_model = rospy.get_param(self.ns +'drone_model', 'iris') # default --> iris self.frontal_image = Image() self.ventral_image = Image() - cam_frontal_topic = '/' + drone_model + '/cam_frontal/image_raw' - cam_ventral_topic = '/' + drone_model + '/cam_ventral/image_raw' + cam_frontal_topic = '/' + self.drone_model + '/cam_frontal/image_raw' + cam_ventral_topic = '/' + self.drone_model + '/cam_ventral/image_raw' rospy.Subscriber(cam_frontal_topic, Image, self.cam_frontal_cb) rospy.Subscriber(cam_ventral_topic, Image, self.cam_ventral_cb) @@ -672,4 +711,4 @@ def __init__(self, name='drone', ns='', verbose=False): 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 17f2a62..bc3b321 100755 --- a/rotors_driver/src/rotors_driver_node.py +++ b/rotors_driver/src/rotors_driver_node.py @@ -288,13 +288,13 @@ def publish_position_desired(self, msg): self.drone_flight_state = States.TAKINGOFF - if (desired_x_to_go ==0.0 and desired_y_to_go ==0.0 and yaw_des == 0.0) and (vx_des >0.0 or vy_des >0.0 or yaw_rate_des>0) and vz_des >=0.0: + if (desired_x_to_go ==0.0 and desired_y_to_go ==0.0 and yaw_des == 0.0 and desired_z_to_go == 0.0) and (vx_des >0.0 or vy_des >0.0 or yaw_rate_des>0) and vz_des >=0.0: desired_x_to_go=self.current_x+(vx_des*self.sample_time) desired_y_to_go=self.current_y+(vy_des*self.sample_time) desired_z_to_go=self.current_z+(vz_des*self.sample_time) desired_yaw_to_go = self.current_yaw +(yaw_rate_des*self.sample_time) print("Vel control") - elif (desired_x_to_go ==0.0 and desired_y_to_go ==0.0 and vz_des ==0.0 and yaw_des == 0.0) and ((vx_des >0.0 or vy_des >0.0 or yaw_rate_des>0)and desired_z_to_go>0.0 ): + elif (desired_x_to_go ==0.0 and desired_y_to_go ==0.0 and vz_des ==0.0 and yaw_des == 0.0) and ((vx_des >0.0 or vy_des >0.0 or yaw_rate_des>0)and desired_z_to_go>=0.0 ): desired_x_to_go=self.current_x+(vx_des*self.sample_time) desired_y_to_go=self.current_y+(vy_des*self.sample_time) desired_z_to_go= desired_z_to_go