diff --git a/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py b/drone_wrapper/src/drone_wrapper/drone_wrapper_class.py index 20e8995..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('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 d2cd1af..bc3b321 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', '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.drone_flight_state = States.LANDED self.current_state = Odometry() self.current_x=self.current_state.pose.pose.position.x @@ -82,25 +83,22 @@ 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) - - - - 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.firefly_command_publisher = rospy.Publisher('/firefly/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) + 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): @@ -198,7 +196,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 +211,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 @@ -290,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 @@ -335,7 +333,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): @@ -357,4 +355,4 @@ def __del__(self): except rospy.ROSInterruptException: print("ROS Terminated") - del rotors \ No newline at end of file + del rotors