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 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 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 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 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 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 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 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 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 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 getOrientation(self):
ort = Quaternion()
ort.x = self._orientationX
ort.y = self._orientationY
ort.z = self._orientationZ
ort.w = self._orientationW
return ort
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 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 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 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 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 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 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 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 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 _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 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_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 odomUpdate(self, timer):
ddist = self.create.getSensor('DISTANCE') / 1000.0
dth = self.create.getSensor('ANGLE') * pi / 180.0
self.dist += ddist
self.theta += dth
if (ddist != 0):
dx = cos(dth) * ddist
dy = -sin(dth) * ddist
self.x += (cos(self.theta) * dx - sin(self.theta) * dy)
self.y += (sin(self.theta) * dx + cos(self.theta) * dy)
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.theta / 2)
quaternion.w = cos(self.theta / 2)
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.baseFrame, self.odomFrame)
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = self.odomFrame
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.baseFrame
if timer.last_duration:
odom.twist.twist.linear.x = ddist / timer.last_duration
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = dth / timer.last_duration
self.odomPub.publish(odom)
def main(args):
rospy.init_node('gazebo', anonymous = True)
goal_publisher = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size = 10)
rospy.sleep(1)
header= Header()
header.seq = 1
header.stamp = rospy.Time.now()
header.frame_id = 'map'
check_start(args)
point = Point()
point.x = float(args.x_goal[0])
point.y = float(args.y_goal[0])
point.z = 0.0
quat = Quaternion()
quat.x = float(args.x_goal[0])
quat.y = float(args.y_goal[0])
quat.z = 90
quat.w = float(args.theta_goal[0])
pose = Pose()
pose.position = point
pose.orientation = quat
posestamped = PoseStamped()
posestamped.header = header
posestamped.pose = pose
print(posestamped)
prev_x = args.x_goal
prev_y = args.y_goal
prev_theta = args.theta_goal
while not rospy.is_shutdown():
goal_publisher.publish(posestamped)
def add(self, position=None, orientation=None):
if orientation is not None:
q = Quaternion()
q.z = sin(orientation / 2)
q.w = cos(orientation / 2)
orientation = {'x': q.x, 'y': q.y, 'z': q.z, 'w': q.w}
if position is not None:
position = {'x': position[0], 'y': position[1], 'z': 0}
if position is None:
position = self.__waypoints['poses'][-1]['pose']['position']
if orientation is None:
orientation = self.__waypoints['poses'][-1]['pose']['orientation']
self.__waypoints['poses'].append({
'header': {
'frame_id': self.__frame_id
},
'pose': {
'orientation': orientation,
'position': position
}
})
def create_marker(name, pose):
global marker_server
# Create an interactive marker
int_marker = InteractiveMarker()
int_marker.header.frame_id = 'map'
int_marker.header.stamp = rospy.Time.now()
int_marker.name = name
int_marker_pose = pose
int_marker_pose.position.z = 0.1
int_marker.pose = int_marker_pose
# Create controls for the interactive marker
### Box control
arrow_marker = Marker()
arrow_marker.type = Marker.ARROW
arrow_marker.scale.x = 0.7
arrow_marker.scale.y = 0.2
arrow_marker.scale.z = 0.2
arrow_marker.color.r = 0.5
arrow_marker.color.g = 0.5
arrow_marker.color.b = 0.5
arrow_marker.color.a = 1.0
arrow_control = InteractiveMarkerControl()
arrow_control.always_visible = True
arrow_control.markers.append(arrow_marker)
### Rotate control
rotate_control = InteractiveMarkerControl()
rotate_control.name = "move_rotate"
rotate_control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
orien = Quaternion()
orien.x = 0
orien.y = 1
orien.z = 0
orien.w = 1
rotate_control.orientation = orien
# Add the control to the interactive marker
int_marker.controls.append(arrow_control)
int_marker.controls.append(rotate_control)
marker_server.insert(int_marker, handle_viz_input)
marker_server.applyChanges()
def poll(self):
(x, y, theta) = self.arduino.bupigo_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.bupigo_set_velocity(self.forwardSpeed, self.angularSpeed)
self.odomPub.publish(odom)
blobs = self.arduino.get_blobs()
blobs.header.stamp = rospy.Time.now()
self.blob_publisher.publish(blobs)
def publish_odom(self, odom):
# get motor encoder values
left, right = self.robot.getMotors()
d_left = (left - self.encoders[0]) / 1000.0
d_right = (right - self.encoders[1]) / 1000.0
self.encoders = [left, right]
dx = (d_left + d_right) / 2
dth = (d_right - d_left) / (self.robot.base_width / 1000.0)
x = cos(dth) * dx
y = -sin(dth) * dx
self.x += cos(self.th) * x - sin(self.th) * y
self.y += sin(self.th) * x + cos(self.th) * y
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)
dt = (odom.header.stamp - rospy.Time.now()).secs
# prepare odometry
odom.header.stamp = rospy.Time.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 = dx / dt
odom.twist.twist.angular.z = dth / dt
# publish everything
self.odomPub.publish(odom)
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), "base_link", "odom")
def update_odometry(self, event):
"""
Gets the current odometry from the robot and publish the ROS transform
"""
# send command
self.cmd.send("cmd_get_odometry")
# receive response from Arduino
data = self.cmd.receive()
x = data[1][0]
y = data[1][1]
theta = data[1][2]
now = rospy.Time.now()
# publish the odom information
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = math.sin(theta / 2)
quaternion.w = math.cos(theta / 2)
self.odom_broadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w), now,
self.base_frame, self.odom_frame)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame
odom.twist.twist.linear.x = 0 # linear velocity
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = 0 # angular velocity
self.odom_pub.publish(odom)
def mag_to_quat(imu_raw):
msg_out = Imu()
msg_out.header.stamp=rospy.Time.now()
yaw = float(imu_raw[1])
pitch = float(imu_raw[2])
roll = float(imu_raw[3])
accx = float(imu_raw[7])
accy = float(imu_raw[8])
accz = float(imu_raw[9])
gyrox = float(imu_raw[10])
gyroy = float(imu_raw[11])
gyroz = float(imu_raw[12])
orientation = Quaternion()
quat = quaternion_from_euler(roll,pitch,yaw)
orientation.x = quat[0]
orientation.y = quat[1]
orientation.z = quat[2]
orientation.w = quat[3]
angular_velocity = Vector3()
angular_velocity.x = gyrox
angular_velocity.y = gyroy
angular_velocity.z = gyroz
linear_acceleration = Vector3()
linear_acceleration.x = accx
linear_acceleration.y = accy
linear_acceleration.z = accz
msg_out.orientation = orientation
msg_out.angular_velocity = angular_velocity
msg_out.linear_acceleration = linear_acceleration
return msg_out
def move_pos(goal, limb, timeout=3):
# Call Baxter's inbuilt service
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
# Request for same service
ikreq = SolvePositionIKRequest()
# Fill in all message parts
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
# Define required x, y, z parts
pose = Pose()
# Define orientation of gripper.
quat = Quaternion()
quat.x = goal[3]
quat.y = goal[4]
quat.z = goal[5]
quat.w = goal[6]
# Fill the position vectors from goal configuration
pose.orientation = quat
pose.position.x = goal[0]
pose.position.y = goal[1]
pose.position.z = goal[2]
ikreq.pose_stamp = [PoseStamped(header=hdr, pose=pose)]
# Using Random seeds till a "VALID" IK solution is found
# FOR SEED METHOD:
# cite1: IK_Service_Client in package: Baxter_Examples
# cite2: test_moveit.py from NXR repo linked in Course notes
try:
rospy.wait_for_service(ns, 3.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.loginfo("Service exception")
def main():
global go
rospy.init_node('Global_Planner', anonymous=True)
rospy.Subscriber("/move_base/global_costmap/costmap", OccupancyGrid,
mapcallback)
rospy.Subscriber('/map_metadata', MapMetaData, metaCallback)
rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped, poseCallback)
rospy.Subscriber("move_base_simple/goal", PoseStamped, goalCallback)
pathPub = rospy.Publisher("/plan", Path, queue_size=1)
rate = rospy.Rate(100)
while not rospy.is_shutdown():
if go:
go = False
path, cost = AStarSearch((currentLocation[0], currentLocation[1]),
(goalLocation[0], goalLocation[1]))
newPath = []
for i in range(len(path) - 1):
p1 = toPose(path[i])
p2 = toPose(path[i + 1])
theta = calculateAngle(p1.pose.position, p2.pose.position)
q = quaternion_from_euler(0, 0, theta)
quat = Quaternion()
quat.x = q[0]
quat.y = q[1]
quat.z = q[2]
quat.w = q[3]
p1.pose.orientation = quat
newPath.append(p1)
plan = Path()
plan.poses = newPath
header = Header()
header.frame_id = "map"
plan.header = header
pathPub.publish(plan)
print("published path")
display(path, cost)
rate.sleep()
def launchGraspJobs():
state, target = database.getNextTarget()
# print database.states
print state, target
# print type(state)
# print isinstance(state,Recognized)
if isinstance(state, Recognized): #go grab
print "publishing to move: ",
#pub_move.publish(x)
binName = target[1] #char 'a'
#binName = ord(binName) - ord('A')
#if binName%3 < 1:
# arm = 1 #Left ARM?
#else:
# arm = 2 #Right ARM?
point = Point()
point.x = state.centroid_x
point.y = state.centroid_y
point.z = state.centroid_z
pose = Pose()
pose.position = point
q = Quaternion()
q.x = 0.707
q.y = 0
q.z = 0.707
q.w = 0
pose.orientation = q
arm2d, armMove = choose_arm(binName)
if state.is3d:
print 'ere really'
sys.stdout.flush()
controller.move_to_pick(binName, armMove, pose)
else:
controller.move_along_line(binName, armMove, point)
database.resetScores()
launchRecogJobs(0)
else: #Move to bin for picture
launchRecogJobs(state)
def publish_robot_pose(self):
"""
Publisher for pose, sends a message containing
Point: x, y, z positions representing the position of the robot, in millimeters
Quaternion: x, y, z, w values representing the orientation of the robot
"""
pose = Pose()
point = Point()
quat = Quaternion()
point.x = self.cozmo.pose.position.x
point.y = self.cozmo.pose.position.y
point.z = self.cozmo.pose.position.z
quat.x = self.cozmo.pose.rotation.q0
quat.y = self.cozmo.pose.rotation.q1
quat.z = self.cozmo.pose.rotation.q2
quat.w = self.cozmo.pose.rotation.q3
pose.position = point
pose.orientation = quat
self.pose_publisher.publish(pose)
def compute_orientation_from_diff(self, last_orientation, d_theta):
""" Add a given angle to an orientation
Parameters:
- last_orientation: last orientation (geometry_msgs/Quaternion)
- d_theta: the difference of orientation between the last and the new odom
Return:
- new_orientation: the new orientation (geometry_msgs/Quaternion)
"""
last_theta = tf.transformations.euler_from_quaternion([
last_orientation.x, last_orientation.y, last_orientation.z,
last_orientation.w
])[2]
new_theta = last_theta + d_theta
quaternion = tf.transformations.quaternion_from_euler(
0.0, 0.0, new_theta)
new_orientation = Quaternion()
new_orientation.x = quaternion[0]
new_orientation.y = quaternion[1]
new_orientation.z = quaternion[2]
new_orientation.w = quaternion[3]
return new_orientation
def CreateImuMessage(orientation, linear_acceleration, angular_velocity,
imu_time, sequence):
msg_av = Vector3()
msg_la = Vector3()
msg_quat = Quaternion()
msg_imu = Imu()
msg_imu.header.stamp = imu_time
msg_imu.header.frame_id = "body_frame"
msg_imu.header.seq = sequence
msg_quat.x = orientation.x_val
msg_quat.y = orientation.y_val
msg_quat.z = orientation.z_val
msg_quat.w = orientation.w_val
msg_imu.orientation = msg_quat
msg_av.x = angular_velocity.x_val
msg_av.y = angular_velocity.y_val
msg_av.z = angular_velocity.z_val
msg_imu.angular_velocity = msg_av
msg_la.x = linear_acceleration.x_val
msg_la.y = linear_acceleration.y_val
msg_la.z = linear_acceleration.z_val
msg_imu.linear_acceleration = msg_la
return msg_imu
def publish_odometry(self, translation, quaternion, child, parent):
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = parent
odom.child_frame_id = child
odom.header.seq = self.seq
odom.pose.pose.position.x = translation[0]
odom.pose.pose.position.y = translation[1]
odom.pose.pose.position.z = translation[2]
quat = Quaternion()
quat.x = quaternion[0]
quat.y = quaternion[1]
quat.z = quaternion[2]
quat.w = quaternion[3]
odom.pose.pose.orientation = quat
self.pub_odom.publish(odom)
self.seq += 1
def create_odometry_message(world_x, world_y, world_theta, world_x_linear_v,
world_y_linear_v, world_z_angular_v, odom_time,
base_frame_id, world_frame_id):
# Convert world orientation (theta) to a Quaternion for use with tf and Odometry
quat_vals = tf.transformations.quaternion_from_euler(0, 0, world_theta)
quat = Quaternion()
quat.x = quat_vals[0]
quat.y = quat_vals[1]
quat.z = quat_vals[2]
quat.w = quat_vals[3]
odom = Odometry()
odom.header.stamp = odom_time
odom.header.frame_id = world_frame_id
odom.pose.pose.position.x = world_x
odom.pose.pose.position.y = world_y
odom.pose.pose.position.z = 0.0 # Because this robot can't fly to a vertical position
odom.pose.pose.orientation = quat
odom.child_frame_id = base_frame_id
odom.twist.twist.linear.x = world_x_linear_v
odom.twist.twist.linear.y = world_y_linear_v
odom.twist.twist.angular.z = world_z_angular_v
return odom
def diff(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]
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
def drawBox(self, contours):
color = (255, 0, 0)
scale = 3
a, b, c, maxh = cv2.boundingRect(contours[0])
maxcontour = contours[0]
for contour in contours:
#print contour
x, y, w, h = cv2.boundingRect(contour)
if h > maxh:
maxh = h
maxcontour = contour
xf, yf, wf, hf = cv2.boundingRect(maxcontour)
cv2.rectangle(self.original, (xf, yf), (xf + wf, yf + hf), color,
scale)
self.boxparam = [xf, yf, wf, hf]
rec_msg = Quaternion()
rec_msg.x = xf
rec_msg.y = yf
rec_msg.z = wf
rec_msg.w = hf
self.rec_cmd.publish(rec_msg)
def move_base_to(self, x, y, theta):
'''
Description: sends a position goal to the ROS node to move the robot
base to
Input: self <Object>, x <Int>, y <Int>, theta <Float>
Return: None
'''
goal = MoveBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = "map"
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
quat = transformations.quaternion_from_euler(0, 0, theta)
orientation = Quaternion()
orientation.x = quat[0]
orientation.y = quat[1]
orientation.z = quat[2]
orientation.w = quat[3]
goal.target_pose.pose.orientation = orientation
# send goal
self.base_client.send_goal(goal)
def pub_tf(self):
global x, y, th, pos_x, pos_y, ori_th, v_pos_x, v_pos_y, v_ori_th
broadcaster = tf2_ros.TransformBroadcaster()
robot_quat = Quaternion()
q = quaternion_from_euler(0, 0, math.radians(ori_th))
robot_quat.x = q[0]
robot_quat.y = q[1]
robot_quat.z = q[2]
robot_quat.w = q[3]
ts = TransformStamped()
ts.header.frame_id = "map"
ts.child_frame_id = "base_footprint"
ts.header.stamp = self.get_clock().now()
ts.transform.translation.x = var_pos_x / 1000
ts.transform.translation.y = var_pos_y / 1000
ts.transform.translation.z = 0
ts.transform.rotation = robot_quat
#ifdef VICON
ts.transform.rotation = v_rotation
#endif
broadcaster.sendTransform(ts)
def GoToPose(self, x = 3.6, y = 0, qx = 0, qy = 0, qz = 0, qw = 1):
print('GoToPose start')
msg = PoseStamped()
msg.header.frame_id = 'map'
# msg.header.stamp = self.get_clock().now()
point = Point()
point.x = float(x)
point.y = float(y)
quaternion = Quaternion()
quaternion.x = float(qx)
quaternion.y = float(qy)
quaternion.z = float(qx)
quaternion.w = float(qw)
print(point, quaternion)
pose = Pose()
pose.position = point
pose.orientation = quaternion
msg.pose = pose
self.publisher_gotopose.publish(msg)
def get_xy_based_on_lat_long(msg, distance_pub):
#d_lat = 21.167190773505162
#d_long = 72.78580315411091
p = gps_point()
p.lat = msg.latitude
p.lon = msg.longitude
xg2, yg2 = ll2xy(p.lat, p.lon, d_lat, d_lon) #ll2xy???
utmy, utmx, utmzone = LLtoUTM(p.lat, p.lon)
xa, ya = ll2xy(p.lat, p.lon, d_lat, d_lon) #ll2xy???
rospy.loginfo(p.name)
'''rospy.loginfo("LAT COORDINATES ==>"+str(p.lat)+","+str(p.lon))
rospy.loginfo("COORDINATES XYZ==>"+str(xg2)+","+str(yg2))
rospy.loginfo("COORDINATES AXY==>"+str(xa)+","+str(ya))
rospy.loginfo("COORDINATES UTM==>"+str(utmx)+","+str(utmy))'''
quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0,
p.theta) #incomplete
pose = Pose()
pose.position.x = xg2
pose.position.y = yg2
q = Quaternion()
q.x = quaternion[0]
q.y = quaternion[1]
q.z = quaternion[2]
q.w = quaternion[3]
pose.orientation = q
distance_pub.publish(pose)
def createTranslationFootStepOffset(self, step_side, offset):
footstep = self.createFootStepInPlace(step_side)
# transform the offset to world frame
#quat = footstep.orientation
#Get Pelvis orientation as we want to be with respect to the pelvis
pelvis_world = self.tfBuffer.lookup_transform('world',
self.PELVIS_FRAME_NAME,
rospy.Time())
quat_pelvis_world = pelvis_world.transform.rotation
quat_pelvis = numpy.array([
quat_pelvis_world.w, quat_pelvis_world.x, quat_pelvis_world.y,
quat_pelvis_world.z
])
quat_to_use = self.get_pelvis_xy_coplanar_quat(quat_pelvis)
quat = Quaternion()
quat.w = quat_to_use[0]
quat.x = quat_to_use[1]
quat.y = quat_to_use[2]
quat.z = quat_to_use[3]
rot = quaternion_matrix([quat.x, quat.y, quat.z, quat.w])
transformedOffset = numpy.dot(rot[0:3, 0:3], offset)
footstep.location.x += transformedOffset[0]
footstep.location.y += transformedOffset[1]
if (ON_REAL_ROBOT_USE):
footstep.location.z = 0.0 #+= transformedOffset[2]
else:
footstep.location.z = -SOLE_FRAME_Z_OFFSET #+= transformedOffset[2]
#print "(left, right)", self.LEFT_FOOT_FRAME_NAME, self.RIGHT_FOOT_FRAME_NAME
#print "(x,y,z)", footstep.location.x, footstep.location.y, footstep.location.z
return footstep
def handle_init_ahrs(req):
# def computeVals():
global sum_accel, sum_gyro, num_data
# estimate gravity vector
g_b = sum_accel / num_data
# compute initial roll (phi) and pitch (theta)
phi = math.atan2(-g_b[1], -g_b[2])
theta = math.atan2(g_b[0], math.sqrt(g_b[1]**2 + g_b[2]**2))
# set yaw (psi) as zero since no absolute heading available
psi = 0
# q is navigation to body transformation: R_bi
# YPR: R_ib = R(yaw)R(pitch)R(Roll)
# RPY: R_bi = R(-Roll)R(-Pitch)R(-yaw)
q = quaternion_from_euler(-phi, -theta, -psi, 'sxyz')
quat = Quaternion()
quat.x = q[0]
quat.y = q[1]
quat.z = q[2]
quat.w = q[3] # tf convention
# compute gyroscope biases
gyro_avg = sum_gyro / num_data
gyro_biases = [
Float64(gyro_avg[0]),
Float64(gyro_avg[1]),
Float64(gyro_avg[2])
]
print("ahrs_initialization_server: Initial YPR: ", psi, theta, phi)
print("ahrs_initialization_server: Gyro Biases ", gyro_biases)
return initRequestResponse(quat, gyro_biases)
def talker():
pub1 = rospy.Publisher('chatter', Point, queue_size=10)
pub2 = rospy.Publisher('chatter1', Vector3, queue_size=10)
pub3 = rospy.Publisher('chatter2', String, queue_size=10)
pub4 = rospy.Publisher('chatter3', Quaternion, queue_size=10)
pub5 = rospy.Publisher('chatter4', Pose2D, queue_size=10)
rospy.init_node('publisher')
rate = rospy.Rate(10)
msg1 = Point()
msg1.x = 5
msg1.y = 6
msg1.z = 7
msg2 = Vector3()
msg2.x = 1
msg2.y = 3
msg2.z = 0
msg3 = String()
msg3.data = "hello_World"
msg4 = Quaternion()
msg4.x = 8
msg4.y = 9
msg4.z = 10
msg4.w = 11
msg5 = Pose2D()
msg5.x = 1
msg5.y = 2
msg5.theta = 80
while not rospy.is_shutdown():
pub1.publish(msg1)
pub2.publish(msg2)
pub3.publish(msg3)
pub4.publish(msg4)
pub5.publish(msg5)
rate.sleep()
def get_relative(self, odometry):
pose = odometry.pose
x = pose.pose.position.x
y = pose.pose.position.y
orientation = pose.pose.orientation
quaternion = PyKDL.Rotation.Quaternion(orientation.x, orientation.y,
orientation.z, orientation.w)
if not self.initial_quaternion:
self.initial_quaternion = quaternion
self.initial_x = x
self.initial_y = x
else:
relative_x = x - self.initial_x
relative_y = y - self.initial_y
relative_quaternion = self.initial_quaternion * quaternion.Inverse(
)
relative_quaternion = relative_quaternion.GetQuaternion()
odometry = Odometry()
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "odom"
odometry.header = header
odometry.child_frame_id = "base_link"
odometry.pose.pose.position.x = relative_x
odometry.pose.pose.position.y = relative_y
orientation = Quaternion()
orientation.x = relative_quaternion[0]
orientation.y = relative_quaternion[1]
orientation.z = relative_quaternion[2]
orientation.w = relative_quaternion[3]
odometry.pose.pose.orientation = orientation
self.relative_pub.publish(odometry)
def update(self, Z):
"""
Make the robot turn towards the swarm.
@param Z State of the robot within the swarm of type SwarmStateData.
"""
if not Z.pose_and_target_valid:
rospy.logwarn_throttle(
10, "Pose and/or target are invalid... Perhaps you haven't" +
" set a target?")
return IdleState()
# Calculate the required angle and update velocity
req_ang = math.atan2(
Z.swarm_pose.pose.position.y - Z.robot_data.mTb.position.y,
Z.swarm_pose.pose.position.x - Z.robot_data.mTb.position.x)
req_quat = Quaternion()
req_quat.w = math.cos(req_ang / 2)
req_quat.z = math.sin(req_ang / 2)
self.update_cmd_vel(Z.robot_data, req_quat)
# Update the state
r = random.random()
p_idle = 0.01
if r < p_idle:
return IdleState()
r -= p_idle
p_toswarm = 1 - abs(
calc_ang_diff(Z.robot_data.mTb.orientation, req_quat) /
self._ANG_MAX_TOL)
if r < p_toswarm:
return MoveToSwarmState()
return self
def move_pos(des_pose):
limb = "right"
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
# quat = quaternion_from_euler(des_pose[3],des_pose[4],des_pose[5])
pose = Pose()
quat = Quaternion()
quat.x = des_pose[3]
quat.y = des_pose[4]
quat.z = des_pose[5]
quat.w = des_pose[6]
pose.orientation = quat
pose.position.x = des_pose[0]
pose.position.y = des_pose[1]
pose.position.z = des_pose[2]
ikreq.pose_stamp = [PoseStamped(header=hdr, pose=pose)]
# print ikreq
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.loginfo("Service exception")
def process_data(self):
pose_candidate = None
reachable_pose_found = False
pose_candidate_idx = 0
while not reachable_pose_found and pose_candidate_idx <= len(self.pose_candidates):
candidate = self.pose_candidates[pose_candidate_idx]
pose = PoseStamped()
pose.header.frame_id = candidate.frame_id
pose.pose.position.x = candidate.position.x
pose.pose.position.y = candidate.position.y
pose.pose.position.y = candidate.position.z
quaternion = quaternion_from_euler(candidate.orientation.x,
candidate.orientation.y,
candidate.orientation.z)
quaternion_msg = Quaternion()
quaternion_msg.x = quaternion[0]
quaternion_msg.y = quaternion[1]
quaternion_msg.z = quaternion[2]
quaternion_msg.w = quaternion[3]
pose.pose.orientation = quaternion_msg
self.arm.set_pose_reference_frame(self.manipulated_object.pose.frame_id)
self.arm.clear_pose_targets()
self.arm.set_pose_target(pose.pose)
plan = self.arm.plan()
if len(plan.joint_trajectory.points) > 0:
reachable_pose_found = True
pose_candidate = candidate
else:
pose_candidate_idx += 1
return {'pose_candidate': pose_candidate, 'pose_candidate_idx': pose_candidate_idx}
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 = (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
if (dt > 0):
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)
