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 goal_orientation(self, theta): orientation = Quaternion() if -numpy.pi < theta < -numpy.pi*2.0/3.0: print 'reverse orientation' orientation.z = -numpy.sin(theta/2.0) orientation.w = -numpy.cos(theta/2.0) else: orientation.z = numpy.sin(theta/2.0) orientation.w = numpy.cos(theta/2.0) return orientation
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 GoalOrientation(self, theta): orientation = Quaternion() if -numpy.pi < theta < -numpy.pi*2.0/3.0: orientation.z = -numpy.sin(theta/2.0) orientation.w = -numpy.cos(theta/2.0) else: orientation.z = numpy.sin(theta/2.0) orientation.w = numpy.cos(theta/2.0) return orientation
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 interpOdom(self, o1, o2, frac):
"""
Interpolates two odometry objects, and returns a third one based
on frac, the distance between the two.
"""
pos1 = o1.pose.pose.position
pos2 = o2.pose.pose.position
posOut = Point(x = pos1.x + (pos2.x - pos1.x) * frac,
y = pos1.y + (pos2.y - pos1.y) * frac,
z = pos1.z + (pos2.z - pos1.z) * frac)
or1 = o1.pose.pose.orientation
or2 = o2.pose.pose.orientation
orOut = Quaternion()
# Interpolating quaternions is complicated. First,
# compute the dot product of the quaternions. "Theta" in what
# follows is the angle between the two quaternions.
cosHalfTheta = or1.z * or2.z + or1.w * or2.w
if math.fabs(cosHalfTheta) >= 1.0:
orOut.z = or1.z
orOut.w = or1.w
else:
halfTheta = math.acos(cosHalfTheta)
sinHalfTheta = (1.0 - cosHalfTheta**2)**0.5
if math.fabs(sinHalfTheta) < 0.001:
orOut.z = (or1.z + or2.z)/2.0
orOut.w = (or1.w + or2.w)/2.0
else:
rA = math.sin((1 - frac) * halfTheta) / sinHalfTheta
rB = math.sin(frac * halfTheta) / sinHalfTheta
orOut.w = (or1.w * rA + or2.w * rB);
orOut.z = (or1.z * rA + or2.z * rB);
poseOut = Pose(position = posOut, orientation = orOut)
poseCVOut = PoseWithCovariance(pose = poseOut)
lin1 = o1.twist.twist.linear
lin2 = o2.twist.twist.linear
linOut = Vector3(x = lin1.x + (lin2.x - lin1.x) * frac,
y = lin1.y + (lin2.y - lin1.y) * frac,
z = lin1.z + (lin2.z - lin1.z) * frac)
ang1 = o1.twist.twist.angular
ang2 = o2.twist.twist.angular
angOut = Vector3(x = ang1.x + (ang2.x - ang1.x) * frac,
y = ang1.y + (ang2.y - ang1.y) * frac,
z = ang1.z + (ang2.z - ang1.z) * frac)
twistOut = Twist(linear = linOut, angular = angOut)
twistCVOut = TwistWithCovariance(twist = twistOut)
return Odometry(pose = poseCVOut, twist = twistCVOut)
def getOrientation(self):
ort = Quaternion()
ort.x = self._orientationX
ort.y = self._orientationY
ort.z = self._orientationZ
ort.w = self._orientationW
return ort
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 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 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 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_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 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 on_aseba_odometry_event(self,data): now = data.stamp dt = (now-self.then).to_sec() self.then = now dsl = (data.data[0]*dt)/SPEED_COEF # left wheel delta in mm dsr = (data.data[1]*dt)/SPEED_COEF # right wheel delta in mm ds = ((dsl+dsr)/2.0)/1000.0 # robot traveled distance in meters dth = atan2(dsr-dsl,BASE_WIDTH) # turn angle self.x += ds*cos(self.th+dth/2.0) self.y += ds*sin(self.th+dth/2.0) self.th+= dth # prepare tf from base_link to odom quaternion = Quaternion() quaternion.z = sin(self.th/2.0) quaternion.w = cos(self.th/2.0) # prepare odometry self.odom.header.stamp = rospy.Time.now() # OR TO TAKE ONE FROM THE EVENT? self.odom.pose.pose.position.x = self.x self.odom.pose.pose.position.y = self.y self.odom.pose.pose.position.z = 0 self.odom.pose.pose.orientation = quaternion self.odom.twist.twist.linear.x = ds/dt self.odom.twist.twist.angular.z = dth/dt # publish odometry self.odom_broadcaster.sendTransform((self.x,self.y,0),(quaternion.x,quaternion.y,quaternion.z,quaternion.w),self.then,"base_link","odom") self.odom_pub.publish(self.odom)
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 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 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(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 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 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 __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 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 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 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 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 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 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 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 _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 euler2quat(euler):
q = Quaternion()
cpsi = cos(0.5 * euler.z)
spsi = sin(0.5 * euler.z)
ctheta = cos(0.5 * euler.y)
stheta = sin(0.5 * euler.y)
cphi = cos(0.5 * euler.x)
sphi = sin(0.5 * euler.x)
q.w = cphi * ctheta * cpsi + sphi * stheta * spsi
q.x = sphi * ctheta * cpsi - cphi * stheta * spsi
q.y = cphi * stheta * cpsi + sphi * ctheta * spsi
q.z = cphi * ctheta * spsi - sphi * stheta * cpsi
return q
def _BroadcastOdometryInfo(self, lineParts):
partsCount = len(lineParts)
# rospy.logwarn(partsCount)
if (partsCount < 6):
pass
try:
x = float(lineParts[1])
y = float(lineParts[2])
theta = float(lineParts[3])
vx = float(lineParts[4])
omega = float(lineParts[5])
# quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
rosNow = rospy.Time.now()
# First, we'll publish the transform from frame odom to frame base_footprint over tf
# Note that sendTransform requires that 'to' is passed in before 'from' while
# the TransformListener' lookupTransform function expects 'from' first followed by 'to'.
self._OdometryTransformBroadcaster.sendTransform(
(x, y, 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 = x
odometry.pose.pose.position.y = y
odometry.pose.pose.position.z = 0
odometry.pose.pose.orientation = quaternion
odometry.child_frame_id = "base_footprint"
odometry.twist.twist.linear.x = vx
odometry.twist.twist.linear.y = 0
odometry.twist.twist.angular.z = omega
self._OdometryPublisher.publish(odometry)
# rospy.loginfo(odometry)
except:
rospy.logwarn("Unexpected error odomfrom arduino.py :" +
str(sys.exc_info()[0]))
def numpy_quaternion_to_ros_quaternion(numpy_quaternion):
"""
Convert a quaternion from transforms to a ROS msg quaternion
:param numpy_quaternion: a numpy quaternion
:type numpy_quaternion: numpy.array
:return: a ROS quaternion
:rtype: geometry_msgs.msg.Quaternion
"""
ros_quaternion = Quaternion()
ros_quaternion.x = numpy_quaternion[0]
ros_quaternion.y = numpy_quaternion[1]
ros_quaternion.z = numpy_quaternion[2]
ros_quaternion.w = numpy_quaternion[3]
return ros_quaternion
def poseFromFloats(floatList):
myPose = Pose()
myPoint = Point()
myQuaternion = Quaternion()
myPoint.x = floatList[0]
myPoint.y = floatList[1]
myPoint.z = floatList[2]
myQuaternion.x = floatList[3]
myQuaternion.y = floatList[4]
myQuaternion.z = floatList[5]
myQuaternion.w = floatList[6]
myPose.position = myPoint
myPose.orientation = myQuaternion
return myPose
def euler2quaternion(euler):
q = Quaternion()
cy = np.cos(euler[2] * 0.5)
sy = np.sin(euler[2] * 0.5)
cp = np.cos(euler[1] * 0.5)
sp = np.sin(euler[1] * 0.5)
cr = np.cos(euler[0] * 0.5)
sr = np.sin(euler[0] * 0.5)
q.w = cy * cp * cr + sy * sp * sr
q.x = cy * cp * sr - sy * sp * cr
q.y = sy * cp * sr + cy * sp * cr
q.z = sy * cp * cr - cy * sp * sr
return q
def init():
global g_limb, g_orientation_hand_down, g_position_neutral, pos, posp, gripper
rospy.init_node('cairo_sawyer_ik_example')
g_limb = intera_interface.Limb('right')
gripper = intera_interface.Gripper()
# This quaternion will have the hand face straight down (ideal for picking tasks)
g_orientation_hand_down = Quaternion()
g_orientation_hand_down.x = 0.704238785359
g_orientation_hand_down.y = 0.709956638597
g_orientation_hand_down.z = -0.00229009932359
g_orientation_hand_down.w = 0.00201493272073
# g_orientation_hand_down.x = 0.65759208006
# g_orientation_hand_down.y = 0.00507142331692
# g_orientation_hand_down.z = 0.753339706619
# g_orientation_hand_down.w = -0.00512087278968
# This is the default neutral position for the robot's hand (no guarantee this will move the joints to neutral though)
g_position_neutral = Point()
# g_position_neutral.x = 0.449559195663
# g_position_neutral.y = 0.16070379419
# g_position_neutral.z = 0.212938808947
g_position_neutral.x = 0.45371551183
g_position_neutral.y = 0.0663097073071
g_position_neutral.z = 0.0271459370863
pos = Quaternion()
pos.x = 0.704238785359
pos.y = 0.709956638597
pos.z = -0.00229009932359
pos.w = 0.00201493272073
posp = Point()
posp.x = 0.45371551183
posp.y = 0.2663097073071
posp.z = 0.0401459370863
def _BroadcastOdometryInfo(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 6):
pass
try:
x = float(lineParts[1])
y = float(lineParts[2])
theta = float(lineParts[3])
vx = float(lineParts[4])
omega = float(lineParts[5])
#quaternion = tf.transformations.quaternion_about_axis(theta, (0,0,1))
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
rosNow = rospy.Time.now()
# first, we'll publish the transform over tf
self._OdometryTransformBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rosNow, "base_link", "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 = x
odometry.pose.pose.position.y = y
odometry.pose.pose.position.z = 0
odometry.pose.pose.orientation = quaternion
odometry.child_frame_id = "base_link"
odometry.twist.twist.linear.x = vx
odometry.twist.twist.linear.y = 0
odometry.twist.twist.angular.z = omega
self._OdometryPublisher.publish(odometry)
#rospy.loginfo(odometry)
except:
rospy.logwarn("Unexpected error:" + str(sys.exc_info()[0]))
def spinOnce(self):
now = rospy.Time.now()
dT = now - self.last
dT = dT.to_sec()
self.last = now
d_left = float(self.lticks - self.lastticks_l) / self.ticks_meter
d_right = float(self.rticks - self.lastticks_r) / self.ticks_meter
self.lastticks_l = self.lticks
self.lastticks_r = self.rticks
d = (d_left + d_right) / 2
th = (d_right - d_left) / self.base_width
self.dx = d / dT
self.dr = th / dT
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
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), 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 sense(self):
now = datetime.now()
elapsed = now - self.then
self.then = now
elapsed = float(elapsed.seconds) + elapsed.microseconds / 1000000.
d = self.create.d_distance / 1000.
th = self.create.d_angle * pi / 180
dx = d / elapsed
dth = th / elapsed
if (d != 0):
x = cos(th) * d
y = -sin(th) * d
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
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(), "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)
packet = SensorPacket()
for field in self.fields:
packet.__setattr__(field, self.create.__getattr__(field))
self.packetPub.publish(packet)
def update(self):
#############################################################################
now = rospy.Time.now()
if self.d_left_update and self.d_right_update and self.v_left_update and self.v_right_update:
# distance traveled is the average of the two wheels
d = (self.d_left + self.d_right) / 2
# this approximation works (in radians) for small angles
th = (self.d_right - self.d_left) / self.base_width
# calculate velocities
self.dx = (self.v_left + self.v_right) / 2
self.dr = (self.v_left - self.v_right) / self.base_width
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)
self.v_left_update = True
self.v_right_update = True
self.d_left_update = True
self.d_right_update = True
def euler_to_quaternion(yaw, pitch, roll):
#from euler angle to quaternions:
cy = cos(yaw * 0.5)
sy = sin(yaw * 0.5)
cp = cos(pitch * 0.5)
sp = sin(pitch * 0.5)
cr = cos(roll * 0.5)
sr = sin(roll * 0.5)
q = Quaternion()
q.w = cr * cp * cy + sr * sp * sy
q.x = sr * cp * cy - cr * sp * sy
q.y = cr * sp * cy + sr * cp * sy
q.z = cr * cp * sy - sr * sp * cy
return q
def get_quat(orientation):
quat = Quaternion()
if orientation is None:
quat.x = 0.0
quat.y = 0.0
quat.z = 0.0
quat.w = 1.0
elif('x' in dir(orientation)):
quat.w = orientation.w
quat.x = orientation.x
quat.y = orientation.y
quat.z = orientation.z
elif len(orientation)==4:
quat.x = orientation[0]
quat.y = orientation[1]
quat.z = orientation[2]
quat.w = orientation[3]
else:
q2 = tf.transformations.quaternion_from_euler(orientation[0],orientation[1],orientation[2])
quat.x = q2[0]
quat.y = q2[1]
quat.z = q2[2]
quat.w = q2[3]
return quat
def _set_initial_pose_of_eef(self):
# End effector orientation is fixed.
q = Quaternion()
q.x = 0.642161760993
q.y = 0.766569045326
q.z = 0.000271775440016
q.w = 0.00031241683589
point = Point()
pose = Pose()
pose.position = point
pose.orientation = q
return pose, point, q
def makeMsg(data):
pointMsg = Point()
quatMsg = Quaternion()
poseMsg = Pose()
pointMsg.x = data['position']['x']
pointMsg.y = data['position']['y']
pointMsg.z = data['position']['z']
quatMsg.x = data['rotation']['x']
quatMsg.y = data['rotation']['y']
quatMsg.z = data['rotation']['z']
quatMsg.w = data['rotation']['w']
poseMsg.position = pointMsg
poseMsg.orientation = quatMsg
return poseMsg
def heading_to_quaternion(heading):
"""
Converts a Euler yaw/heading angle to equivalent quaternion
input: euler heading in radians
output: nav_msgs.msg.Quaternion
"""
quat = tft.quaternion_from_euler(0, 0, heading)
quaternion = Quaternion()
quaternion.x = quat[0]
quaternion.y = quat[1]
quaternion.z = quat[2]
quaternion.w = quat[3]
return quaternion
def numpy_quaternion_to_icv_quaternion(numpy_quaternion):
"""
Convert a quaternion from transforms to a icv msg quaternion
:param numpy_quaternion: a numpy quaternion
:type numpy_quaternion: numpy.array
:return: a icv quaternion
:rtype: geometry_msgs.msg.Quaternion
"""
icv_quaternion = Quaternion()
icv_quaternion.x = numpy_quaternion[0]
icv_quaternion.y = numpy_quaternion[1]
icv_quaternion.z = numpy_quaternion[2]
icv_quaternion.w = numpy_quaternion[3]
return icv_quaternion
def generate_stamped_pose(x=0, y=0, quaternion=None):
if quaternion is None:
quaternion = Quaternion()
quaternion.x = 0
quaternion.y = 0
quaternion.z = 0
quaternion.w = 0
pose = Pose()
pose.position.x = x
pose.position.y = y
pose.position.z = 0
pose.orientation = quaternion
return stamp_pose(pose)
def rpy_to_quat(roll, pitch, yaw):
cy = math.cos(yaw * 0.5)
sy = math.sin(yaw * 0.5)
cp = math.cos(pitch * 0.5)
sp = math.sin(pitch * 0.5)
cr = math.cos(roll * 0.5)
sr = math.sin(roll * 0.5)
q = Quaternion()
q.w = cr * cp * cy + sr * sp * sy
q.x = sr * cp * cy - cr * sp * sy
q.y = cr * sp * cy + sr * cp * sy
q.z = cr * cp * sy - sr * sp * cy
return q
def euler_to_quaternion(yaw, pitch, roll): # yaw (Z), pitch (Y), roll (X)
cy = math.cos(yaw * 0.5)
sy = math.sin(yaw * 0.5)
cp = math.cos(pitch * 0.5)
sp = math.sin(pitch * 0.5)
cr = math.cos(roll * 0.5)
sr = math.sin(roll * 0.5)
q = Quaternion()
q.w = cy * cp * cr + sy * sp * sr
q.x = cy * cp * sr - sy * sp * cr
q.y = sy * cp * sr + cy * sp * cr
q.z = sy * cp * cr - cy * sp * sr
return q
def publishOdometry(self, now):
#############################################################################
# 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)
# TF Broadcaster
tfOdometry = TransformStamped()
tfOdometry.header.stamp = now.to_msg()
tfOdometry.header.frame_id = self.odom_frame_id
tfOdometry.child_frame_id = self.base_frame_id
tfOdometry.transform.translation.x = self.x
tfOdometry.transform.translation.y = self.y
tfOdometry.transform.translation.z = 0.0
tfOdometry.transform.rotation.x = quaternion.x
tfOdometry.transform.rotation.y = quaternion.y
tfOdometry.transform.rotation.z = quaternion.z
tfOdometry.transform.rotation.w = quaternion.w
self.odomBroadcaster.sendTransform(tfOdometry)
# Odometry
odom = Odometry()
odom.header.stamp = now.to_msg()
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.0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.getRollingMean(
self.linear_accumulator)
odom.twist.twist.linear.y = 0.0
odom.twist.twist.angular.z = self.getRollingMean(
self.angular_accumulator)
self.odom_pub.publish(odom)
def publish_odom(self, cur_x, cur_y, cur_theta, vx,vy, vth):
# quat = tf.transformations.quaternion_from_euler(0, 0, cur_theta)
roll=0
pitch =0
yaw = cur_theta
cy = cos(yaw * 0.5)
sy = sin(yaw * 0.5)
cp = cos(pitch * 0.5)
sp = sin(pitch * 0.5)
cr = cos(roll * 0.5)
sr = sin(roll * 0.5)
quat = Quaternion()
quat.w = cy * cp * cr + sy * sp * sr
quat.x = cy * cp * sr - sy * sp * cr
quat.y = sy * cp * sr + cy * sp * cr
quat.z = sy * cp * cr - cy * sp * sr
current_time = rospy.Time.now()
# br = tf.TransformBroadcaster()
# br.sendTransform((cur_x, cur_y, 0),
# tf.transformations.quaternion_from_euler(0, 0, -cur_theta),
# current_time,
# "summit_base_footprint",
# "odom")
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = 'odom'
odom.pose.pose.position.x = cur_x
odom.pose.pose.position.y = cur_y
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation = quat
odom.pose.covariance[0] = 0.01
odom.pose.covariance[7] = 0.01
odom.pose.covariance[14] = 99999
odom.pose.covariance[21] = 99999
odom.pose.covariance[28] = 99999
odom.pose.covariance[35] = 0.01
# odom.child_frame_id = 'summit_base_footprint'
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = vy
odom.twist.twist.angular.z = vth
odom.twist.covariance = odom.pose.covariance
self.odom_pub.publish(odom)
def toQuaternion(yaw):
pitch = 0.0
roll = 0.0
cy = np.cos(yaw * 0.5)
sy = np.sin(yaw * 0.5)
cp = np.cos(pitch * 0.5)
sp = np.sin(pitch * 0.5)
cr = np.cos(roll * 0.5)
sr = np.sin(roll * 0.5)
q = Quaternion()
q.w = cy * cp * cr + sy * sp * sr
q.x = cy * cp * sr - sy * sp * cr
q.y = sy * cp * sr + cy * sp * cr
q.z = sy * cp * cr - cy * sp * sr
return q
def ToQuat(roll, pitch, yaw):
cr = cos(roll * 0.5)
sr = sin(roll * 0.5)
cp = cos(pitch * 0.5)
sp = sin(pitch * 0.5)
cy = cos(yaw * 0.5)
sy = sin(yaw * 0.5)
q = Quaternion()
q.w = cr * cp * cy + sr * sp * sy
q.x = sr * cp * cy - cr * sp * sy
q.y = cr * sp * cy + sr * cp * sy
q.z = cr * cp * sy - sr * sp * cy
return q
def angle_to_quaternion(self, angle):
"""
Method to translate angles in degrees over Y axis to quaternions
:param angle (int) degrees wrt Y axis
:return geometry_msgs/Quaternion
"""
pyq = pyquaternion.Quaternion(
axis=[0.0, 0.0,
1.0], degrees=angle) # Rotation in degrees over Z axis
q = Quaternion()
q.x = pyq.x
q.y = pyq.y
q.z = pyq.z
q.w = pyq.w
return q
def QuaternionGenerater(self, end, start):
theta = self.angle_generater(end, start)
quat = Quaternion()
ax = 0
ay = 0
az = 1 #绕z轴旋转
quat.x = ax * numpy.sin(theta / 2)
quat.y = ay * numpy.sin(theta / 2)
quat.z = az * numpy.sin(theta / 2)
quat.w = numpy.cos(theta / 2)
#or try this:
#import maplib
#(quat.x, quat.y, quat.z, quat.w) = maplib.angle_to_quat(theta)
return quat
def eul2quaternion(rpy):
sr = math.sin(rpy.x/2)
cr = math.cos(rpy.x/2)
sp = math.sin(rpy.y/2)
cp = math.cos(rpy.y/2)
sy = math.sin(rpy.z/2)
cy = math.cos(rpy.z/2)
q = Quaternion()
q.w = cr*cp*cy + sr*sp*sy
q.x = sr*cp*cy - cr*sp*sy
q.y = cr*sp*cy + sr*cp*sy
q.z = cr*cp*sy - sr*sp*cy
return q
def quaternion_from_euler(roll, pitch, yaw):
cy = np.cos(yaw * 0.5)
sy = np.sin(yaw * 0.5)
cp = np.cos(pitch * 0.5)
sp = np.sin(pitch * 0.5)
cr = np.cos(roll * 0.5)
sr = np.sin(roll * 0.5)
q = Quaternion()
# print(q)
q.w = cr * cp * cy + sr * sp * sy
q.x = sr * cp * cy - cr * sp * sy
q.y = cr * sp * cy + sr * cp * sy
q.z = cr * cp * sy - sr * sp * cy
return q
def QuaternionMsgFromEuler(roll, pitch, yaw):
q = Quaternion()
cosRoll = math.cos(roll)
sinRoll = math.sin(roll)
cosPit = math.cos(pitch)
sinPit = math.sin(pitch)
cosYaw = math.cos(yaw)
sinYaw = math.sin(yaw)
q.w = cosRoll * cosPit * cosYaw + sinRoll * sinPit * sinYaw
q.x = sinRoll * cosPit * cosYaw - cosRoll * sinPit * sinYaw
q.y = cosRoll * sinPit * cosPit + sinRoll * cosPit * sinYaw
q.z = cosRoll * cosPit * sinYaw - sinRoll * sinPit * cosYaw
return q
def computeQuaternion(x0, y0, x1, y1):
#rospy.loginfo("x0:"+str(x0)+",y0"+str(y0)+" ,x1:"+str(x1)+",y1"+str(y1))
y = np.arctan2(float(y1 - y0), float(x1 - x0))
#y=y+math.pi
y = y
p = 0
r = 0
#print '[x0:'+str(x0)+',y0:'+str(y0)+'] ' + ' [x1:'+str(x1)+',y1:'+str(y1)+'] '
#print 'angle: '+str(180*y/math.pi)
quat = tf.transformations.quaternion_from_euler(r, p, y)
quaternion = Quaternion()
quaternion.x = quat[0]
quaternion.y = quat[1]
quaternion.z = quat[2]
quaternion.w = quat[3]
return quaternion
def getEulerToQuat(roll=0., pitch=0., yaw=0.):
"""
Transform euler angels to quaternion
:param roll:
:param pitch:
:param yaw:
:return: quaternion
"""
# type(pose) = geometry_msgs.msg.Pose
q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
quat = Quaternion()
quat.x = q[0]
quat.y = q[1]
quat.z = q[2]
quat.w = q[3]
return quat
