def quaternion_product(q1, q2):
q_prod = Quaternion()
a1 = q1.w
b1 = q1.x
c1 = q1.y
d1 = q1.z
a2 = q2.w
b2 = q2.x
c2 = q2.y
d2 = q2.z
q_prod.w = a1 * a2 - b1 * b2 - c1 * c2 - d1 * d2
q_prod.x = a1 * b2 + b1 * a2 + c1 * d2 - d1 * c2
q_prod.y = a1 * c2 - b1 * d2 + c1 * a2 + d1 * b2
q_prod.z = a1 * d2 + b1 * c2 - c1 * b2 + d1 * a2
# This is exactly the same quaternion as before, but with positive
# real part (seems to be the standard for ROS TF)
if q_prod.w < 0.0:
q_prod.x = -q_prod.x
q_prod.y = -q_prod.y
q_prod.z = -q_prod.z
q_prod.w = -q_prod.w
return q_prod
def sense(self):
""" collects sensor data from the Neato and publishes
it to the neatoSensors topic
"""
# receive and publish sensor data
self.fields = self.neato.state.keys() # update sensor fields
sensor_data = NeatoSensors() # see NOTE above
for field in self.fields:
try:
sensor_data.__setattr__(field, self.neato.state[field])
except:
pass
self.sensorPub.publish(sensor_data)
# receive and publish laser range data
range_data = LaserRangeData()
range_data.__setattr__("range_data", self.neato.range_data)
self.rangePub.publish(range_data)
# odomemtry in testing
self.odomUpdate()
# transform position into tf frame
quaternionOdom = Quaternion()
quaternionOdom.x = 0.0
quaternionOdom.y = 0.0
quaternionOdom.z = sin(self.theta/2)
quaternionOdom.w = -cos(self.theta/2)
quaternionLL = Quaternion()
quaternionLL.x = 0.0
quaternionLL.y = 0.0
quaternionLL.z = 0.0
quaternionLL.w = 1.0
# base_link -> base_laser transformation
self.odomBroadcaster.sendTransform(
(0.0, -0.1, 0.0),
(quaternionLL.x, quaternionLL.y, quaternionLL.z, quaternionLL.w),
rospy.Time.now(),
"/base_laser",
"/base_link")
# odom -> base_link transformation
self.odomBroadcaster.sendTransform(
(self.x/1000, self.y/1000, 0),
(quaternionOdom.x, quaternionOdom.y, quaternionOdom.z, quaternionOdom.w),
rospy.Time.now(),
"/base_link",
"/odom")
def Publish(self, broadcaster, orientation, x_dist, y_dist, linear_speed, angular_speed, use_pose_ekf=False):
ros_now = rospy.Time.now()
quat = Quaternion()
# Orientation can be one of two things:
# Euler Angles or Quaternion
# Orientation is a dictionary keyed by 'euler' and 'quaternion'
if orientation.has_key('euler'):
euler = orientation['euler']
# Note: Euler values are in degrees
roll = euler['roll']
pitch = euler['pitch']
yaw = euler['yaw']
q = transformations.quaternion_from_euler(roll, pitch, yaw)
quat.x = q[0]
quat.y = q[1]
quat.z = q[2]
quat.w = q[3]
elif orientation.has_key('quaternion'):
quat.x = orientation['quaternion']['x']
quat.y = orientation['quaternion']['y']
quat.z = orientation['quaternion']['z']
quat.w = orientation['quaternion']['w']
# Publish the transform from frame odom to frame base_link over tf
# Note: pose ekf is how turtlebot does its thing. If there is an imu and/or gyro, it might be best to take
# this approach
if use_pose_ekf:
# This transform conflicts with transforms built into the Turtle stack
# http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28Python%29
# This is done in/with the robot_pose_ekf because it can integrate IMU/gyro data
# using an "extended Kalman filter"
# REMOVE this "line" if you use robot_pose_ekf
pass
else:
broadcaster.sendTransform(
(x_dist, y_dist, 0),
(quat.x, quat.y, quat.z, quat.w),
ros_now,
"base_footprint",
"odom"
)
self._publisher.publish(self._msg(quat, x_dist, y_dist, linear_speed, angular_speed, ros_now, use_pose_ekf))
def quaternion_to_msg(q):
msg = Quaternion()
msg.x = q[0]
msg.y = q[1]
msg.z = q[2]
msg.w = q[3]
return msg
def convert_planar_phi_to_quaternion(phi):
quaternion = Quaternion()
quaternion.x = 0
quaternion.y = 0
quaternion.z = math.sin(phi / 2)
quaternion.w = math.cos(phi / 2)
return quaternion
def msg_from_quat(self, quaternion):
quat = Quaternion()
quat.x = quaternion.x
quat.y = quaternion.y
quat.z = quaternion.z
quat.w = quaternion.w
return quat
def array_to_quaternion(nparr):
quat = Quaternion()
quat.x = nparr[0]
quat.y = nparr[1]
quat.z = nparr[2]
quat.w = nparr[3]
return quat
def update_kalman(ar_tags):
print "started update"
rospy.wait_for_service('innovation')
update = rospy.ServiceProxy('innovation', NuSrv)
listener = tf.TransformListener()
while True:
try:
try:
(trans, rot) = listener.lookupTransform(ar_tags['arZ'], ar_tags['ar1'], rospy.Time(0))
except:
print "Couldn't look up transform"
continue
lin = Vector3()
quat = Quaternion()
lin.x = trans[0]
lin.y = trans[1]
lin.z = trans[2]
quat.x = rot[0]
quat.y = rot[1]
quat.z = rot[2]
quat.w = rot[3]
transform = Transform()
transform.translation = lin
transform.rotation = quat
test = update(transform, ar_tags['ar1'])
print "Service call succeeded"
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def cal_actor_pose(self, distance):
xp = 0.
yp = 0.
rxp = 0.
ryp = 0.
rq = Quaternion()
# xp = random.uniform(-2.8, 2.8)
# yp = random.uniform(3.0, 5.0)
# ang = 0
# rxp = xp + (distance * math.sin(math.radians(ang)))
# ryp = yp - (distance * math.cos(math.radians(ang)))
# q = quaternion.from_euler_angles(0,0,math.radians(ang))
# rq.x = q.x
# rq.y = q.y
# rq.z = q.z
# rq.w = q.w
while True:
xp = random.uniform(HUMAN_XMIN, HUMAN_XMAX)
yp = random.uniform(HUMAN_YMIN, HUMAN_YMAX)
ang = 0
rxp = xp + (distance * math.sin(math.radians(ang)))
ryp = yp - (distance * math.cos(math.radians(ang)))
human_ud_distance = math.hypot(rxp, ryp)
if rxp < HUMAN_XMAX and rxp > HUMAN_XMIN and ryp < HUMAN_YMAX and ryp > HUMAN_YMIN - distance and human_ud_distance > self.min_threshold_arrive:
# print human_ud_distance
q = quaternion.from_euler_angles(0, 0, math.radians(ang))
rq.x = q.x
rq.y = q.y
rq.z = q.z
rq.w = q.w
break
return xp, yp, rxp, ryp, rq
def conjugate(self, q):
r = Quaternion()
r.w = q.w
r.x = -q.x
r.y = -q.y
r.z = -q.z
return r
def startNode():
c = QuadJoy()
# load data
df = pandas.read_excel(open('droneSolution_copy.xlsx', 'rb'))
n_elements = len(df['time_tot'])
for i in range(n_elements - 1):
c.gazebo_state.model_name = c.name
c.gazebo_state.pose.position.x = df['x_tot'][i]
c.gazebo_state.pose.position.y = df['y_tot'][i]
c.gazebo_state.pose.position.z = df['z_tot'][i]
quat = euler2quat(
[df['roll_tot'][i], df['pitch_tot'][i], df['yaw_tot'][i]])
orient = Quaternion()
orient.x = quat[0]
orient.y = quat[1]
orient.z = quat[2]
orient.w = quat[3]
c.gazebo_state.pose.orientation = orient
c.gazebo_state.reference_frame = "world"
c.pubState.publish(c.gazebo_state)
dt = df['time_tot'][i + 1] - df['time_tot'][i]
scale = 2
rospy.sleep(dt * scale) # in seconds
def convert_planar_phi_to_quaternion(phi):
quaternion = Quaternion()
quaternion.x = 0
quaternion.y = 0
quaternion.z = math.sin(phi / 2)
quaternion.w = math.cos(phi / 2)
return quaternion
def get_quaternion(self):
q = Quaternion()
q.w = self.X[0][0]
q.x = self.X[1][0]
q.y = self.X[2][0]
q.z = self.X[3][0]
return q
def __init__(self,robot):
orientation = Quaternion()
orientation.x = -0.636984559527
orientation.y = -0.249563909878
orientation.z = 0.245820513626
orientation.w = 0.686688285098
MyAction.__init__(self,'push',robot,orientation)
def quaternion_vector2quaternion_ros(quaternion_vector):
"""
Quaternion: [x, y, z, w]
"""
quaternion_ros = Quaternion()
quaternion_ros.x, quaternion_ros.y, quaternion_ros.z, quaternion_ros.w = quaternion_vector
return quaternion_ros
def quat_array_to_quat(quat_array):
new_quat = Quaternion()
new_quat.x = quat_array[0]
new_quat.y = quat_array[1]
new_quat.z = quat_array[2]
new_quat.w = quat_array[3]
return new_quat
def publish_transform_stamped(model_name, x, pub):
ts = TransformStamped()
ts.child_frame_id = model_name
# Header
ts.header.stamp = rospy.Time.now()
ts.header.frame_id = "world"
# Translation
translation = Vector3()
translation.x = x[0]
translation.y = x[1]
translation.z = x[2]
# Rotation
quat = Quaternion()
quat.x = x[6]
quat.y = x[7]
quat.z = x[8]
quat.w = x[9]
# Message
transform = Transform()
transform.translation = translation
transform.rotation = quat
ts.transform = transform
# Publish a transform stamped message
pub.sendTransform(ts)
def serial_listen():
print("doing serial listen")
empty = True
while empty:
try:
imu_in = ser.readline().decode('ASCII')
#imu_in = '[0.433,0.082,-0.271,0.856] [-0.061,-0.061,-0.061] [5591,-5879,12525] 0.000 0.000 \n'
imu_in = imu_in.split(' ')
quaternion = imu_in[0][1:-1].split(',')
quaternion = [float(i) for i in quaternion]
#quaternion.append(quaternion.pop(0))
gyro = imu_in[1][1:-1].split(',')
gyro = [float(i) for i in gyro]
accel = imu_in[2][1:-1].split(',')
accel = [float(i) for i in accel]
#encoder_vel_left = (float(imu_in[3])*0.0001850586) #convert encoder clicks to m
#encoder_vel_right = (float(imu_in[4])*0.0001850586) #convert encoder clicks to m
encoder_pos = imu_in[3][1:-1].split(',')
encoder_pos = [((float(i)/8192)*9.5*3.141)/39.37 for i in encoder_pos]
encoder_vel = imu_in[4][1:-1].split(',')
encoder_vel = [(float(i)*0.111) for i in encoder_vel]
q = Quaternion()
q.x = quaternion[1]
q.y = quaternion[2]
q.z = quaternion[0]
#q.w = quaternion[3]
g = Vector3()
g.x = gyro[0]/57.2958
g.y = gyro[1]/57.2958
g.z = gyro[2]/57.2958
a = Vector3()
a.x = (accel[0]/16384)*9.80665
a.y = (accel[1]/16384)*9.80665
a.z = (accel[2]/16384)*9.80665
twist_linear = Vector3()
linear_velocity = (encoder_vel[0] + encoder_vel[1])/2
twist_linear.y = linear_velocity
twist_angular = Vector3()
angular_velocity = (encoder_vel[1] - encoder_vel[0])/0.4191 #separation of wheels in m
twist_angular.z = angular_velocity
pose_pt = Point()
pose_pt.y = (abs(encoder_pos[0]) + abs(encoder_pos[1]))/2
empty = False
except:
print("failed to get message")
ser.reset_input_buffer()
return q, g ,a, twist_linear, twist_angular, pose_pt
def quat_from_quat_array(quat_array):
quat = Quaternion()
quat.x = quat_array[0]
quat.y = quat_array[1]
quat.z = quat_array[2]
quat.w = quat_array[3]
return quat
def callback(self, msg):
# construct transform from vicon to pseudo-UAV frame, considering that
# the transform is given using the robotics convention: T_XAV_V
x, y, z = msg.pose.position.x, msg.pose.position.y, msg.pose.position.z
qx, qy, qz, qw = msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w
T_XAV_V = homogeneous_matrix((x, y, z), (qx, qy, qz, qw))
# perform required coordinate frame transformations
self.T_UAV_NED = TR.concatenate_matrices(self.T_V_NED, T_XAV_V,
self.T_UAV_XAV)
# publish odometry
ref_msg = Odometry()
ref_msg.header.stamp = rospy.Time.now()
q = TR.quaternion_from_matrix(self.T_UAV_NED)
t = TR.translation_from_matrix(self.T_UAV_NED)
ref_msg.pose.pose.position.x = t[0]
ref_msg.pose.pose.position.y = t[1]
ref_msg.pose.pose.position.z = t[2]
ref_msg.pose.pose.orientation.x = q[0]
ref_msg.pose.pose.orientation.y = q[1]
ref_msg.pose.pose.orientation.z = q[2]
ref_msg.pose.pose.orientation.w = q[3]
att_msg = Quaternion()
att_msg.x = q[0]
att_msg.y = q[1]
att_msg.z = q[2]
att_msg.w = q[3]
self.reference_pub.publish(ref_msg)
self.ext_att_pub.publish(att_msg)
def publish(self):
# publish or perish
now = rospy.Time.now()
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2)
quaternion.w = cos(self.th / 2)
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame_id, self.odom_frame_id)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.twist.linear.x
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.twist.angular.z
self.odomPub.publish(odom)
def quaternion_vector2quaternion_ros(quaternion_vector): """ Quaternion: [x, y, z, w] """ quaternion_ros = Quaternion() quaternion_ros.x, quaternion_ros.y, quaternion_ros.z, quaternion_ros.w = quaternion_vector return quaternion_ros
def quaternion_to_msg(q):
msg = Quaternion()
msg.x = q[0]
msg.y = q[1]
msg.z = q[2]
msg.w = q[3]
return msg
def publish_imu(self):
euler_data = self.bno.getVector(BNO055.BNO055.VECTOR_EULER)
# print('reg IMU = ' + repr(euler_data))
# euler_data = self.bno.read_euler()
# print('read Euler = ')
# print(self.bno.read_euler())
# print('getVector = ')
# print(euler_data)
rpy_message = Vector3(euler_data[0], euler_data[1], euler_data[2])
# print(rpy_message)
# print(' ')
quaternion_data = self.bno.getQuat()
gyro_data = self.bno.getVector(BNO055.BNO055.VECTOR_GYROSCOPE)
linear = Vector3(0, 0, 0)
angular = Vector3(gyro_data[0], gyro_data[1], gyro_data[2])
header = Header()
header.stamp = rospy.Time.now()
quat = Quaternion()
quat.w = quaternion_data[3]
quat.x = quaternion_data[0]
quat.y = quaternion_data[1]
quat.z = quaternion_data[2]
twist_message = TwistStamped(header, Twist(linear, angular))
point = Point(0, 0, 0)
pose = Pose(point, quat)
pose_message = PoseStamped(header, pose)
self.pose_publisher.publish(pose_message)
self.twist_publisher.publish(twist_message)
self.rpy_publisher.publish(rpy_message)
def rotateQuaternion(q_orig, yaw):
"""
Converts a basic rotation about the z-axis (in radians) into the
Quaternion notation required by ROS transform and pose messages.
:Args:
| q_orig (geometry_msgs.msg.Quaternion): to be rotated
| yaw (double): rotate by this amount in radians
:Return:
| (geometry_msgs.msg.Quaternion) q_orig rotated yaw about the z axis
"""
# Create a temporary Quaternion to represent the change in heading
q_headingChange = Quaternion()
p = 0
y = yaw / 2.0
r = 0
sinp = math.sin(p)
siny = math.sin(y)
sinr = math.sin(r)
cosp = math.cos(p)
cosy = math.cos(y)
cosr = math.cos(r)
q_headingChange.x = sinr * cosp * cosy - cosr * sinp * siny
q_headingChange.y = cosr * sinp * cosy + sinr * cosp * siny
q_headingChange.z = cosr * cosp * siny - sinr * sinp * cosy
q_headingChange.w = cosr * cosp * cosy + sinr * sinp * siny
# ----- Multiply new (heading-only) quaternion by the existing (pitch and bank)
# ----- quaternion. Order is important! Original orientation is the second
# ----- argument rotation which will be applied to the quaternion is the first
# ----- argument.
return multiply_quaternions(q_headingChange, q_orig)
def getOrientation(self):
ort = Quaternion()
ort.x = self._orientationX
ort.y = self._orientationY
ort.z = self._orientationZ
ort.w = self._orientationW
return ort
def get_ros_quaternion(self, almath_quaternion):
output = Quaternion()
output.w = almath_quaternion.w
output.x = almath_quaternion.x
output.y = almath_quaternion.y
output.z = almath_quaternion.z
return output
def quaternionToQuaternionMsg(quat):
q = Quaternion()
q.x = quat[0]
q.y = quat[1]
q.z = quat[2]
q.w = quat[3]
return q
def talker():
# pub_angle = rospy.Publisher('angles', Point, queue_size=10) #quitar el numeral
pub_angle = rospy.Publisher('covers', Vector3, queue_size=10) #vector3 nuevo borrar
pub_position = rospy.Publisher('positions', Quaternion, queue_size=10)
rospy.init_node('angle_sender', anonymous=True)
rate = rospy.Rate(1) # 1hz
point_message=Point()
quaternion_message= Quaternion()
vector3_message=Vector3() #vector3 nuevo
index=0
while not rospy.is_shutdown():
# parameters = np.array([[angles[index][0],angles[index][1],0.4,0.3]]) #quitar el numeral
parameters = np.array([[angles[index][index][0],angles[index][index][1],0.4,0.3]]) #vector3 nuevo
position_matrix = subs_dh(T03,t2,t3,l2,l3,parameters)
quaternion_message.x = position_matrix[0][3]
quaternion_message.y = position_matrix[1][3]
quaternion_message.z = position_matrix[2][3]
quaternion_message.w = 0
# point_message.x= angles[index][0] #quitar el numeral
# point_message.y= angles[index][1] #quitar el numeral
# point_message.z= 0 #quitar el numeral
vector3_message.x= covers[index][index][0] #vector3 nuevo
vector3_message.y= covers[index][index][0] #vector3 nuevo
vector3_message.z= covers[index][index][0] #vector3 nuevo
# hello_str = "hello world %s" % rospy.get_time()
rospy.loginfo('hello')
# pub_angle.publish(point_message) #quitar el numeral
pub_angle.publish(vector3_message)
pub_position.publish(quaternion_message)
rate.sleep()
index=index+1
def phi2quat(self, phi):
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = math.sin(phi / 2.0)
quaternion.w = math.cos(phi / 2.0)
return quaternion
def __init__(self):
position=Point()
rotation=Quaternion()
rospy.init_node('tf_listener')
self.pub = rospy.Publisher('robot_odom', robot_odom, queue_size=10)
self.now=rospy.Time()
self.tf_listener=tf.TransformListener()
rospy.sleep(0.5)
frame='/odom'
wantedframe = self.frame_checker(frame)
while not rospy.is_shutdown():
(po,ro)=self.get_odom(frame,wantedframe)
position.x=po[0]
position.y=po[1]
position.z=po[2]
rotation.x=ro[0]
rotation.y=ro[1]
rotation.z=ro[2]
rotation.w=ro[3]
#rospy.sleep()
self.publish(position,rotation,wantedframe)
#print 'position: \n',position,'\n','rotation: \n',self.quat_to_angle(rotation),'time: ', self.robot_odom.header.stamp
print self.robot_odom,'\nangular: ',self.quat_to_angle(self.robot_odom.rotation)#self.normalize_angle(self.quat_to_angle(self.robot_odom.rotation))
def publish_transform_stamped_relative(model_name, parent_name, struct_x, struct_y, pub):
ts = TransformStamped()
ts.child_frame_id = model_name
# Header
ts.header.stamp = rospy.Time.now()
ts.header.frame_id = parent_name
# Translation
translation = Vector3()
translation.x = struct_x
translation.y = struct_y
translation.z = 0
# Rotation
quat = Quaternion()
quat.x = 0
quat.y = 0
quat.z = 0
quat.w = 1
# Message
transform = Transform()
transform.translation = translation
transform.rotation = quat
ts.transform = transform
# Publish a transform stamped message
pub.sendTransform(ts)
def array_to_quaternion(nparr):
quat = Quaternion()
quat.x = nparr[0]
quat.y = nparr[1]
quat.z = nparr[2]
quat.w = nparr[3]
return quat
def onPose(self, pose): """ store current pose and diff.""" self.m_pre_pose = pose # get amcl corrected pose. try: (trans, rot) = self.tf_listener.lookupTransform(self.map_frame, self.odom_frame, rospy.Time(0)) curQ = Quaternion() curQ.x = rot[0] curQ.y = rot[1] curQ.z = rot[2] curQ.w = rot[3] diff = self.decQuaternion(self.pre_rot , curQ) #(r, p, y) = tf.transformations.euler_from_quaternion([diff.x, diff.y, diff.z, diff.w]) #if y < 1: self.rot_diff = self.addQuaternion(self.rot_diff , diff) self.pre_rot = rot if CommonConfig.DEBUG_FLAG is True: rospy.logdebug( "ImuDriftCorrect#onPose diff[%f][%f]" % (self.rot_diff.z, self.rot_diff.w)) except tf.LookupException as err1: rospy.logerr( "[ImuDriftCorrect][ERROR]1 Cannot get TF!!" + str(err1)) except tf.ConnectivityException as err2: rospy.logerr( "[ImuDriftCorrect][ERROR]2 Cannot get TF!!" + str(err2)) except tf.ExtrapolationException as err3: rospy.logerr( "[ImuDriftCorrect][ERROR]3 Cannot get TF!!" + str(err3))
def publish(self, data):
if not self._calib and data.getImuMsgId() == PUB_ID:
q = data.getOrientation()
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))
res = Quaternion()
res.w = array[0]
res.x = array[1]
res.y = array[2]
res.z = array[3]
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = res
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
elif data.getImuMsgId() == CALIB_ID:
x, y, z = data.getValues()
msg = imuCalib()
if x > self._xMax:
self._xMax = x
if x < self._xMin:
self._xMin = x
if y > self._yMax:
self._yMax = y
if y < self._yMin:
self._yMin = y
if z > self._zMax:
self._zMax = z
if z < self._zMin:
self._zMin = z
msg.x.data = x
msg.x.max = self._xMax
msg.x.min = self._xMin
msg.y.data = y
msg.y.max = self._yMax
msg.y.min = self._yMin
msg.z.data = z
msg.z.max = self._zMax
msg.z.min = self._zMin
self._pubCalib.publish(msg)
def list_to_dict(lst):
"""
Convert a list to a pose dictionary, assuming it is in the order
x, y, z, orientation x, orientation y, orientation z[, orientation w].
"""
if len(lst) == 6:
qtn = tf.transformations.quaternion_from_euler(lst[3], lst[4], lst[5])
elif len(lst) == 7:
qtn = Quaternion()
qtn.x = lst[3]
qtn.y = lst[4]
qtn.z = lst[5]
qtn.w = lst[6]
else:
raise MoveItCommanderException("""Expected either 6 or 7 elements
in list: (x,y,z,r,p,y) or (x,y,z,qx,qy,qz,qw)""")
pnt = Point()
pnt.x = lst[0]
pnt.y = lst[1]
pnt.z = lst[2]
pose_dict = {
'position': pnt,
'orientation': qtn
}
return pose_dict
def rotateQuaternion(q_orig, yaw):
"""
Converts a basic rotation about the z-axis (in radians) into the
Quaternion notation required by ROS transform and pose messages.
:Args:
| q_orig (geometry_msgs.msg.Quaternion): to be rotated
| yaw (double): rotate by this amount in radians
:Return:
| (geometry_msgs.msg.Quaternion) q_orig rotated yaw about the z axis
"""
# Create a temporary Quaternion to represent the change in heading
q_headingChange = Quaternion()
p = 0
y = yaw / 2.0
r = 0
sinp = math.sin(p)
siny = math.sin(y)
sinr = math.sin(r)
cosp = math.cos(p)
cosy = math.cos(y)
cosr = math.cos(r)
q_headingChange.x = sinr * cosp * cosy - cosr * sinp * siny
q_headingChange.y = cosr * sinp * cosy + sinr * cosp * siny
q_headingChange.z = cosr * cosp * siny - sinr * sinp * cosy
q_headingChange.w = cosr * cosp * cosy + sinr * sinp * siny
# Multiply new (heading-only) quaternion by the existing (pitch and bank)
# quaternion. Order is important! Original orientation is the second
# argument rotation which will be applied to the quaternion is the first
# argument.
return multiply_quaternions(q_headingChange, q_orig)
def axis_angle_to_quaternion(axis, angle):
q = Quaternion()
q.x = axis.x * math.sin(angle / 2.0)
q.y = axis.y * math.sin(angle / 2.0)
q.z = axis.z * math.sin(angle / 2.0)
q.w = math.cos(angle / 2.0)
return q
def publish(self, data):
q = data.getOrientation()
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
# print "Before ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"
array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))
res = Quaternion()
res.w = array[0]
res.x = array[1]
res.y = array[2]
res.z = array[3]
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
# print "after ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = res
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
def Publish_Odom_Tf(self): #quaternion = tf.transformations.quaternion_from_euler(0, 0, theta) quaternion = Quaternion() quaternion.x = 0 quaternion.y = 0 quaternion.z = math.sin(self.Heading / 2.0) quaternion.w = math.cos(self.Heading / 2.0) rosNow = rospy.Time.now() self._tf_broad_caster.sendTransform( (self.X_Pos, self.Y_Pos, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), rosNow, "base_footprint", "odom" ) # next, we'll publish the odometry message over ROS odometry = Odometry() odometry.header.frame_id = "odom" odometry.header.stamp = rosNow odometry.pose.pose.position.x = self.X_Pos odometry.pose.pose.position.y = self.Y_Pos odometry.pose.pose.position.z = 0 odometry.pose.pose.orientation = quaternion odometry.child_frame_id = "base_link" odometry.twist.twist.linear.x = self.Velocity odometry.twist.twist.linear.y = 0 odometry.twist.twist.angular.z = self.Omega self._odom_publisher.publish(odometry)
def compare_all(data):
global k
last_row = csv_data[k-frequency_offset]
diff = Quaternion()
diff.x = round(float(last_row[1])-convert_ppm(data.channels[3]),4)
diff.y = round(float(last_row[2])-convert_ppm(data.channels[1]),4)
diff.z = round(float(last_row[4])-convert_ppm(data.channels[0]),4)
diff.w = round(float(last_row[5])+convert_ppm(data.channels[2]),4)
testing_pub.publish(diff)
if (k < len(csv_data)):
row = csv_data[k]
#log for post processing
#joystick log
c = csv.writer(open("/home/jinahadam/catkin_ws/src/testing/gps_joy.csv", "ab"))
c.writerow([data.header.stamp.secs,row[1],row[2],row[4],row[5]])
#rc Inn log
c = csv.writer(open("/home/jinahadam/catkin_ws/src/testing/rc_in.csv", "ab"))
c.writerow([data.header.stamp.secs,convert_ppm(data.channels[3]),convert_ppm(data.channels[1]),convert_ppm(data.channels[0]),convert_ppm(data.channels[2])])
msg = Joy()
msg.axes = (float(row[1]),float(row[2]),float(row[3]),float(row[4]),float(row[5]),float(row[6]),float(row[7]))
msg.buttons = (0,0,0,0,0,0,0,0,0,0,0)
k = k + 1
pub.publish(msg)
else:
#k = 0
print("Joystick & RC In data logged with GPS Time")
exit()
def get_ros_quaternion(self, almath_quaternion):
output = Quaternion()
output.w = almath_quaternion.w
output.x = almath_quaternion.x
output.y = almath_quaternion.y
output.z = almath_quaternion.z
return output
def poll(self):
(x, y, theta) = self.arduino.arbot_read_odometry()
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
# Create the odometry transform frame broadcaster.
now = rospy.Time.now()
self.odomBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
now,
"base_link",
"odom"
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.header.stamp = now
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = self.forwardSpeed
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.angularSpeed
self.arduino.arbot_set_velocity(self.forwardSpeed, self.angularSpeed)
self.odomPub.publish(odom)
def get_nearest_cloud(self, msg):
points = list()
points_xy = list()
# Get all the points in the visible cloud (may be prefiltered by other nodes)
for point in point_cloud2.read_points(msg, skip_nans=True):
points.append(point[:3])
points_xy.append(point[:2])
# Convert to a numpy array
points_arr = np.float32([p for p in points]).reshape(-1, 1, 3)
# Compute the COG
cog = np.mean(points_arr, 0)
# Convert to a Point
cog_point = Point()
cog_point.x = cog[0][0]
cog_point.y = cog[0][1]
cog_point.z = cog[0][2]
#cog_point.z = 0.35
# Abort if we get an NaN in any component
if np.isnan(np.sum(cog)):
return
# If we have enough points, find the best fit ellipse around them
try:
if len(points_xy) > 6:
points_xy_arr = np.float32([p for p in points_xy]).reshape(-1, 1, 2)
track_box = cv2.fitEllipse(points_xy_arr)
else:
# Otherwise, find the best fitting rectangle
track_box = cv2.boundingRect(points_xy_arr)
angle = pi - radians(track_box[2])
except:
return
#print angle
# Convert the rotation angle to a quaternion
q_angle = quaternion_from_euler(0, angle, 0, axes='sxyz')
q = Quaternion(*q_angle)
q.x = 0.707
q.y = 0
q.z = 0.707
q.w = 0
# Publish the COG and orientation
target = PoseStamped()
target.header.stamp = rospy.Time.now()
target.header.frame_id = msg.header.frame_id
target.pose.position = cog_point
target.pose.orientation = q
# Publish the movement command
self.target_pub.publish(target)
def main():
odomPub=rospy.Publisher("odomet", Odometry,queue_size=10)
rospy.Subscriber("cmd_vel",Twist,Odomcallback)
then = rospy.Time.now()
d=0.0
theta=0.0
x=0
y=0
Gx=0
Gy=0
global dx
global dw
while not rospy.is_shutdown():
now = rospy.Time.now()
elapsed = now - then
then=now
elapsed = elapsed.to_sec()
d = d + dx*elapsed
theta = theta+( dw*elapsed)
x = cos( theta ) * d
y = -sin( theta ) * d
Gx = Gx + ( cos( theta ) * x - sin( theta ) * y )
Gy = Gy + ( sin( theta) * x + cos( theta ) * y )
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin( theta / 2 )
quaternion.w = cos( theta / 2 )
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = "odom"
odom.pose.pose.position.x =Gx
odom.pose.pose.position.y = Gy
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = "chassis"
odom.twist.twist.linear.x = dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = dw
odomPub.publish(odom)
r = rospy.Rate(10)
rospy.loginfo("x= %s " % Gx)
rospy.loginfo("y= %s " % Gy)
rospy.loginfo("theta %s " % theta)
r.sleep()
def update(self):
#############################################################################
now = rospy.Time.now()
if now > self.t_next:
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
# calculate odometry
if self.enc_left == None:
d_left = 0
d_right = 0
else:
d_left = (self.left - self.enc_left) / self.ticks_meter
d_right = (self.right - self.enc_right) / self.ticks_meter
self.enc_left = self.left
self.enc_right = self.right
# distance traveled is the average of the two wheels
d = (d_left + d_right) / 2
# this approximation works (in radians) for small angles
th = (d_right - d_left) / self.base_width
# calculate velocities
self.dx = d / elapsed
self.dr = th / elapsed
if (d != 0):
# calculate distance traveled in x and y
x = cos(th) * d
y = -sin(th) * d
# calculate the final position of the robot
self.x = self.x + (cos(self.th) * x - sin(self.th) * y)
self.y = self.y + (sin(self.th) * x + cos(self.th) * y)
if (th != 0):
self.th = self.th + th
# publish the odom information
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2)
quaternion.w = cos(self.th / 2)
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame_id, self.odom_frame_id)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)
def quat_product (qa,qb): qc=Quaternion() qc.w = qa.w*qb.w - (qa.x*qb.x + qa.y*qb.y + qa.z*qb.z) qc.x = (qa.w*qb.x + qa.x*qb.w + qa.y*qb.z - qa.z*qb.y) qc.y = (qa.w*qb.y - qa.x*qb.z + qa.y*qb.w + qa.z*qb.x) qc.z = (qa.w*qb.z + qa.x*qb.y - qa.y*qb.x + qa.z*qb.w) return qc
def orientation_to_quaternion(self, pitch, roll, yaw):
quaternion_list = quaternion_from_euler(pitch, roll, yaw)
quaternion = Quaternion()
quaternion.x = quaternion_list[0]
quaternion.y = quaternion_list[1]
quaternion.z = quaternion_list[2]
quaternion.w = quaternion_list[3]
return quaternion
def quat_product(qa, qb):
qc = Quaternion()
qc.w = qa.w * qb.w - (qa.x * qb.x + qa.y * qb.y + qa.z * qb.z)
qc.x = (qa.w * qb.x + qa.x * qb.w + qa.y * qb.z - qa.z * qb.y)
qc.y = (qa.w * qb.y - qa.x * qb.z + qa.y * qb.w + qa.z * qb.x)
qc.z = (qa.w * qb.z + qa.x * qb.y - qa.y * qb.x + qa.z * qb.w)
return qc
def get_orientation_from_angles(r, p, y):
quaternion = quaternion_from_euler(r, p, y)
orientation = Quaternion()
orientation.x = quaternion[0]
orientation.y = quaternion[1]
orientation.z = quaternion[2]
orientation.w = quaternion[3]
return orientation
def array_to_q(array):
#return geometry_msgs/Quaternion.msg from 4-element list
q = Quaternion()
q.x = array[0]
q.y = array[1]
q.z = array[2]
q.w = array[3]
return q
def publish(self, data):
q = Quaternion()
q.x = 0
q.y = 0
q.z = data[6]
q.w = data[7]
odomMsg = Odometry()
odomMsg.header.frame_id = self._odom
odomMsg.header.stamp = rospy.get_rostime()
odomMsg.child_frame_id = self._baseLink
odomMsg.pose.pose.position.x = data[0]
odomMsg.pose.pose.position.y = data[1]
odomMsg.pose.pose.position.z = 0
odomMsg.pose.pose.orientation = q
if self._firstTimePub:
self._prevOdom = odomMsg
self._firstTimePub = False
return
velocity = Twist()
deltaTime = odomMsg.header.stamp.to_sec() - self._prevOdom.header.stamp.to_sec()
yaw, pitch, roll = euler_from_quaternion(
[odomMsg.pose.pose.orientation.w, odomMsg.pose.pose.orientation.x, odomMsg.pose.pose.orientation.y,
odomMsg.pose.pose.orientation.z])
prevYaw, prevPitch, prevRollprevYaw = euler_from_quaternion(
[self._prevOdom.pose.pose.orientation.w, self._prevOdom.pose.pose.orientation.x,
self._prevOdom.pose.pose.orientation.y, self._prevOdom.pose.pose.orientation.z])
if deltaTime > 0:
velocity.linear.x = (data[8] / deltaTime)
deltaYaw = yaw - prevYaw
# rospy.loginfo("yaw: %f\t\tpevYaw: %f\t\tdeltaYaw: %f" % (yaw,prevYaw,deltaYaw))
if deltaYaw > math.pi: deltaYaw -= 2 * math.pi
elif deltaYaw < -math.pi: deltaYaw += 2 * math.pi
if deltaTime > 0:
velocity.angular.z = -(deltaYaw / deltaTime)
# rospy.loginfo("deltaYaw after check: %f\t\t angular: %f" % (deltaYaw, velocity.angular.z))
odomMsg.twist.twist = velocity
self._prevOdom = odomMsg
traMsg = TransformStamped()
traMsg.header.frame_id = self._odom
traMsg.header.stamp = rospy.get_rostime()
traMsg.child_frame_id = self._baseLink
traMsg.transform.translation.x = data[0]
traMsg.transform.translation.y = data[1]
traMsg.transform.translation.z = 0
traMsg.transform.rotation = q
self._pub.publish(odomMsg)
self._broadCase.sendTransformMessage(traMsg)
def run(self):
rosRate = rospy.Rate(self.rate)
rospy.loginfo("Publishing Odometry data at: " + str(self.rate) + " Hz")
vx = 0.0
vy = 0.0
vth = 0.0
while not rospy.is_shutdown() and not self.finished.isSet():
now = rospy.Time.now()
if now > self.t_next:
dt = now - self.then
self.then = now
dt = dt.to_sec()
delta_x = (vx * cos(self.th) - vy * sin(self.th)) * dt
delta_y = (vx * sin(self.th) + vy * cos(self.th)) * dt
delta_th = vth * dt
self.x += delta_x
self.y += delta_y
self.th += delta_th
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
"base_link",
"odom"
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.odomPub.publish(odom)
self.t_next = now + self.t_delta
rosRate.sleep()
def only_yaw(self, quat, mag=1, add=0):
_, _, yaw = euler_from_quaternion(self.quat_to_list(quat))
quat_yaw = quaternion_from_euler(0.0, 0.0, mag * yaw + add)
ret_quat = Quaternion()
ret_quat.x = quat_yaw[0]
ret_quat.y = quat_yaw[1]
ret_quat.z = quat_yaw[2]
ret_quat.w = quat_yaw[3]
return ret_quat
def advance(self, dt):
now = self.then + dt
elapsed = now - self.then
self.then = now
if self.debug:
print "advancing to t=%5.3f" % (self.then)
ldist = dt * self.lwSpeed
rdist = dt * self.rwSpeed
d = (ldist + rdist)/2.0
th = (rdist - ldist)/self.distanceBetweenWheels
dx = d / dt
dth = th / dt
if (d != 0):
x = math.cos(th)*d
y = math.sin(th)*d # Does this differ from the create driver? Should it?
self.x = self.x + (math.cos(self.th)*x - math.sin(self.th)*y)
self.y = self.y + (math.sin(self.th)*x + math.cos(self.th)*y)
if (th != 0):
self.th = self.th + th
self.th = self.th % (2 * math.pi)
self.th = self.th - (2 * math.pi) if self.th > math.pi else self.th
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = math.sin(self.th/2.0)
quaternion.w = math.cos(self.th/2.0)
# Most of the applications I'm working on don't need this.
# self.odomBroadcaster.sendTransform(
# (self.x, self.y, 0),
# (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
# rospy.Time.now(),
# "base_link",
# "odom"
# )
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "odom"
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = "base_link"
odom.twist.twist.linear.x = dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = dth
self.odomPub.publish(odom)
def quat_normalize(q): q1=Quaternion() qnorm = sqrt(q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z) q1.w = q.w/qnorm q1.x = q.x/qnorm q1.y = q.y/qnorm q1.z = q.z/qnorm return q1
def quat_inverse (q): q_inv=Quaternion() norm = sqrt(q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z) q_inv.w = +q.w / norm q_inv.x = -q.x / norm q_inv.y = -q.y / norm q_inv.z = -q.z / norm return q_inv
def _x_y_yaw_to_pose(self, x, y, yaw):
position = Point(x, y, 0)
orientation = Quaternion()
q = quaternion_from_euler(0, 0, yaw)
orientation.x = q[0]
orientation.y = q[1]
orientation.z = q[2]
orientation.w = q[3]
return Pose(position=position, orientation=orientation)
def publish_odometry(self):
quat = tf.transformations.quaternion_from_euler(0, 0, self.yaw)
quaternion = Quaternion()
quaternion.x = quat[0]
quaternion.y = quat[1]
quaternion.z = quat[2]
quaternion.w = quat[3]
if self.last_time is None:
self.last_time = rospy.Time.now()
vx = self.vx #(msg->twist.twist.linear.x) *trans_mul_;
vy = self.vy #msg->twist.twist.linear.y;
vth = self.vyaw #(msg->twist.twist.angular.z) *rot_mul_;
current_time = rospy.Time.now()
dt = (current_time - self.last_time).to_sec()
delta_x = (vx * math.cos(self.yaw) - vy * math.sin(self.yaw)) * dt
delta_y = (vx * math.sin(self.yaw) + vy * math.cos(self.yaw)) * dt
delta_th = vth * dt
self.x += delta_x
self.y += delta_y
self.yaw += delta_th
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = vy
odom.twist.twist.angular.z = vth
self.odom_pub.publish(odom)
transform_stamped = TransformStamped()
transform_stamped.header = odom.header
transform_stamped.transform.translation.x = self.x
transform_stamped.transform.translation.y = self.y
transform_stamped.transform.translation.z = 0.0
transform_stamped.transform.rotation = quaternion
#self.tfbr.sendTransform(transform_stamped)
self.tfbr.sendTransform((self.x, self.y, 0.0),
(quat[0], quat[1], quat[2], quat[3]),
rospy.Time.now(),
"odom",
"base_link")
self.last_time = current_time
def update(self):
#############################################################################
now = rospy.Time.now()
if now > self.t_next:
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
# this approximation works (in radians) for small angles
th = self.th - self.th_pre
self.dr = th / elapsed
# publish the odom information
quaternion = Quaternion()
quaternion.x = self.qx
quaternion.y = self.qy
quaternion.z = self.qz
quaternion.w = self.qw
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(0, 0, quaternion.z, quaternion.w),
rospy.Time.now(),
self.base_frame_id,
self.odom_frame_id,
)
self.laserBroadcaster.sendTransform(
(0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), self.laser_frame_id, self.base_frame_id
)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)
laser = LaserScan()
laser.header.stamp = now
laser.header.frame_id = self.laser_frame_id
laser.angle_min = self.laser.angle_min
laser.angle_max = self.laser.angle_max
laser.angle_increment = self.laser.angle_increment
laser.time_increment = self.laser.time_increment
laser.scan_time = self.laser.scan_time
laser.range_min = self.laser.range_min
laser.range_max = self.laser.range_max
laser.ranges = self.laser.ranges
laser.intensities = self.laser.intensities
self.laserPub.publish(laser)
