def __init__(self):
# publisher
self.error_publisher = rospy.Publisher('/riseq/state_error_publisher',
Odometry,
queue_size=10)
self.state = Odometry()
self.state.pose.pose.orientation.x = 0.0
self.state.pose.pose.orientation.y = 0.0
self.state.pose.pose.orientation.z = 0.0
self.state.pose.pose.orientation.w = 1.0
self.traj = riseq_uav_trajectory()
self.traj.rot = [1, 0, 0, 0, 1, 0, 0, 0, 1]
self.received_first = False
self.pos_sub = rospy.Subscriber('/mavros/local_position/pose',
PoseStamped, self.position_cb)
self.vel_sub = rospy.Subscriber(
'/mavros/local_position/velocity_local', TwistStamped,
self.velocity_cb)
self.reftraj_sub = rospy.Subscriber('riseq/trajectory/uav_trajectory',
riseq_uav_trajectory, self.traj_cb)
self.publish_timer = rospy.Timer(rospy.Duration(0.01),
self.publish_error)
def pub_traj(self):
hz = rospy.get_param('riseq/trajectory_update_rate', 200)
# publish at Hz
rate = rospy.Rate(hz)
while not rospy.is_shutdown():
ref_traj = self.compute_reference_traj()
# create and fill message
traj = riseq_uav_trajectory()
traj.header.stamp = rospy.Time.now()
traj.header.frame_id = ""
x, y, z = ref_traj[0]
vx, vy, vz = ref_traj[1]
ax, ay, az = ref_traj[10]
jx, jy, jz = ref_traj[11]
sx, sy, sz = ref_traj[12]
phi, theta, psi = ref_traj[2]
p, q, r = ref_traj[3]
yaw = ref_traj[13]
yawdot = ref_traj[14]
yawddot = ref_traj[15]
uax, uay, uaz = ref_traj[4]
ubx, uby, ubz = ref_traj[5]
ucx, ucy, ucz = ref_traj[6]
Rbw = np.array(ref_traj[9]).flatten().tolist()
# Input (T, M) publisher to be used in estimation
u_1 = ref_traj[7]
u_xx, u_xy, u_xz = ref_traj[8]
traj.pose.position.x = x
traj.pose.position.y = y
traj.pose.position.z = z
# Following http://docs.ros.org/jade/api/tf/html/python/transformations.html
# the axes parameter is such that
# r = apply rotation on the "new" frame
# zyx = this means first a rotation of 'psi' radians around the z axis is performed,
# then of 'theta' radians about the new y axis ( because 'r' was specified)
# then of 'phi' radians about the new x axis ( becuase 'r' was specified)
quat = tf.transformations.quaternion_from_euler(psi,
theta,
phi,
axes='rzyx')
traj.pose.orientation.x = quat[0]
traj.pose.orientation.y = quat[1]
traj.pose.orientation.z = quat[2]
traj.pose.orientation.w = quat[3]
traj.twist.linear.x = vx
traj.twist.linear.y = vy
traj.twist.linear.z = vz
traj.twist.angular.x = p
traj.twist.angular.y = q
traj.twist.angular.z = r
traj.ua.x = uax
traj.ua.y = uay
traj.ua.z = uaz
traj.ub.x = ubx
traj.ub.y = uby
traj.ub.z = ubz
traj.uc.x = ucx
traj.uc.y = ucy
traj.uc.z = ucz
traj.rot = Rbw
traj.acc.x = ax
traj.acc.y = ay
traj.acc.z = az
traj.jerk.x = jx
traj.jerk.y = jy
traj.jerk.z = jz
traj.snap.x = sx
traj.snap.y = sy
traj.snap.z = sz
traj.yaw = yaw
traj.yawdot = yawdot
traj.yawddot = yawddot
# publish message
self.traj_pub.publish(traj)
#rospy.loginfo(traj)
rate.sleep()
def pub_traj():
# init node
rospy.init_node('riseq_uav_simple_trajectory_publisher', anonymous=True)
# create topic for publishing ref trajectory
traj_publisher = rospy.Publisher('riseq/tests/uav_simple_trajectory',
riseq_uav_trajectory,
queue_size=10)
# wait time for simulator to get ready...
wait_time = int(rospy.get_param("riseq/trajectory_wait"))
while (rospy.Time.now().to_sec() < wait_time):
if ((rospy.Time.now().to_sec() % 1.0) == 0.0):
#rospy.loginfo("Starting Trajectory Generator in {:.2f} seconds".format(wait_time - rospy.Time.now().to_sec()))
continue
# create a trajectory generator
traj_gen = Trajectory_Generator2()
# IMPORTANT WAIT TIME! DO NOT DELETE!
#rospy.sleep(0.1)
# If this is not here, the "start_time" in the trajectory generator is
# initialized to zero (because the node has not started fully) and the
# time for the trajectory will be degenerated
while (not traj_gen.received_init_pose):
continue
print("\n\n >>Init pose: \n {} \n\n".format(traj_gen.init_pose))
traj_gen.compute_waypoints()
# publish at 10Hz
while ((traj_gen.mavros_state.armed != True)
or (traj_gen.mavros_state.mode != 'OFFBOARD')):
#print(" >> Trajectory generator waiting for Drone ARMing")
continue
traj_gen.start_time = rospy.get_time()
rate = rospy.Rate(rospy.get_param('riseq/trajectory_update_rate', 200))
print("\n\n >>>>> Trajectory rate: {} <<<<<<".format(
rospy.get_param('riseq/trajectory_update_rate', 200)))
while not rospy.is_shutdown():
try:
# Compute trajectory at time = now
time = rospy.get_time()
ref_traj = traj_gen.compute_reference_traj(time)
# create and fill message
traj = riseq_uav_trajectory()
traj.header.stamp = rospy.Time.now()
traj.header.frame_id = ""
# get all values... we need to convert to np.array()
# becuase ref_traj contains old, ugly np.matrix
# objects >:(
x, y, z = np.array(ref_traj[0]).flatten()
vx, vy, vz = np.array(ref_traj[1]).flatten()
ax, ay, az = np.array(ref_traj[10]).flatten()
jx, jy, jz = np.array(ref_traj[11]).flatten()
sx, sy, sz = np.array(ref_traj[12]).flatten()
phi, theta, psi = np.array(ref_traj[2]).flatten()
p, q, r = np.array(ref_traj[3]).flatten()
yaw = ref_traj[13].item(0)
yawdot = ref_traj[14].item(0)
yawddot = ref_traj[15].item(0)
uax, uay, uaz = np.array(ref_traj[4]).flatten()
ubx, uby, ubz = np.array(ref_traj[5]).flatten()
ucx, ucy, ucz = np.array(ref_traj[6]).flatten()
Rbw = np.array(ref_traj[9]).flatten().tolist()
#print("ref_traj[9]: {}".format(ref_traj[9]))
# Input (T, M) publisher to be used in estimation
u_1 = np.array(ref_traj[7]).flatten()
u_xx, u_xy, u_xz = np.array(ref_traj[8]).flatten()
traj.pose.position.x = x
traj.pose.position.y = y
traj.pose.position.z = z
# Following http://docs.ros.org/jade/api/tf/html/python/transformations.html
# the axes parameter is such that
# r = apply rotation on the "new" frame
# zyx = this means first a rotation of 'psi' radians around the z axis is performed,
# then of 'theta' radians about the new y axis ( because 'r' was specified)
# then of 'phi' radians about the new x axis ( becuase 'r' was specified)
quat = tf.transformations.quaternion_from_euler(psi,
theta,
phi,
axes='rzyx')
traj.pose.orientation.x = quat[0]
traj.pose.orientation.y = quat[1]
traj.pose.orientation.z = quat[2]
traj.pose.orientation.w = quat[3]
traj.twist.linear.x = vx
traj.twist.linear.y = vy
traj.twist.linear.z = vz
traj.twist.angular.x = p
traj.twist.angular.y = q
traj.twist.angular.z = r
traj.ua.x = uax
traj.ua.y = uay
traj.ua.z = uaz
traj.ub.x = ubx
traj.ub.y = uby
traj.ub.z = ubz
traj.uc.x = ucx
traj.uc.y = ucy
traj.uc.z = ucz
traj.rot = Rbw
traj.acc.x = ax
traj.acc.y = ay
traj.acc.z = az
traj.jerk.x = jx
traj.jerk.y = jy
traj.jerk.z = jz
traj.snap.x = sx
traj.snap.y = sy
traj.snap.z = sz
traj.yaw = yaw
traj.yawdot = yawdot
traj.yawddot = yawddot
# publish message
traj_publisher.publish(traj)
#rospy.loginfo(traj)
rate.sleep()
except Exception:
rospy.loginfo('People...we have a problem: {}'.format(Exception))
continue
def __init__(self):
# determine environment
environment = rospy.get_param("riseq/environment")
# high level control publisher
self.hlc_pub = rospy.Publisher('riseq/control/uav_high_level_control', riseq_high_level_control, queue_size = 10)
self.px4_pub = rospy.Publisher('/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size = 10)
# flightgoggles publisher
if(environment == "simulator"):
self.fg_publisher = rospy.Publisher('/uav/input/rateThrust', RateThrust, queue_size = 10)
else:
pass
# reference trajectory subscriber
self.reftraj_sub = rospy.Subscriber('riseq/trajectory/uav_trajectory', riseq_uav_trajectory, self.traj_cb)
# select controller's state input soure: true state, estimated state
try:
self.state_input = rospy.get_param('riseq/controller_state_input')
except:
print('riseq/controller_state_input parameter unavailable')
print(' Setting controller state input to: true_state')
print(' The only possible controller input states are: true_state, estimated_state, fg_true_state')
self.state_input = 'true_state'
if(self.state_input == 'estimated_state'):
self.state_sub = rospy.Subscriber('riseq/estimator/uav_estimated_state', riseq_uav_state, self.state_cb)
elif(self.state_input == 'true_state'):
self.state_sub = rospy.Subscriber('riseq/tests/uav_ot_true_state', riseq_uav_state, self.state_cb)
elif(self.state_input == 'fg_true_state'):
# for flight googles simulator
#self.state_sub = rospy.Subscriber('/mavros/local_position/odom', Odometry, self.state_cb)
self.pos_sub = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.position_cb)
self.vel_sub = rospy.Subscriber('/mavros/local_position/velocity_local', TwistStamped, self.velocity_cb)
#self.state_sub = message_filters.Subscriber('/pelican/odometry_sensor1/odometry', Odometry)
else:
print('riseq/controller_state_input parameter not recognized. Defaulting to true_state')
print(' The only possible controller input states are: true_state, estimated_state')
self.state_sub = rospy.Subscriber('riseq/tests/uav_ot_true_state', riseq_uav_state)
# select controller's controller type: euler angle based controller, geometric controller
# --------------------------------- #
# Initialize controller parameters #
# --------------------------------- #
self.g = rospy.get_param("riseq/gravity")
self.mass = rospy.get_param("riseq/mass")
self.thrust_coeff = rospy.get_param("riseq/thrust_coeff")
self.max_rotor_speed = rospy.get_param("riseq/max_rotor_speed")
self.rotor_count = rospy.get_param("riseq/rotor_count")
self.max_thrust = self.rotor_count*self.thrust_coeff*(self.max_rotor_speed**2) # assuming cuadratic model for rotor thrust
self.min_thrust = 0.0
self.flc = Feedback_Linearization_Controller(mass = self.mass, max_thrust = self.max_thrust, min_thrust = self.min_thrust)
self.gc = Geometric_Controller(mass = self.mass, max_thrust = self.max_thrust, min_thrust = self.min_thrust)
self.state = Odometry()
self.state.pose.pose.orientation.x = 0.0
self.state.pose.pose.orientation.y = 0.0
self.state.pose.pose.orientation.z = 0.0
self.state.pose.pose.orientation.w = 1.0
self.traj = riseq_uav_trajectory()
self.traj.rot = [1,0,0,0,1,0,0,0,1]
self.controller_type = rospy.get_param('riseq/controller_type','euler_angle_controller')
if( self.controller_type == 'euler_angle_controller'):
self.control_timer = rospy.Timer(rospy.Duration(0.01), self.euler_angle_controller)
elif( self.controller_type == 'geometric_controller'):
self.control_timer = rospy.Timer(rospy.Duration(0.01), self.geometric_controller)
else:
print('riseq/controller_type parameter not recognized. Defaulting to geometric_controller')
print(' The only possible types are: euler_angle_controller, geometric_controller')
self.control_timer = rospy.Timer(rospy.Duration(0.01), self.euler_angle_controller)
# PX4 SITL
self.mavros_state = State()
self.mavros_state.connected = False
self.mavros_state_sub = rospy.Subscriber('mavros/state', State, self.mavros_state_cb)
self.arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
self.set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)
self.local_pos_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10)
self.wait_mavros_connection()
self.last_mavros_request = rospy.Time.now()
self.enable_sim = rospy.get_param('/riseq/enable_sim', False)
if(self.enable_sim):
self.send_setpoints()
self.status_timer = rospy.Timer(rospy.Duration(0.5), self.mavros_status_cb)