def pose_callback(self, pose_msg):
'''
Uses a pose message to generate an odometry and state message.
:param pose_msg: (geometry_msgs/PoseStamped) pose message
'''
new_pose_msg, trans, rot = self.tf_to_pose("LO01_base_link", "map",
pose_msg.header)
if not self.use_amcl:
pose_msg = new_pose_msg
self.new_pose_pub.publish(
pose_msg
) # if using AMCL, this will be the same as the original pose message
if trans and rot: # if not getting tf, trans and rot will be None
footprint = PolygonStamped()
footprint.header = pose_msg.header # has same frame_id (map) and time stamp
loomo_points = np.array([[0.16, -0.31], [0.16, 0.31],
[-0.16, 0.31], [-0.16, -0.31]])
roll, pitch, yaw = tf.transformations.euler_from_quaternion(rot)
rot = np.array([[np.cos(yaw), -np.sin(yaw)],
[np.sin(yaw), np.cos(yaw)]])
rotated_points = np.matmul(rot, loomo_points.T) # 2x4 array
rot_and_trans = rotated_points + np.array([[trans[0]], [trans[1]]])
polygon = Polygon(
points=[Point32(x=x, y=y, z=0) for x, y in rot_and_trans.T])
footprint.polygon = polygon
self.footprint_pub.publish(footprint)
odom_msg = Odometry()
odom_msg.pose.pose = pose_msg.pose
odom_msg.header = pose_msg.header
self.odom_pub.publish(odom_msg)
state_msg = State()
state_msg.header = pose_msg.header
state_msg.state_stamp = pose_msg.header.stamp
state_msg.pos.x = pose_msg.pose.position.x
state_msg.pos.y = pose_msg.pose.position.y
state_msg.pos.z = pose_msg.pose.position.z
state_msg.quat.x = pose_msg.pose.orientation.x
state_msg.quat.y = pose_msg.pose.orientation.y
state_msg.quat.z = pose_msg.pose.orientation.z
state_msg.quat.w = pose_msg.pose.orientation.w
if self.last_state_time and self.last_state:
dt = pose_msg.header.stamp.nsecs - self.last_state_time
state_msg.vel.x = (state_msg.pos.x -
self.last_state.pos.x) / (float(dt) / 10**9)
state_msg.vel.y = (state_msg.pos.y -
self.last_state.pos.y) / (float(dt) / 10**9)
state_msg.vel.z = 0 # ground robot not traveling in z direction
self.last_state_time = pose_msg.header.stamp.nsecs
self.last_state = state_msg
self.state_pub.publish(state_msg)
def __init__(self):
self.state=State()
self.state.pos.x = rospy.get_param('~x', 0.0);
self.state.pos.y = rospy.get_param('~y', 0.0);
self.state.pos.z = rospy.get_param('~z', 0.0);
yaw = rospy.get_param('~yaw', 0.0);
pitch=0.0;
roll=0.0;
quat = quaternion_from_euler(yaw, pitch, roll, 'rzyx')
self.state.quat.x = quat[0]
self.state.quat.y = quat[1]
self.state.quat.z = quat[2]
self.state.quat.w = quat[3]
self.pubState = rospy.Publisher('state', State, queue_size=1, latch=True)
self.timer = rospy.Timer(rospy.Duration(0.01), self.pubTF)
name = rospy.get_namespace()
self.name = name[1:-1]
rospy.sleep(1.0)
self.state.header.frame_id="world"
self.pubState.publish(self.state)
pose=Pose()
pose.position.x=self.state.pos.x;
pose.position.y=self.state.pos.y;
pose.position.z=self.state.pos.z;
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
def __init__(self):
self.state = State()
self.state.pos.x = rospy.get_param('~x', 0.0)
self.state.pos.y = rospy.get_param('~y', 0.0)
self.state.pos.z = rospy.get_param('~z', 0.0)
yaw = rospy.get_param('~yaw', 0.0)
pitch = 0.0
roll = 0.0
quat = quaternion_from_euler(yaw, pitch, roll, 'szyx')
self.state.quat.x = quat[0]
self.state.quat.y = quat[1]
self.state.quat.z = quat[2]
self.state.quat.w = quat[3]
self.pubGazeboState = rospy.Publisher('/gazebo/set_model_state',
ModelState,
queue_size=1)
self.pubState = rospy.Publisher('state',
State,
queue_size=1,
latch=True)
self.timer = rospy.Timer(rospy.Duration(0.01), self.pubTF)
name = rospy.get_namespace()
self.name = name[1:-1]
rospy.sleep(1.0)
self.state.header.frame_id = "world"
self.pubState.publish(self.state)
def __init__(self):
self.state=State()
self.state.pos.x = rospy.get_param('~x', 0.0);
self.state.pos.y = rospy.get_param('~y', 0.0);
self.state.pos.z = rospy.get_param('~z', 0.0);
yaw = rospy.get_param('~yaw', 0.0);
self.publish_marker_drone=False; #TODO: this mesh is not scaled by the radius of the UAV of panther.yaml
pitch=0.0;
roll=0.0;
quat = quaternion_from_euler(yaw, pitch, roll, 'rzyx')
self.state.quat.x = quat[0]
self.state.quat.y = quat[1]
self.state.quat.z = quat[2]
self.state.quat.w = quat[3]
self.pubState = rospy.Publisher('state', State, queue_size=1, latch=True)
self.pubMarkerDrone = rospy.Publisher('drone_marker', Marker, queue_size=1, latch=True)
self.timer = rospy.Timer(rospy.Duration(0.01), self.pubTF)
name = rospy.get_namespace()
self.name = name[1:-1]
rospy.sleep(1.0)
self.state.header.frame_id="world"
self.pubState.publish(self.state)
if(self.publish_marker_drone):
self.pubMarkerDrone.publish(self.getDroneMarker());
def publishMavstate(self):
msg = State()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'map'
msg.state_stamp = rospy.Time.now()
msg.pos.x = self.mavpos_[0]
msg.pos.y = self.mavpos_[1]
msg.pos.z = self.mavpos_[2]
msg.quat.x = self.mavrot_[0]
msg.quat.y = self.mavrot_[1]
msg.quat.z = self.mavrot_[2]
msg.quat.w = self.mavrot_[3]
msg.vel.x = self.mav_linear_vel_[0]
msg.vel.y = self.mav_linear_vel_[1]
msg.vel.z = self.mav_linear_vel_[2]
msg.w.x = self.mav_angular_vel_[0]
msg.w.y = self.mav_angular_vel_[1]
msg.w.z = self.mav_angular_vel_[2]
self.mavstatePub_.publish(msg)
def __init__(self):
self.state = State()
self.state.pos.x = rospy.get_param('~x', 0.0)
self.state.pos.y = rospy.get_param('~y', 0.0)
self.state.pos.z = rospy.get_param('~z', 0.0)
yaw = rospy.get_param('~yaw', 0.0)
pitch = 0.0
roll = 0.0
quat = quaternion_from_euler(yaw, pitch, roll, 'rzyx')
self.state.quat.x = quat[0]
self.state.quat.y = quat[1]
self.state.quat.z = quat[2]
self.state.quat.w = quat[3]
self.pubGazeboState = rospy.Publisher('/gazebo/set_model_state',
ModelState,
queue_size=1)
self.pubMarkerDrone = rospy.Publisher('marker',
Marker,
queue_size=1,
latch=True)
self.pubState = rospy.Publisher('state',
State,
queue_size=1,
latch=True)
self.timer = rospy.Timer(rospy.Duration(0.01), self.pubTF)
name = rospy.get_namespace()
self.name = name[1:-1]
rospy.sleep(1.0)
self.state.header.frame_id = "world"
self.pubState.publish(self.state)
self.initialized = False
pose = Pose()
pose.position.x = self.state.pos.x
pose.position.y = self.state.pos.y
pose.position.z = self.state.pos.z
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
self.pubMarkerDrone.publish(self.getDroneMarker(pose))
def __init__(self):
self.state = State()
self.state.pos.x = rospy.get_param('~x', 0.0)
self.state.pos.y = rospy.get_param('~y', 0.0)
self.state.pos.z = rospy.get_param('~z', 0.0)
self.state.quat.x = 0
self.state.quat.y = 0
self.state.quat.z = 0
self.state.quat.w = 1
self.current_yaw = 0.0
#Publishers
self.pubCmdVel = rospy.Publisher('jackal_velocity_controller/cmd_vel',
Twist,
queue_size=1,
latch=True)
self.pubState = rospy.Publisher('state',
State,
queue_size=1,
latch=False)
#Timers
self.timer = rospy.Timer(rospy.Duration(0.1), self.cmdVelCB)
self.kv = 1.0
self.kdist = 2.5 #0.8
self.kw = 1.0
self.kyaw = 2.0
self.kalpha = 1.5
self.state_initialized = False
self.goal_initialized = False
self.goal = Goal()
self.goal.p.x = 0.0
self.goal.p.y = 0.0
self.goal.p.z = 0.0
self.goal.v.x = 0.0
self.goal.v.y = 0.0
self.goal.v.z = 0.0
self.goal.a.x = 0.0
self.goal.a.y = 0.0
self.goal.a.z = 0.0
self.state_initialized = False
def goalCB(self, data):
state = State()
axis_z=[0,0,1]
#Hopf fibration approach
thrust=np.array([data.a.x, data.a.y, data.a.z + 9.81]);
thrust_normalized=thrust/np.linalg.norm(thrust);
a=thrust_normalized[0];
b=thrust_normalized[1];
c=thrust_normalized[2];
qabc = random_quaternion();
tmp=(1/math.sqrt(2*(1+c)));
#From http://docs.ros.org/en/jade/api/tf/html/python/transformations.html, the documentation says
#"Quaternions ix+jy+kz+w are represented as [x, y, z, w]."
qabc[3] = tmp*(1+c) #w
qabc[0] = tmp*(-b) #x
qabc[1] = tmp*(a) #y
qabc[2] = 0 #z
qpsi = random_quaternion();
qpsi[3] = math.cos(data.psi/2.0); #w
qpsi[0] = 0; #x
qpsi[1] = 0; #y
qpsi[2] = math.sin(data.psi/2.0); #z
w_q_b=quaternion_multiply(qabc,qpsi)
# accel=[data.a.x, data.a.y, data.a.z + 9.81];
self.state.header.frame_id="world"
self.state.pos=data.p
self.state.vel=data.v
self.state.quat.w=w_q_b[3] #w
self.state.quat.x=w_q_b[0] #x
self.state.quat.y=w_q_b[1] #y
self.state.quat.z=w_q_b[2] #z
self.pubState.publish(self.state)
if(self.publish_marker_drone):
self.pubMarkerDrone.publish(self.getDroneMarker());
def goalCB(self, data):
state = State()
axis_z = [0, 0, 1]
accel = [data.a.x, data.a.y, data.a.z + 9.81]
drone_quaternion_with_yaw = []
if (LA.norm(accel) > 1e-6
and LA.norm(np.cross(accel, axis_z)) > 1e-6): #TODO
norm_accel = LA.norm(accel)
accel = accel / norm_accel
axis = np.cross(accel, axis_z)
dot = np.dot(accel, axis_z)
angle = math.acos(dot)
drone_quaternion = quaternion_about_axis(
-angle, axis) # Quaternion(axis=axis, angle=-angle)
#Use the yaw from goal
drone_quaternion_with_yaw = quaternion_multiply(
drone_quaternion,
quaternion_from_euler(data.yaw, 0.0, 0.0, 'rzyx'))
else: #Take only the yaw angle
drone_quaternion_with_yaw = quaternion_from_euler(
data.yaw, 0.0, 0.0, 'rzyx')
self.state.header.frame_id = "world"
self.state.pos = data.p
self.state.vel = data.v
self.state.quat.x = drone_quaternion_with_yaw[0]
self.state.quat.y = drone_quaternion_with_yaw[1]
self.state.quat.z = drone_quaternion_with_yaw[2]
self.state.quat.w = drone_quaternion_with_yaw[3]
self.pubState.publish(self.state)
def __init__(self, total_num_obs):
self.state = State()
name = rospy.get_namespace()
self.name = name[1:-1]
#self.num_of_objects = 0;
self.world = MovingForest(total_num_obs)
available_meshes_static = [
"package://mader/meshes/ConcreteDamage01b/model3.dae",
"package://mader/meshes/ConcreteDamage01b/model2.dae"
]
available_meshes_dynamic = [
"package://mader/meshes/ConcreteDamage01b/model4.dae"
]
# available_meshes=["package://mader/meshes/ConcreteDamage01b/model3.dae"]
self.x_all = []
self.y_all = []
self.z_all = []
self.offset_all = []
self.slower = []
self.meshes = []
self.type = []
#"dynamic" or "static"
self.bboxes = []
for i in range(self.world.num_of_dyn_objects):
self.x_all.append(
random.uniform(self.world.x_min, self.world.x_max))
self.y_all.append(
random.uniform(self.world.y_min, self.world.y_max))
self.z_all.append(
random.uniform(self.world.z_min, self.world.z_max))
self.offset_all.append(random.uniform(-2 * math.pi, 2 * math.pi))
self.slower.append(
random.uniform(self.world.slower_min, self.world.slower_max))
self.type.append("dynamic")
self.meshes.append(random.choice(available_meshes_dynamic))
self.bboxes.append(self.world.bbox_dynamic)
for i in range(self.world.num_of_stat_objects):
bbox_i = []
if (i < self.world.percentage_vert *
self.world.num_of_stat_objects):
bbox_i = self.world.bbox_static_vert
self.z_all.append(bbox_i[2] / 2.0)
#random.uniform(self.world.z_min, self.world.z_max) for sphere sim
self.type.append("static_vert")
self.meshes.append(random.choice(available_meshes_static))
else:
bbox_i = self.world.bbox_static_horiz
self.z_all.append(random.uniform(0.0, 3.0))
#-3.0, 3.0 for sphere sim
self.type.append(
"static_horiz"
) #They are actually dynamic (moving in z) //TODO (change name)
self.meshes.append(random.choice(available_meshes_dynamic))
self.x_all.append(
random.uniform(self.world.x_min - self.world.scale,
self.world.x_max + self.world.scale))
self.y_all.append(
random.uniform(self.world.y_min - self.world.scale,
self.world.y_max + self.world.scale))
self.offset_all.append(random.uniform(-2 * math.pi, 2 * math.pi))
self.slower.append(
random.uniform(self.world.slower_min, self.world.slower_max))
# self.type.append("static")
self.bboxes.append(bbox_i)
self.pubTraj = rospy.Publisher('/trajs',
DynTraj,
queue_size=1,
latch=True)
self.pubShapes_static = rospy.Publisher('/shapes_static',
Marker,
queue_size=1,
latch=True)
self.pubShapes_static_mesh = rospy.Publisher('/shapes_static_mesh',
MarkerArray,
queue_size=1,
latch=True)
self.pubShapes_dynamic_mesh = rospy.Publisher('/shapes_dynamic_mesh',
MarkerArray,
queue_size=1,
latch=True)
self.pubShapes_dynamic = rospy.Publisher('/shapes_dynamic',
Marker,
queue_size=1,
latch=True)
self.pubGazeboState = rospy.Publisher('/gazebo/set_model_state',
ModelState,
queue_size=1)
self.already_published_static_shapes = False
rospy.sleep(0.5)
def __init__(self, total_num_obs):
self.state=State()
name = rospy.get_namespace()
self.name = name[1:-1]
#self.num_of_objects = 0;
world_type="MovingCorridor"
if(world_type=="MovingCorridor"):
self.world=MovingCorridor(total_num_obs)
if(world_type=="MovingCircle"):
self.world=MovingCircle()
available_meshes_static=["package://mader/meshes/ConcreteDamage01b/model3.dae", "package://mader/meshes/ConcreteDamage01b/model2.dae"]
available_meshes_dynamic=["package://mader/meshes/ConcreteDamage01b/model4.dae"]
# available_meshes=["package://mader/meshes/ConcreteDamage01b/model3.dae"]
self.x_all=[];
self.y_all=[];
self.z_all=[];
self.offset_all=[];
self.slower=[];
self.meshes=[];
self.type=[];#"dynamic" or "static"
self.bboxes=[];
for i in range(self.world.num_of_dyn_objects):
self.x_all.append(random.uniform(self.world.x_min, self.world.x_max));
self.y_all.append(random.uniform(self.world.y_min, self.world.y_max));
self.z_all.append(random.uniform(self.world.z_min, self.world.z_max));
self.offset_all.append(random.uniform(-2*math.pi, 2*math.pi));
self.slower.append(random.uniform(self.world.slower_min, self.world.slower_max));
self.type.append("dynamic")
self.meshes.append(random.choice(available_meshes_dynamic));
self.bboxes.append(self.world.bbox_dynamic);
for i in range(self.world.num_of_stat_objects):
bbox_i=[];
if(i<self.world.percentage_vert*self.world.num_of_stat_objects):
bbox_i=self.world.bbox_static_vert;
self.z_all.append(bbox_i[2]/2.0);
self.type.append("static_vert")
else:
bbox_i=self.world.bbox_static_horiz;
self.z_all.append(random.uniform(0.0, 3.0));
self.type.append("static_horiz")
self.x_all.append(random.uniform(self.world.x_min-self.world.scale, self.world.x_max+self.world.scale));
self.y_all.append(random.uniform(self.world.y_min-self.world.scale, self.world.y_max+self.world.scale));
self.offset_all.append(random.uniform(-2*math.pi, 2*math.pi));
self.slower.append(random.uniform(self.world.slower_min, self.world.slower_max));
# self.type.append("static")
self.meshes.append(random.choice(available_meshes_static));
self.bboxes.append(bbox_i)
self.pubTraj = rospy.Publisher('/trajs', DynTraj, queue_size=self.world.total_num_obs)#If queue_size=1, pubTraj will not be able to keep up (due to the loop in pubTF)#, latch=True
self.pubShapes_static = rospy.Publisher('/shapes_static', Marker, queue_size=1, latch=True)
self.pubShapes_static_mesh = rospy.Publisher('/shapes_static_mesh', MarkerArray, queue_size=1, latch=True)
self.pubShapes_dynamic_mesh = rospy.Publisher('/shapes_dynamic_mesh', MarkerArray, queue_size=1, latch=True)
self.pubShapes_dynamic = rospy.Publisher('/shapes_dynamic', Marker, queue_size=1, latch=True)
self.pubGazeboState = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=1)
self.already_published_static_shapes=False;
rospy.sleep(0.5)
def __init__(self, total_num_obs,gazebo):
self.state=State()
name = rospy.get_namespace()
self.name = name[1:-1]
print(total_num_obs)
self.num_of_dyn_objects=int(0.65*total_num_obs);
self.num_of_stat_objects=total_num_obs-self.num_of_dyn_objects;
self.x_min= 2.0
self.x_max= 75.0
self.y_min= -3.0
self.y_max= 3.0
self.z_min= 1.0
self.z_max= 2.0
self.scale=1.0;
self.slower_min=1.1
self.slower_max= 1.1
self.bbox_dynamic=[0.8, 0.8, 0.8]
self.bbox_static_vert=[0.4, 0.4, 4]
self.bbox_static_horiz=[0.4, 8, 0.4]
self.percentage_vert=0.0;
self.name_obs="obs_"
#HACK
self.num_of_dyn_objects=2;
self.num_of_stat_objects=0;
self.x_min= 2.0
self.x_max= 3.0
self.y_min= -2.0
self.y_max= 2.0
#END OF HACK
self.available_meshes_static=["package://panther/meshes/ConcreteDamage01b/model3.dae", "package://panther/meshes/ConcreteDamage01b/model2.dae"]
self.available_meshes_dynamic=["package://panther/meshes/ConcreteDamage01b/model4.dae"]
self.marker_array=MarkerArray();
self.all_dyn_traj=[]
self.total_num_obs=self.num_of_dyn_objects + self.num_of_stat_objects
for i in range(self.total_num_obs):
[traj_x, traj_y, traj_z, x, y, z, mesh, bbox]=self.getTrajectoryPosMeshBBox(i);
self.marker_array.markers.append(self.generateMarker(mesh, bbox, i));
dynamic_trajectory_msg=DynTraj();
dynamic_trajectory_msg.use_pwp_field=False;
dynamic_trajectory_msg.is_agent=False;
dynamic_trajectory_msg.header.stamp= rospy.Time.now();
dynamic_trajectory_msg.s_mean = [traj_x, traj_y, traj_z]
dynamic_trajectory_msg.s_var = ["0.0", "0.0", "0.0"]
dynamic_trajectory_msg.bbox = [bbox[0], bbox[1], bbox[2]];
dynamic_trajectory_msg.pos.x=x #Current position, will be updated later
dynamic_trajectory_msg.pos.y=y #Current position, will be updated later
dynamic_trajectory_msg.pos.z=z #Current position, will be updated later
dynamic_trajectory_msg.id = 4000+ i #Current id 4000 to avoid interference with ids from agents #TODO
self.all_dyn_traj.append(dynamic_trajectory_msg);
self.pubTraj = rospy.Publisher('/trajs', DynTraj, queue_size=1, latch=True)
self.pubShapes_dynamic_mesh = rospy.Publisher('/obstacles_mesh', MarkerArray, queue_size=1, latch=True)
#self.pubGazeboState = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=100)
if(gazebo):
# Spawn all the objects in Gazebo
for i in range(self.total_num_obs):
self.spawnGazeboObstacle(i)
rospy.sleep(0.5)
def goalCB(self, data):
state = State()
gazebo_state = ModelState()
gazebo_state.model_name = self.name
axis_z = [0, 0, 1]
accel = [data.accel.x, data.accel.y, data.accel.z + 9.81]
gazebo_state.pose.position.x = data.pos.x
gazebo_state.pose.position.y = data.pos.y
gazebo_state.pose.position.z = data.pos.z
drone_quaternion_with_yaw = []
if (LA.norm(accel) > 0.000001
and LA.norm(np.cross(accel, axis_z)) > 0.0000001):
norm_accel = LA.norm(accel)
accel = accel / norm_accel
axis = np.cross(accel, axis_z)
dot = np.dot(accel, axis_z)
angle = math.acos(dot)
drone_quaternion = quaternion_about_axis(
-angle, axis) # Quaternion(axis=axis, angle=-angle)
#Use the yaw from goal
drone_quaternion_with_yaw = quaternion_multiply(
drone_quaternion,
quaternion_from_euler(data.yaw, 0.0, 0.0, 'rzyx'))
else: #Take only the yaw angle
drone_quaternion_with_yaw = quaternion_from_euler(
data.yaw, 0.0, 0.0, 'rzyx')
gazebo_state.pose.orientation.x = drone_quaternion_with_yaw[0]
gazebo_state.pose.orientation.y = drone_quaternion_with_yaw[1]
gazebo_state.pose.orientation.z = drone_quaternion_with_yaw[2]
gazebo_state.pose.orientation.w = drone_quaternion_with_yaw[3]
#self.gazebo_state.twist = data.twist
## HACK TO NOT USE GAZEBO
gazebo_state.reference_frame = "world"
self.pubGazeboState.publish(gazebo_state)
## END OF HACK TO NOT USE GAZEBO
self.state.header.frame_id = "world"
self.state.pos = data.pos
self.state.vel = data.vel
self.state.quat = gazebo_state.pose.orientation
self.pubState.publish(self.state)
print("I'm here: ", data.pos)
print(data.vel)
if not self.initialized:
self.file_name = '/home/derek/Desktop/state_' + str(
datetime.now()) + ".csv"
with open(self.file_name, mode='a+') as data_file:
data_writer = csv.writer(data_file, delimiter=',')
data_writer.writerow(['x', 'y', 'z', 'vx', 'vy', 'vz'])
self.initialized = True
with open(self.file_name, mode='a+') as data_file:
data_writer = csv.writer(data_file, delimiter=',')
data_writer.writerow([
data.pos.x, data.pos.y, data.pos.z, data.vel.x, data.vel.y,
data.vel.z
])
# print("State after:")
# print(self.state.quat)
self.pubMarkerDrone.publish(self.getDroneMarker(gazebo_state.pose))
def goalCB(self, data):
# print(" ")
# print("==================================")
# print("Goal received:")
# print(data.yaw)
# print("State before:")
# print(self.state.quat)
state = State()
gazebo_state = ModelState()
gazebo_state.model_name = self.name
axis_z = [0, 0, 1]
accel = [data.accel.x, data.accel.y, data.accel.z + 9.81]
gazebo_state.pose.position.x = data.pos.x
gazebo_state.pose.position.y = data.pos.y
gazebo_state.pose.position.z = data.pos.z
drone_quaternion_with_yaw = []
if (LA.norm(accel) > 0.001
and LA.norm(np.cross(accel, axis_z)) > 0.0001):
norm_accel = LA.norm(accel)
accel = accel / norm_accel
axis = np.cross(accel, axis_z)
dot = np.dot(accel, axis_z)
angle = math.acos(dot)
drone_quaternion = Quaternion(axis=axis, angle=-angle)
#Use the yaw from goal
euler = euler_from_quaternion(
(drone_quaternion[1], drone_quaternion[2], drone_quaternion[3],
drone_quaternion[0]), 'szyx')
yaw = euler[0]
pitch = euler[1]
roll = euler[2]
drone_quaternion_with_yaw = quaternion_from_euler(
data.yaw, pitch, roll, 'szyx')
else: #Take only the yaw angle
drone_quaternion_with_yaw = quaternion_from_euler(
data.yaw, 0, 0, 'szyx')
gazebo_state.pose.orientation.x = drone_quaternion_with_yaw[0]
gazebo_state.pose.orientation.y = drone_quaternion_with_yaw[1]
gazebo_state.pose.orientation.z = drone_quaternion_with_yaw[2]
gazebo_state.pose.orientation.w = drone_quaternion_with_yaw[3]
#self.gazebo_state.twist = data.twist
## HACK TO NOT USE GAZEBO
gazebo_state.reference_frame = "world"
self.pubGazeboState.publish(gazebo_state)
## END OF HACK TO NOT USE GAZEBO
self.state.header.frame_id = "world"
self.state.pos = data.pos
self.state.vel = data.vel
self.state.quat = gazebo_state.pose.orientation
self.pubState.publish(self.state)