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