def move_base(self, action, horizon=1, update_rate=100, init= False):
"""
It will move the base based on the linear and angular velocties given.
:param action: u_x, u_y, u_z
:param horizon: Prediction horizon for MAV always(1)
:param update_rate: Rate at which we check the odometry.
:return:
"""
import tf
#use LTI dynamics with A = I with velocity control input
# for k in range(horizon):
# outPose.position.x += outPose.velocity.x/update_rate
# outPose.position.y += outPose.velocity.y/update_rate
outPose = uav_pose()
outPose.header.stamp = rospy.Time.now()
outPose.header.frame_id="world"
if init:
outPose.POI.x = 0
outPose.POI.y = 0
outPose.POI.z = 0
r = 8
t = np.random.choice(5,1);
outPose.position.x = r*np.cos(self.theta[t[0]])
outPose.position.y = r*np.sin(self.theta[t[0]])
outPose.position.z = -r
else:
(trans,rot) = self.listener.lookupTransform( 'world_ENU',self.rotors_machine_name+'/base_link', rospy.Time(0))
(r,p,y) = tf.transformations.euler_from_quaternion(rot)
homogeneous_matrix = tf.transformations.euler_matrix(0,0,y,axes='sxyz')
homogeneous_matrix[0:3,3] = trans
yaw = y + action[2]
outPose.POI.x = trans[0]+2*np.cos(yaw)
outPose.POI.y = trans[1]+2*np.sin(yaw)
outPose.POI.z = 1
rospy.logwarn("END ACTION ==>"+str(action))
gt_odom = self.get_gt_odom()
act = np.append(action[0:2],0)
goal = homogeneous_matrix.dot(np.concatenate((np.array(act),np.array([1]))))
rospy.logwarn("END ACTION==>"+str(action))
targetz = self.gt_target_cache.getElemBeforeTime(rospy.Time.now())
#Publish Control Command with Fixed Height of 8m
outPose.position.x = goal[0]
outPose.position.y = goal[1]
outPose.position.z = -8+targetz.pose.pose.position.z
rospy.logwarn("Current MAV Pose ==>x:"+str(gt_odom.position.x)+", y:"+str(gt_odom.position.y)+", z:"+str(gt_odom.position.z))
rospy.logdebug("Firefly Command>>" + str(outPose))
self._check_publishers_connection()
self._cmd_vel_pub.publish(outPose)
rate = rospy.Rate(update_rate)
try:
rate.sleep()
except:
pass
def __init__(self,**kwargs):
"""
Initializes a new FireflyEnv environment.
To check any topic we need to have the simulations running, we need to do two things:
1) Unpause the simulation: without that the stream of data doesnt flow. This is for simulations
that are pause for whatever the reason
2) If the simulation was running already for some reason, we need to reset the controllers.
This has to do with the fact that some plugins with tf, dont understand the reset of the simulation
and need to be reseted to work properly.
The Sensors: The sensors accesible are the ones considered usefull for AI learning.
Sensor Topic List:
* /pose : Reads the estimated pose of the MAV. Type: uav_msgs
* /target_tracker/pose : Reads the fused estimate of the target person. Type: geometry_msgs
Actuators Topic List:
* /command, publishes the desired waypoints and velocity for the robot. Type: uav_msgs
Args:
"""
rospy.logdebug("Start FireflyEnv INIT...")
rospy.logwarn('Setting Up Firefly ENV')
# Variables that we give through the constructor.
# None in this case
# Internal Vars
# Doesnt have any accesibles
self.controllers_list = []
# It doesnt use namespace
self.robot_name_space = ""
# We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
super(FireflyMultiAgentGTEnv, self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=False,
start_init_physics_parameters=False,
reset_world_or_sim="WORLD",**kwargs)
machine_name = '/machine_'+str(self.robotID);rotors_machine_name = '/firefly_'+str(self.robotID)
self.rotors_machine_name = rotors_machine_name
self.num_robots=2
if self.robotID==self.num_robots:
neighbor_name = '/machine_'+str((self.robotID+1)%self.num_robots);rotors_neighbor_name = '/firefly_'+str((self.robotID+1)%self.num_robots)
else:
neighbor_name = '/machine_'+str(self.robotID+1);rotors_neighbor_name = '/firefly_'+str(self.robotID+1)
self.rotors_neighbor_name = rotors_neighbor_name
self.pose_topic = machine_name+"/pose"
self.pose_neighbor_topic = neighbor_name+"/pose"
self.velocity_topic = rotors_machine_name+"/ground_truth/odometry"
self.velocity_neighbor_topic = rotors_neighbor_name+"/ground_truth/odometry"
self.gt_pose_topic = machine_name+"/pose/groundtruth"
self.gt_neighbor_pose_topic = neighbor_name+"/pose/groundtruth"
self.firefly_pose_topic = rotors_machine_name+"/ground_truth/pose_with_covariance"
self.target_topic = machine_name+"/target_tracker/pose"
self.gt_target_topic = "/actorpose"
self.gt_target_vel_topic = "/actorvel"
self.target_velocity_topic = machine_name+"/target_tracker/twist"
self.command_topic = machine_name+"/command"
self.neighbor_command_topic = neighbor_name+"/command"
self.destination_topic = machine_name+"/destination"
self.detections_feedback_topic = machine_name+"/object_detections/feedback"
self.noisy_joints_topic = machine_name+"/noisy_joints"
self.noisy_joints_neighbor_topic = neighbor_name+"/noisy_joints"
self.noisy_bbox_topic = machine_name+"/noisy_bbox"
self.noisy_bbox_neighbor_topic = neighbor_name+"/noisy_bbox"
self.noisy_detection = machine_name+"/noisy_detection"
self.noisy_neighbor_detection = neighbor_name+"/noisy_detection"
self.alphapose_topic = machine_name+"/result_alpha"
self.alphapose_neighbor_topic = neighbor_name+"/result_alpha"
self.alphapose_bbox_topic = machine_name+"/result_bbox"
self.alphapose_bbox_neighbor_topic = neighbor_name+"/result_bbox"
self.alphapose_detection = machine_name+"/detection"
self.alphapose_neighbor_detection = neighbor_name+"/detection"
self.mhmr_topic = "/multihmr_joints"
self.camera_info = rotors_machine_name+rotors_machine_name+"/xtion/rgb/camera_info"
self.camera_info_neighbor = rotors_neighbor_name+rotors_neighbor_name+"/xtion/rgb/camera_info"
self.joints_gt = '/gt_joints'
self.mpc_command = machine_name+"/command_MPC"
import tf
self.listener = tf.TransformListener()
self.gazebo.unpauseSim()
#self.controllers_object.reset_controllers()
# We Start all the ROS related Subscribers and publishers
rospy.Subscriber(self.firefly_pose_topic,PoseWithCovarianceStamped , self._firefly_pose_callback)
pose = message_filters.Subscriber(self.pose_topic, uav_pose)
self.pose_cache = message_filters.Cache(pose,100)
pose_neighbor = message_filters.Subscriber(self.pose_neighbor_topic, uav_pose)
self.pose_neighbor_cache = message_filters.Cache(pose_neighbor,100)
velocity = message_filters.Subscriber(self.velocity_topic, Odometry)
self.velocity_cache = message_filters.Cache(velocity,300)
velocity_neighbor = message_filters.Subscriber(self.velocity_neighbor_topic, Odometry)
self.velocity_neighbor_cache = message_filters.Cache(velocity_neighbor,300)
rospy.Subscriber(self.gt_pose_topic, uav_pose, self._gt_pose_callback)
rospy.Subscriber(self.gt_neighbor_pose_topic, uav_pose,self._gt_neighbor_pose_callback)
# self.gt_neighbor_cache = message_filters.Cache(gt_neighbor,300);
#Estimated Target Pose
target = message_filters.Subscriber(self.target_topic, PoseWithCovarianceStamped)
self.target_cache = message_filters.Cache(target,100)
#GT Target Pose and Velocity
gt_target = message_filters.Subscriber(self.gt_target_topic, Odometry)
self.gt_target_cache = message_filters.Cache(gt_target,300)
#Estimated Target Velocity
target_velocity = message_filters.Subscriber(self.target_velocity_topic, TwistWithCovarianceStamped)
self.target_velocity_cache = message_filters.Cache(target_velocity,100)
rospy.Subscriber(self.detections_feedback_topic, NeuralNetworkFeedback, self._detections_feedback_callback)
noisy_joints = message_filters.Subscriber(self.noisy_joints_topic, AlphaRes)
self.noisy_joints_cache = message_filters.Cache(noisy_joints, 300);self.noisy_joints_cache.registerCallback(self._noisy_joints_callback)
noisy_joints_neighbor = message_filters.Subscriber(self.noisy_joints_neighbor_topic, AlphaRes)
self.noisy_joints_neighbor_cache = message_filters.Cache(noisy_joints_neighbor, 300);self.noisy_joints_neighbor_cache.registerCallback(self._noisy_joints_neighbor_callback)
noisy_bbox = message_filters.Subscriber(self.noisy_bbox_topic, AlphaRes)
self.noisy_bbox_cache = message_filters.Cache(noisy_bbox, 300);self.noisy_bbox_cache.registerCallback(self._noisy_bbox_callback)
noisy_bbox_neighbor = message_filters.Subscriber(self.noisy_bbox_neighbor_topic, AlphaRes)
self.noisy_bbox_neighbor_cache = message_filters.Cache(noisy_bbox_neighbor, 300);self.noisy_bbox_neighbor_cache.registerCallback(self._noisy_bbox_neighbor_callback)
rospy.Subscriber(self.noisy_detection, Int16,self._noisy_detection_callback)
rospy.Subscriber(self.noisy_neighbor_detection, Int16,self._noisy_neighbor_detection_callback)
alphapose = message_filters.Subscriber(self.alphapose_topic, AlphaRes)
self.alphapose_cache = message_filters.Cache(alphapose,300);self.alphapose_cache.registerCallback(self._alphapose_callback)
alphapose_neighbor = message_filters.Subscriber(self.alphapose_neighbor_topic, AlphaRes)
self.alphapose_neighbor_cache= message_filters.Cache(alphapose_neighbor,100);self.alphapose_neighbor_cache.registerCallback(self._alphapose_neighbor_callback)
alphapose_bbox = message_filters.Subscriber(self.alphapose_bbox_topic, AlphaRes)
self.alphapose_bbox_cache= message_filters.Cache(alphapose_bbox,300)
alphapose_bbox_neighbor = message_filters.Subscriber(self.alphapose_bbox_neighbor_topic, AlphaRes)
self.alphapose_bbox_neighbor_cache = message_filters.Cache(alphapose_bbox_neighbor,300)
mhmr = message_filters.Subscriber(self.mhmr_topic, PoseArray)
self.mhmr_cache = message_filters.Cache(mhmr,300)
rospy.Subscriber(self.alphapose_detection, Int16, self._alphapose_detection_callback)
rospy.Subscriber(self.alphapose_neighbor_detection, Int16, self._alphapose_neighbor_detection_callback)
rospy.Subscriber(self.command_topic, uav_pose, self._command_callback)
rospy.Subscriber(self.camera_info, CameraInfo, self._camera_info_callback)
rospy.Subscriber(self.camera_info_neighbor, CameraInfo, self._camera_info_neighbor_callback)
rospy.Subscriber(self.joints_gt, PoseArray, self._joints_gt_callback)
rospy.Subscriber(self.mpc_command, uav_pose, self._mpc_command_callback)
self._dest_vel_pub = rospy.Publisher(self.destination_topic, PoseStamped, queue_size=1)
self._cmd_vel_pub = rospy.Publisher(self.command_topic, uav_pose, queue_size=1)
self._cmd_neighbor_vel_pub = rospy.Publisher(self.neighbor_command_topic, uav_pose, queue_size=1)
self._target_init_pub = rospy.Publisher(self.target_topic, PoseWithCovarianceStamped, queue_size=1)
self.create_circle(radius=8)
# self.gazebo.pauseSim()
outPose = uav_pose()
outPose.header.stamp = rospy.Time.now()
outPose.header.frame_id="world"
outPose.POI.x = 0
outPose.POI.y = 0
outPose.POI.z = 0
r = 8#
t = np.random.choice(63,1);
outPose.position.x = r*np.cos(self.theta[t[0]])
outPose.position.y = r*np.sin(self.theta[t[0]])
outPose.position.z = -r
self._cmd_vel_pub.publish(outPose)
rospy.logwarn("Finished FireflyEnv INIT...")
self._check_all_sensors_ready()
rospy.logwarn("SENSORS OK!.")
self._check_publishers_connection()
rospy.logwarn("PUBLISHERS OK!.")
def move_base(self, action, horizon, update_rate=100, init=False):
"""
It will move the base based on the linear and angular speeds given.
It will wait untill those twists are achived reading from the odometry topic.
:param action: u_x, u_y, u_z
:param horizon: Prediction horizon for MAV
:param update_rate: Rate at which we check the odometry.
:return:
"""
import tf
# outPose.velocity.x = self.odom.velocity.x
# outPose.velocity.y = self.odom.velocity.y
# outPose.velocity.z = self.odom.velocity.z
#use LTI dynamics with A = I with acceleration control input
# for k in range(horizon):
# outPose.velocity.x += action[0]/update_rate**2
# outPose.position.x += outPose.velocity.x/update_rate
# outPose.velocity.y += action[1]/update_rate**2
# outPose.position.y += outPose.velocity.y/update_rate
# outPose.velocity.z += (-1)*action[2]/update_rate**2
# outPose.position.z += outPose.velocity.z/update_rate
#use LTI dynamics with A = I with velocity control input
outPose = uav_pose()
outPose.header.stamp = rospy.Time.now()
outPose.header.frame_id = "world"
if init:
outPose.POI.x = 0
outPose.POI.y = 0
outPose.POI.z = 0
r = 6
t = np.random.choice(5, 1)
# outPose.position.x = r*np.cos(self.theta[t[0]])
# outPose.position.y = r*np.sin(self.theta[t[0]])
# outPose.position.z = -r
# if self.robotID==2:
# outPose.position.x = -6#r*np.cos(self.theta[t[0]])
# outPose.position.y = 0#r*np.sin(self.theta[t[0]])
# outPose.position.z = -6#-r
# else:
outPose.position.x = r * np.cos(self.theta[t[0]])
outPose.position.y = r * np.sin(self.theta[t[0]])
outPose.position.z = -r
else:
#initialize outpose with current position and velocity
# outPose.POI.x = self.target.pose.pose.position.x
# outPose.POI.y = self.target.pose.pose.position.y
# outPose.POI.z = self.target.pose.pose.position.z
outPose.POI.x = self.gt_target.pose.pose.position.x
outPose.POI.y = self.gt_target.pose.pose.position.y
outPose.POI.z = self.gt_target.pose.pose.position.z
# pointstamp = PointStamped()
# pointstamp.header.frame_id = 'firefly_1/base_link'
# pointstamp.header.stamp = rospy.Time(0)
# pointstamp.point.x = action[0]
# pointstamp.point.y = action[1]
# pointstamp.point.z = action[2]
# rospy.logwarn("END ACTION PRETRAIN WITH JOINTS==>"+str(self.act_pretrained_joints))
# outPose.velocity.x = action[0]
# outPose.velocity.y = action[1]
# outPose.velocity.z = action[2]
(trans, rot) = self.listener.lookupTransform(
'world_ENU', self.rotors_machine_name + '/base_link',
rospy.Time(0))
(r, p, y) = tf.transformations.euler_from_quaternion(rot)
homogeneous_matrix = tf.transformations.euler_matrix(0,
0,
y,
axes='sxyz')
homogeneous_matrix[0:3, 3] = trans
# print(homogeneous_matrix) #for debug
# if self.robotID==2:
# rospy.logwarn("END ACTION PRETRAIN==>"+str(self.act_pretrained))
# goal = homogeneous_matrix.dot(np.concatenate((np.array(self.act_pretrained),np.array([1]))))
# else:
# rospy.logwarn("END ACTION PRETRAIN==>"+str(self.act_pretrained))
rospy.logwarn("END ACTION ==>" + str(action))
# Using pretrained single agent follow n/w
# goal = homogeneous_matrix.dot(np.concatenate((np.array(action)+np.array(self.act_pretrained),np.array([1]))))
gt_odom = self.get_gt_odom()
# Using MPC
# action_MPC = np.array([self.mpc_command_data.position.x-gt_odom.position.x,self.mpc_command_data.position.y-gt_odom.position.y, self.mpc_command_data.position.z-gt_odom.position.z])
# goal_MPC = np.array([self.mpc_command_data.position.x,self.mpc_command_data.position.y, self.mpc_command_data.position.z, 1])
# action_MPC = np.dot(np.linalg.inv(homogeneous_matrix),goal_MPC)
# if np.isnan(action_MPC).any():
# firefly_odom = self.get_firefly_odom()
# action_MPC=np.array([gt_odom.position.x,gt_odom.position.y,gt_odom.position.z])
if self.robotID == 1:
goal = homogeneous_matrix.dot(
np.concatenate((np.array(action), np.array([1]))))
# goal = homogeneous_matrix.dot(np.concatenate((np.array(action)+np.array([action_MPC[0],action_MPC[1],action_MPC[2]]),np.array([1]))))
# goal = homogeneous_matrix.dot(np.concatenate((action_MPC[0:3],np.array([1]))))
# rospy.logwarn("END ACTION MPC==>"+str(action_MPC))
rospy.logwarn("END ACTION==>" + str(action))
# Using pretrained single agent follow n/w
# outPose.position.x = goal[0]
# outPose.position.y = goal[1]
# outPose.position.z = goal[2]
# Using MPC
outPose.position.x = goal[0]
outPose.position.y = goal[1]
outPose.position.z = goal[2]
# rospy.logwarn("END Transformed Goal ==>x:"+str(act.point.x)+", y:"+str(act.point.y)+", z:"+str(act.point.z))
# rospy.logwarn("END Transformed Goal ==>x:"+str(outPose.position.x)+", y:"+str(outPose.position.y)+", z:"+str(outPose.position.z))
rospy.logwarn("Current MAV Pose ==>x:" + str(gt_odom.position.x) +
", y:" + str(gt_odom.position.y) + ", z:" +
str(gt_odom.position.z))
# rospy.logwarn("Current MAV Control ==>x:"+str(outPose.position.x - gt_odom.position.x)+", y:"+str(outPose.position.y - gt_odom.position.y)+", z:"+str(outPose.position.z - gt_odom.position.z))
# rospy.logwarn("Current Firefly Pose ==>x:"+str(firefly_odom.pose.pose.position.x)+", y:"+str(firefly_odom.pose.pose.position.y)+", z:"+str(firefly_odom.pose.pose.position.z))
# pointstamp = PointStamped()
# pointstamp.header.frame_id = 'world_ENU'
# pointstamp.header.stamp = rospy.Time(0)
# pointstamp.point.x = act.point.x
# pointstamp.point.y = act.point.y
# pointstamp.point.z = -act.point.z
# act = self.listener.transformPoint('firefly_1/base_link',pointstamp)
# rospy.logwarn("Goal Local ==>x:"+str(act.point.x)+", y:"+str(act.point.y)+", z:"+str(-act.point.z))
rospy.logdebug("Firefly Command>>" + str(outPose))
self._check_publishers_connection()
#publish desired position and velocity
self._cmd_vel_pub.publish(outPose)
rate = rospy.Rate(update_rate) # 10hz
try:
rate.sleep()
except:
pass
#time.sleep(0.02)
"""
def move_base(self, action, horizon, update_rate=100, init= False):
import tf
outPose = uav_pose()
outPose.header.stamp = rospy.Time.now()
outPose.header.frame_id="world"
if init:
outPose.POI.x = 0
outPose.POI.y = 0
outPose.POI.z = 0
r = 8
t = np.random.choice(63,1);
outPose.position.x = r*np.cos(self.theta[t[0]])
outPose.position.y = r*np.sin(self.theta[t[0]])
outPose.position.z = -r
else:
(trans,rot) = self.listener.lookupTransform( 'world_ENU',self.rotors_machine_name+'/base_link', rospy.Time(0))
(r,p,y) = tf.transformations.euler_from_quaternion(rot)
homogeneous_matrix = tf.transformations.euler_matrix(0,0,y,axes='sxyz')
homogeneous_matrix[0:3,3] = trans
yaw = y + action[2]
outPose.POI.x = trans[0]+2*np.cos(yaw)
outPose.POI.y = trans[1]+2*np.sin(yaw)
outPose.POI.z = 1
rospy.logwarn("END ACTION ==>"+str(action))
gt_odom = self.get_gt_odom()
# Using MPC
# goal_MPC = np.array([self.mpc_command_data.position.x,self.mpc_command_data.position.y, self.mpc_command_data.position.z, 1])
# action_MPC = np.dot(np.linalg.inv(homogeneous_matrix),goal_MPC)
# if np.isnan(action_MPC).any():
# firefly_odom = self.get_firefly_odom()
# action_MPC=np.array([gt_odom.position.x,gt_odom.position.y,gt_odom.position.z])
act = []
act.extend([action[0],action[1]])
act.extend([0])
if self.robotID==1:
goal = homogeneous_matrix.dot(np.concatenate((np.array(act),np.array([1]))))
# goal = homogeneous_matrix.dot(np.concatenate((np.array(action)+np.array([action_MPC[0],action_MPC[1],action_MPC[2]]),np.array([1]))))
# goal = homogeneous_matrix.dot(np.concatenate((action_MPC[0:3],np.array([1]))))
# rospy.logwarn("END ACTION MPC==>"+str(action_MPC))
rospy.logwarn("END ACTION==>"+str(action))
outPose.position.x = goal[0]
outPose.position.y = goal[1]
outPose.position.z = -8
rospy.logwarn("Current MAV Pose ==>x:"+str(gt_odom.position.x)+", y:"+str(gt_odom.position.y)+", z:"+str(gt_odom.position.z))
rospy.logdebug("Firefly Command>>" + str(outPose))
self._check_publishers_connection()
#publish desired position and velocity
self._cmd_vel_pub.publish(outPose)
rate = rospy.Rate(update_rate) # 10hz
try:
rate.sleep()
except:
pass
def move_base(self, action, horizon, update_rate=100, init=False):
import tf
#action=[0,1,0]
scale = 1
action[0:2] = scale * action[0:2]
outPose = uav_pose()
destPose = PoseStamped()
tello_command = Twist()
outPose.header.stamp = rospy.Time.now()
outPose.header.frame_id = "world"
destPose.header.stamp = rospy.Time.now()
destPose.header.frame_id = "world"
if init:
outPose.POI.x = 0
outPose.POI.y = 0
outPose.POI.z = 0
r = 8
t = np.random.choice(63, 1)
outPose.position.x = r * np.cos(self.theta[t[0]])
outPose.position.y = r * np.sin(self.theta[t[0]])
outPose.position.z = -r
else:
rot = np.array([
self.gt_odom.orientation.x, self.gt_odom.orientation.y,
self.gt_odom.orientation.z, self.gt_odom.orientation.w
])
trans = np.array([
self.gt_odom.position.y, self.gt_odom.position.x,
-self.gt_odom.position.z
])
(r, p, y) = tf.transformations.euler_from_quaternion(rot)
homogeneous_matrix = tf.transformations.euler_matrix(0,
0,
y,
axes='sxyz')
homogeneous_matrix[0:3, 3] = trans
#yaw = y + action[2]
#outPose.POI.x = trans[0]+2*np.cos(yaw)
#outPose.POI.y = trans[1]+2*np.sin(yaw)
#outPose.POI.z = 1
rospy.logwarn("END ACTION ==>" + str(action))
# Using MPC
# goal_MPC = np.array([self.mpc_command_data.position.x,self.mpc_command_data.position.y, self.mpc_command_data.position.z, 1])
# action_MPC = np.dot(np.linalg.inv(homogeneous_matrix),goal_MPC)
# if np.isnan(action_MPC).any():
# firefly_odom = self.get_firefly_odom()
# action_MPC=np.array([gt_odom.position.x,gt_odom.position.y,gt_odom.position.z])
act = []
act.extend([action[1], action[0]])
act.extend([0])
scale = 1
act = scale * np.array(act)
goal = homogeneous_matrix.dot(
np.concatenate((np.array(act), np.array([1]))))
# goal = homogeneous_matrix.dot(np.concatenate((np.array(action)+np.array([action_MPC[0],action_MPC[1],action_MPC[2]]),np.array([1]))))
# goal = homogeneous_matrix.dot(np.concatenate((action_MPC[0:3],np.array([1]))))
# rospy.logwarn("END ACTION MPC==>"+str(action_MPC))
rospy.logwarn("END ACTION==>" + str(action))
outPose.position.x = goal[1]
outPose.position.y = goal[0]
outPose.position.z = -1.5
outPose.velocity.x = action[1]
outPose.velocity.y = action[0]
#outPose.angular.z = action[2]
destPose.pose.position.x = outPose.position.x
destPose.pose.position.y = outPose.position.y
destPose.pose.position.z = outPose.position.z
print(destPose)
gt_odom = self.get_gt_odom()
vz = self.pid(gt_odom.position.z, -1.5)
orientation = np.array([
self.gt_odom.orientation.x, self.gt_odom.orientation.y,
self.gt_odom.orientation.z, self.gt_odom.orientation.w
])
(r, p, y) = tf.transformations.euler_from_quaternion(orientation)
target = self.get_gt_target()
desired_angle = np.arctan2(
self.gt_odom.position.y - target.pose.pose.position.y,
self.gt_odom.position.x - target.pose.pose.position.x)
yawrate = self.pid_yawrate(y, desired_angle)
action_norm = np.linalg.norm(np.array([action[0], action[1], vz]))
tello_command.linear.x = action[1] / action_norm
tello_command.linear.y = action[0] / action_norm
tello_command.linear.z = vz / action_norm
tello_command.angular.z = yawrate #action[2]
rospy.logwarn("Current MAV Pose ==>x:" + str(gt_odom.position.x) +
", y:" + str(gt_odom.position.y) + ", z:" +
str(gt_odom.position.z))
#rospy.loginfo("Firefly Command>>" + str(outPose))
#self._check_publishers_connection()
#publish desired position and velocity
try:
#self._cmd_vel_pub.publish(outPose)
self._dest_vel_pub.publish(destPose)
except:
print('No TF to publish')
if self.get_gt_odom() is None:
tello_command.linear.x = 0
tello_command.linear.y = 0
tello_command.linear.z = 0
tello_command.angular.z = 0
#self._tello_cmd_pub.publish(tello_command)
#self._tello_cmd_pub_dup.publish(tello_command)
else:
#self._tello_cmd_pub.publish(tello_command)
#self._tello_cmd_pub_dup.publish(tello_command)
pass
rate = rospy.Rate(update_rate)
try:
rate.sleep()
except:
pass
