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)