def __initVars(self):
"""Subscribers vars initialisation"""
self.sensors = Sensors()
self.trans_listener = tf.TransformListener()
self.sensors.add("trans", None)
self.old_trans = None
self.sensors.add("rot", None)
self.rate_pub_enabled = False
self.messager = mav_msgs.RateThrust()
self.collision = False
def publish_actions(self, actions):
"""Publish control actions.
:param actions: [u_thrust, u_phi, u_theta, u_psi]
:return:
"""
tmp_msg = mav_msgs.RateThrust()
tmp_msg.thrust.z = actions[0]
tmp_msg.angular_rates.x = actions[1]
tmp_msg.angular_rates.y = actions[2]
tmp_msg.angular_rates.z = actions[3]
self.thrust_publisher.publish(tmp_msg)
self.rate.sleep()
def simple_commander():
pub = rospy.Publisher('/uav/input/rateThrust',
mav_msgs.RateThrust,
queue_size=10)
rospy.init_node('commander', anonymous=True)
rospy.Subscriber('/uav/sensors/downward_laser_rangefinder', s_msgs.Range,
callback)
rate = rospy.Rate(1) # 10hz
while not rospy.is_shutdown():
tmp_msg = mav_msgs.RateThrust()
tmp_msg.thrust.z = 100
rospy.loginfo(tmp_msg)
pub.publish(tmp_msg)
rate.sleep()