Skip to content

Prakarsh gsoc2022 rotors driver #185

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
pariaspe wants to merge 5 commits into
base: noetic-devel
Choose a base branch
from
Open

Prakarsh gsoc2022 rotors driver #185

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
944422b
bugfix for rotors cam topic issue
RUFFY-369 Sep 2, 2022
de4654f
changed model to iris for optimization in docker container
RUFFY-369 Sep 3, 2022
e3f1643
changes made to fix drone_cat_mouse ex bug
RUFFY-369 Sep 5, 2022
8f20e41
removes initial irrelevant error
RUFFY-369 Sep 16, 2022
d0d1cc5
minor pos control bugfix occuring with some input values
RUFFY-369 Sep 19, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
87 changes: 63 additions & 24 deletions drone_wrapper/src/drone_wrapper/drone_wrapper_class.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"]:
Expand All @@ -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:
Expand All @@ -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:
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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()
Expand Down Expand Up @@ -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)

Expand All @@ -672,4 +711,4 @@ def __init__(self, name='drone', ns='', verbose=False):

if __name__ == "__main__":
drone = DroneWrapper()
rospy.spin()
rospy.spin()
42 changes: 20 additions & 22 deletions rotors_driver/src/rotors_driver_node.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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):

Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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):
Expand All @@ -357,4 +355,4 @@ def __del__(self):

except rospy.ROSInterruptException:
print("ROS Terminated")
del rotors
del rotors
⚡