def handle_convert(data): pub = rospy.Publisher('/navigation_vel', TwistStamped) m = TwistStamped() m.twist = data m.header = Header() m.header.stamp = rospy.Time.now() pub.publish(m)
def set_position(): xc, yc = pyautogui.position() pos = TwistStamped() pos.header = Header() #print("xc is " + str(xc)) if (xc < 680 and yc < 380): pos.twist.linear.z = 0.0 pos.twist.linear.x = 1 pos.twist.linear.y = 1 print("forward_left") elif (xc < 680 and yc > 380): pos.twist.linear.z = 0.0 pos.twist.linear.x = 1 pos.twist.linear.y = -1 print("forward_right") elif (xc > 680 and yc < 380): pos.twist.linear.z = 0.0 pos.twist.linear.x = -1 pos.twist.linear.y = -1 print("backward_left") elif (xc > 680 and yc > 380): pos.twist.linear.z = 0.0 pos.twist.linear.x = -1 pos.twist.linear.y = 1 print("backward_right") else: pos.twist.linear.z = 0 pos.twist.linear.x = 0 pos.twist.linear.y = 0 print("entered default") return pos
def pose_callback(data): # extract x and y position information x = data.pose.pose.position.x y = data.pose.pose.position.y # package and broadcast p = Point() p.x = x p.y = y # send it pub_beacon.publish(p) # and broadbast all odoms on a common channel pub_odom_all.publish(data) # publish pose info for viewing pose_out = PoseStamped() pose_out.header = data.header pose_out.header.frame_id = '/world' pose_out.pose = data.pose.pose pub_pose.publish(pose_out) # and twist stamped for DMS interface vel_out = TwistStamped() vel_out.header = data.header vel_out.header.frame_id = '/world' vel_out.twist = data.twist.twist pub_vel.publish(vel_out) # transform tf_broadcast.sendTransform( (x, y, 0.0), (data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w), rospy.Time.now(), frame_name, "/world")
def navigate(self): rate = self.rospy.Rate(10) # 10hz msg = TwistStamped() msg.header = Header() msg.header.frame_id = "base_footprint" msg.header.stamp = rospy.Time.now() while 1: print "Time since last vel:", (time.time() - self.lastVelTime) if (self.inCollision or (time.time() - self.lastVelTime) > 1.0): # print "inCollision" msg.twist.linear.x = 0 #self.x msg.twist.linear.y = 0 #self.y msg.twist.linear.z = 0 #self.z msg.twist.angular.z = 0 else: msg.twist.linear.x = self.x msg.twist.linear.y = self.y msg.twist.linear.z = self.z msg.twist.angular.z = self.yaw print "Sending vel" #print "collision:", self.inCollision # For demo purposes we will lock yaw/heading to north. #yaw_degrees = 0 # North #yaw = radians(yaw_degrees) #quaternion = quaternion_from_euler(0, 0, yaw) #msg.pose.orientation = Quaternion(*quaternion) self.pub.publish(msg) rate.sleep()
def velocity_loop(self): """Loops in Control_Mode.VELOCITY until either the mode is switched or the set duration has been met.""" # Record start time for timeout tracking start_time = rospy.get_time() # Build message msg = TwistStamped() msg.twist.linear.x = self._target_velocity[0] msg.twist.linear.y = self._target_velocity[1] msg.twist.linear.z = self._target_velocity[2] while self.is_running() and self._mode == Control_Mode.VELOCITY: # Break if timeout is enabled and has been surpassed if self._travel_time > 0 and (rospy.get_time() - start_time) > self._travel_time: break # Update message header msg.header = Header(stamp=rospy.get_rostime()) # Publish message self._vel_pub.publish(msg) # Sleep to maintain appropriate update frequency self._rate.sleep() # Only perform if controller is still running if self.is_running(): # Set mode to HOLD self.set_mode(Control_Mode.INITIAL) # Reset velocity target self.set_velocity((0.0, 0.0, 0.0))
def new_measurement(self, data): '''Processes incoming measurement from Localization. data: meas_world: PoseStamped yaw: float32 ''' self.pc.pose_raw = data.meas_world self.pc.yaw = data.yaw self.measurement_valid = self.pc.measurement_check() measurement = self.kalman.transform_pose(self.pc.pose_raw, "world", "world_rot") if self.measurement_valid: if self.kalman.init: self.wm.yhat_r_t0.header = measurement.header zero_input_cmd = TwistStamped() zero_input_cmd.header = measurement.header self.kalman.input_cmd_list = [zero_input_cmd] self.kalman.init = False # Apply correction step. self.wm.yhat_r, self.wm.yhat_r_t0 = self.kalman.kalman_pos_correct( measurement, self.wm.yhat_r_t0) self.wm.yhat = self.transform_point(self.wm.yhat_r, "world_rot", "world") self.pose_pub.publish(self.wm.yhat)
def onTwist(self,msg): newMsg = TwistStamped() head = std_msgs.msg.Header() newMsg.twist = msg head.stamp = rospy.Time.now() newMsg.header = head self.twist_pub.publish(newMsg)
def veloCallback(self,velo): self.velocity[0] = velo.twist.linear.x self.velocity[1] = velo.twist.linear.y self.velocity[2] = velo.twist.linear.z randx = random.uniform(-1.,1.) * 0.1*0 randy = random.uniform(-1.,1.) * 0.1*0 randz = random.uniform(-1.,1.) * 0.1*0 randdistance = np.linalg.norm(np.array([randx, randy, randz])) #print("RandomErrors Velo:", randx, randy, randz, randdistance) self.velocity_rot = self.q_rot.rotate(np.array([velo.twist.linear.x+randx, velo.twist.linear.y+randy, velo.twist.linear.z+randz])) #print("Velocity", self.velocity) self.veloMedianX = np.roll(self.veloMedianX, 1) self.veloMedianY = np.roll(self.veloMedianY, 1) self.veloMedianZ = np.roll(self.veloMedianZ, 1) self.veloMedianX[0] = self.velocity_rot[0] self.veloMedianY[1] = self.velocity_rot[1] self.veloMedianZ[2] = self.velocity_rot[2] self.filteredVeloValue[0] = (np.sum(self.veloMedianX) / len(self.veloMedianX)) self.filteredVeloValue[1] = (np.sum(self.veloMedianY) / len(self.veloMedianY)) self.filteredVeloValue[2] = (np.sum(self.veloMedianZ) / len(self.veloMedianZ)) veloNED =TwistStamped() veloNED.header = velo.header #TIMESTAMP? veloNED.header.frame_id = 'global_tank' veloNED.twist.linear.x = self.filteredVeloValue[0] veloNED.twist.linear.y = self.filteredVeloValue[1] veloNED.twist.linear.z = self.filteredVeloValue[2] self.velocity_publish.publish(veloNED)
def navigate(self): rate = self.rospy.Rate(10) # 10hz msg = TwistStamped() msg.header = Header() msg.header.frame_id = "base_footprint" msg.header.stamp = rospy.Time.now() while 1: print "Time since last vel:", (time.time() - self.lastVelTime) if (self.inCollision or (time.time() - self.lastVelTime)>1.0): # print "inCollision" msg.twist.linear.x = 0 #self.x msg.twist.linear.y = 0 #self.y msg.twist.linear.z = 0 #self.z msg.twist.angular.z = 0 else: msg.twist.linear.x = self.x msg.twist.linear.y = self.y msg.twist.linear.z = self.z msg.twist.angular.z = self.yaw print "Sending vel" #print "collision:", self.inCollision # For demo purposes we will lock yaw/heading to north. #yaw_degrees = 0 # North #yaw = radians(yaw_degrees) #quaternion = quaternion_from_euler(0, 0, yaw) #msg.pose.orientation = Quaternion(*quaternion) self.pub.publish(msg) rate.sleep()
def pose_callback(data): # extract x and y position information x = data.pose.pose.position.x y = data.pose.pose.position.y # package and broadcast p = Point() p.x = x p.y = y # send it pub_beacon.publish(p) # and broadbast all odoms on a common channel pub_odom_all.publish(data) # publish pose info for viewing pose_out = PoseStamped() pose_out.header = data.header pose_out.header.frame_id = '/world' pose_out.pose = data.pose.pose pub_pose.publish(pose_out) # and twist stamped for DMS interface vel_out = TwistStamped() vel_out.header = data.header vel_out.header.frame_id = '/world' vel_out.twist = data.twist.twist pub_vel.publish(vel_out) # transform tf_broadcast.sendTransform((x, y, 0.0), (data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w), rospy.Time.now(),frame_name,"/world")
def update(self, state): if (self.target is None): rospy.logwarn("Target position for velocity controller is none.") return None # simplify variables a bit time = state.header.stamp.to_sec() position = state.pose.position orientation = state.pose.orientation # create output structure output = TwistStamped() output.header = state.header # output velocities linear = Vector3() angular = Vector3() # Control in X vel linear.x = self.X.update(self.target.position.x, position.x, time) # Control in Y vel linear.y = self.Y.update(self.target.position.y, position.y, time) # Control in Z vel linear.z = self.Z.update(self.target.position.z, position.z, time) # Control yaw (no x, y angular) # TODO output.twist = Twist() output.twist.linear = linear return output
def publish_Transform_Pose_Twist_AccelStamped(trans, ang, timestamp, header_frame_id='world'): transform_msg = TransformStamped() transform_msg.header.frame_id = header_frame_id transform_msg.header.stamp = timestamp transform_msg.child_frame_id = "x1" transform_msg.transform.translation.x = trans.x transform_msg.transform.translation.y = trans.y transform_msg.transform.rotation = th_to_quat(ang.th) tf_broadcaster.sendTransform(transform_msg) pose_msg = PoseStamped() pose_msg.header = transform_msg.header pose_msg.pose.position.x = trans.x pose_msg.pose.position.y = trans.y pose_msg.pose.orientation = th_to_quat(ang.th) x1_pose_pub.publish(pose_msg) vel_msg = TwistStamped() vel_msg.header = transform_msg.header vel_msg.twist.linear.x = trans.xd vel_msg.twist.linear.y = trans.yd vel_msg.twist.angular.z = ang.thd x1_vel_pub.publish(vel_msg) accel_msg = AccelStamped() accel_msg.header = transform_msg.header accel_msg.accel.linear.x = trans.xdd accel_msg.accel.linear.y = trans.ydd x1_accel_pub.publish(accel_msg)
def callBackRightEncoderCount(data): # ----------------------------------------------------------------------------- global robot robot.rightWheel.encoderCount = data.data t = data.header.stamp.secs + 1.E-9 * data.header.stamp.nsecs deltaT = t - robot.rightWheel.lastTimeCountChange # rospy.loginfo(rospy.get_name() + " - time: %f" % t) # angular speed computation if (deltaT > 0.0): # first order filter omegaOld = robot.rightWheel.omega omega = (robot.rightWheel.encoderCount - robot.rightWheel.encoderCountPrev) * ( 2.0 * np.pi / robot.rightWheel.encoderResolution) / deltaT alpha = 0.3 robot.rightWheel.omega = alpha * omega + (1.0 - alpha) * omegaOld # updates for next iteration robot.rightWheel.lastTimeCountChange = t robot.rightWheel.encoderCountPrev = robot.rightWheel.encoderCount # message to be published msgTwist = TwistStamped() msgTwist.header = data.header msgTwist.twist.angular.z = robot.rightWheel.omega # publication pubRightWheelSpeed.publish(msgTwist)
def set_position(): port = '/dev/ttyACM0' baud = 115200 ser = serial.Serial(port, baud) temp = (ser.readline()) a = ser.readline().rstrip() b = ser.readline().rstrip() c = ser.readline().rstrip() d = ser.readline().rstrip() e = ser.readline().rstrip() global hold global holdkm1 global hold_refr hold = 0 pos = TwistStamped() pos.twist = Twist() pos.header = Header() if int(a) > 1000: pos.twist.linear.z = 1 print a elif int(b) > 1000: pos.twist.linear.x = 0.8 print b elif int(c) > 1000: pos.twist.linear.x = -0.8 print c elif int(d) > 1000: pos.twist.linear.y = 0.8 print d elif int(e) > 1000: pos.twist.linear.y = -.8 print e return pos
def callBackLeftEncoderCount(data): # ----------------------------------------------------------------------------- global robot robot.leftWheel.encoderCount = data.data t = data.header.stamp.secs + 1.E-9 * data.header.stamp.nsecs deltaT = t - robot.leftWheel.lastTimeCountChange # angular speed computation if (deltaT > 0.0): # first order filter omegaOld = robot.leftWheel.omega omega = (robot.leftWheel.encoderCount - robot.leftWheel.encoderCountPrev) * ( 2.0 * np.pi / robot.leftWheel.encoderResolution) / deltaT robot.leftWheel.omega = alpha * omega + (1.0 - alpha) * omegaOld # updates for next iteration robot.leftWheel.lastTimeCountChange = t robot.leftWheel.encoderCountPrev = robot.leftWheel.encoderCount # message to be published msgTwist = TwistStamped() msgTwist.header = data.header msgTwist.twist.angular.z = robot.leftWheel.omega msgTwist.twist.linear.x = robot.leftWheel.omega * robot.leftWheel.diameter / 2.0 # TO DO: sign to be checked # publication pubLeftWheelSpeed.publish(msgTwist)
def publish(): header = Header() header.stamp = rospy.Time.now() header.frame_id = 'map' for role, robot_id in WorldModel.assignments.items(): if robot_id is not None: command = WorldModel.commands[role] if command.velocity_control_enable: msg = TwistStamped() msg.header = header msg.twist = command.target_velocity pubs_velocity[robot_id].publish(msg) else: send_pose = WorldModel.tf_listener.transformPose( "map", command.target_pose) send_pose.header.stamp = rospy.Time.now() pubs_position[robot_id].publish(send_pose) pubs_kick_velocity[robot_id].publish(Float32(command.kick_power)) status = AIStatus() status.avoidBall = command.avoid_ball status.avoidDefenceArea = command.avoid_defence_area status.do_chip = command.chip_enable status.dribble_power = command.dribble_power pubs_ai_status[robot_id].publish(status) command.reset_adjustments()
def twistUpdate(self): if self.curr_state: x_des, v_des, R_des, w_des = self.getGoalState(self.curr_state) e_x, e_v, e_R, e_w = self.getErrors(self.curr_state) twist_cmd = TwistStamped() twist_cmd.header = Header() twist_cmd.header.stamp = rospy.Time.now() twist_cmd.twist.linear.x = -self.k_x * e_x[0] - self.k_v * e_v[ 0] + v_des[0] twist_cmd.twist.linear.y = -self.k_x * e_x[1] - self.k_v * e_v[ 1] + v_des[1] twist_cmd.twist.linear.z = -self.k_x * e_x[2] - self.k_v * e_v[ 2] + v_des[2] twist_cmd.twist.angular.x = -self.k_R * e_R[0] - self.k_w * e_w[ 0] + w_des[0] twist_cmd.twist.angular.y = -self.k_R * e_R[1] - self.k_w * e_w[ 1] + w_des[1] twist_cmd.twist.angular.z = -self.k_R * e_R[2] - self.k_w * e_w[ 2] + w_des[2] self.twist_pub.publish(twist_cmd) self.publishGoal(self.curr_state) return
def turn_to_goal(self): rx, ry, rz, r, p, y = self.parse_local_position("e") vel = TwistStamped() vel.header = Header() vel.header.frame_id = "map" vel.header.stamp = rospy.Time.now() vel.twist.linear.x = 0 vel.twist.linear.y = 0 vel.twist.linear.z = 0 vel.twist.angular.x = 0 #pid.step(0-r) vel.twist.angular.y = 0 #pid.step(0-p) if self.global_goal is None: self.global_goal = self.goal yaw = math.atan2(self.global_goal[1] - ry, self.global_goal[0] - rx) if abs(yaw - y) > math.pi: yaw = (2 * math.pi - abs(yaw)) * np.sign(-yaw) while abs(yaw - y) > 0.1: yaw = math.atan2(self.global_goal[1] - ry, self.global_goal[0] - rx) if abs(yaw - y) > math.pi: yaw = (2 * math.pi - abs(yaw)) * np.sign(-yaw) vel.twist.angular.z = self.pid.step(yaw - y) self.pos_setvel_pub.publish(vel) rospy.sleep(-1) rx, ry, rz, r, p, y = self.parse_local_position("e")
def initialise(self, measurement: Odometry): self.prev_measurement = measurement self.tmeas = float(measurement.header.stamp.to_sec()) zero_input_cmd = TwistStamped() zero_input_cmd.header = measurement.header self.cmd_list = [zero_input_cmd] self.requires_initialisation = False
def write_wheel(wheel, i, bag): utime = wheel[i, 0] timestamp = rospy.Time.from_sec(utime / 1e6) header = Header() header.seq = i header.stamp = timestamp header.frame_id = 'wheelSpeed' wheelLeft = TwistStamped() wheelLeft.header = header wheelLeft.twist.linear.z = float(wheel[i, 1]) wheelRight = TwistStamped() wheelRight.header = header wheelRight.twist.linear.z = float(wheel[i, 2]) bag.write('wheelLeft', wheelLeft, t=timestamp) bag.write('wheelRight', wheelRight, t=timestamp)
def listener_callback(self, inMsg): outMsg = TwistStamped() outMsg.header = Header() outMsg.header.stamp = self.get_clock().now().to_msg() outMsg.header.frame_id = self.frame_id outMsg.twist = inMsg self.publisher_.publish(outMsg)
def create_twist(self, velocity, angular): tw = TwistStamped() tw.header = Header() tw.header.stamp = rospy.Time.now() tw.header.frame_id = '/vehicle' tw.twist.linear.x = velocity tw.twist.angular.z = angular return tw
def publish(self): print "publishing" message = TwistStamped() message.header = self.pose.header message.twist.linear.x = self.pose.vector.x message.twist.linear.y = self.pose.vector.y message.twist.linear.z = 0.0 message.twist.angular.x = 0.0 message.twist.angular.y = 0.0 message.twist.angular.z = self.pose.vector.z print self.pose.vector.z self.pub.publish(message) message = Vector3Stamped() message.header = self.pose.header message.vector = self.encoderSpeed self.motor_test.publish(message)
def set_velocity(self,vx,vy,vz): vel = TwistStamped() vel.twist.linear.x = vx vel.twist.linear.y = vy vel.twist.linear.z = vz vel.header = Header() vel.header.frame_id = "map" vel.header.stamp = rospy.Time.now() self.pos_setvel_pub.publish(vel)
def default(self, ci='unused'): twist = TwistStamped() twist.header = self.get_ros_header() # Fill twist-msg with the values from the sensor twist.twist.linear.x = self.data['velocity'][0] twist.twist.linear.y = self.data['velocity'][1] twist.twist.linear.z = self.data['velocity'][2] self.publish(twist)
def _make_twist_stamped(self, linear, angular): twist_stamped = TwistStamped() header = Header() header.stamp = rclpy.clock.Clock().now().to_msg() header.frame_id = 'key_teleop' twist_stamped.header = header twist_stamped.twist.linear.x = linear twist_stamped.twist.angular.z = angular return twist_stamped
def update(self, state): if (self.target is None): rospy.logwarn("Target position for velocity controller is none.") return None # simplify variables a bit time = state.header.stamp.to_sec() position = state.pose.position orientation = state.pose.orientation quat_arr = np.array( [orientation.x, orientation.y, orientation.z, orientation.w]) att = euler_from_quaternion(quat_arr, 'sxyz') # create output structure output = TwistStamped() output.header = state.header # output velocities linear = Vector3() angular = Vector3() if (False): #abs(att[2]-self.heading_target) > deg2rad(5.0)): # Control in X vel linear.x = self.X.update(position.x, position.x, time) # Control in Y vel linear.y = self.Y.update(position.y, position.y, time) # Control in Z vel linear.z = self.Z.update(position.z, position.z, time) else: # Control in X vel linear.x = self.X.update(self.target.position.x, position.x, time) # Control in Y vel linear.y = self.Y.update(self.target.position.y, position.y, time) # Control in Z vel linear.z = self.Z.update(self.target.position.z, position.z, time) # Control yaw (no x, y angular) delta = 0.3 #if((abs(linear.y) > delta) and (abs(linear.x) > delta)) or (self.psi_target == None): output.twist = Twist() #if(self.distance(self.target,state.pose)>delta): phi_target = 0.0 theta_target = 0.0 psi_target = self.heading_target #np.arctan((self.target.position.y-self.prev_target.position.y)/(self.target.position.x-self.prev_target.position.x))#(linear.y,linear.x) #psi_target = np.arctan((self.target.position.y-self.prev_target.position.y)/(self.target.position.x-self.prev_target.position.x))#(linear.y,linear.x) #psi_target = math.atan2(linear.y,linear.x) #angular.x = self.PHI.update(phi_target, att[0],time) #angular.y = self.THETA.update(theta_target, att[1],time) angular.z = self.PSI.update(psi_target, att[2], time) output.twist.angular = angular # TODO output.twist.linear = linear if output == None: print "output", output return output
def default(self, ci='unused'): twist = TwistStamped() twist.header = self.get_ros_header() twist.twist.linear.x = self.data['world_linear_velocity'][0] twist.twist.linear.y = self.data['world_linear_velocity'][1] twist.twist.linear.z = self.data['world_linear_velocity'][2] twist.twist.angular.x = self.data['angular_velocity'][0] twist.twist.angular.y = self.data['angular_velocity'][1] twist.twist.angular.z = self.data['angular_velocity'][2] self.publish(twist)
def default(self, ci='unused'): twist = TwistStamped() twist.header = self.get_ros_header() twist.twist.linear.x = self.data['linear_velocity'][0] twist.twist.linear.y = self.data['linear_velocity'][1] twist.twist.linear.z = self.data['linear_velocity'][2] twist.twist.angular.x = self.data['angular_velocity'][0] twist.twist.angular.y = self.data['angular_velocity'][1] twist.twist.angular.z = self.data['angular_velocity'][2] self.publish(twist)
def vec_cb(msg): global twist_pub twst = TwistStamped() twst.twist.linear.x = -msg.vector.y twst.twist.linear.y = msg.vector.x twst.twist.linear.z = msg.vector.z twst.header = msg.header twst.twist.angular = Vector3(0, 0, 0) twist_pub.publish(twst)
def angularVeloCallback(self, ang_velo): self.angular_velo[0] = ang_velo.twist.angular.x self.angular_velo[1] = ang_velo.twist.angular.y self.angular_velo[2] = ang_velo.twist.angular.z tmp_angular_velo_rot = np.array([ang_velo.twist.angular.x, ang_velo.twist.angular.y, ang_velo.twist.angular.z]) self.angular_velo_rot = self.qx_180.rotate(tmp_angular_velo_rot) ang_veloNED = TwistStamped() ang_veloNED.header = ang_velo.header # TIMESTAMP? ang_veloNED.header.frame_id = 'global_tank' ang_veloNED.twist.angular.x= self.angular_velo_rot[0] ang_veloNED.twist.angular.y = self.angular_velo_rot[1] ang_veloNED.twist.angular.z = self.angular_velo_rot[2] self.angular_velocity_publish.publish(ang_veloNED)
def navigate(self): rate = self.rospy.Rate(10) # 10hz msg = TwistStamped() msg.header = Header() msg.header.frame_id = "base_footprint" msg.header.stamp = rospy.Time.now() while 1: msg.twist.linear.x = self.x msg.twist.linear.y = self.y msg.twist.linear.z = self.z self.pub.publish(msg) rate.sleep()
def land_loop(self): """Loops in Control_Mode.TAKEOFF until either the mode is switched or the target altitude has been reached.""" # Count the number of iterations through the loop loops = 0 # Build message msg = TwistStamped() msg.twist.linear.x = 0 msg.twist.linear.y = 0 msg.twist.linear.z = -0.4 while self.is_running() and self._mode == Control_Mode.LAND: # Break if the magnitude is below the threshold for the desired amount of time if abs( math.sqrt( math.pow(self._vehicle.get_velocity_x(), 2) + math.pow(self._vehicle.get_velocity_y(), 2) + math.pow(self._vehicle.get_velocity_z(), 2))) < 0.1: # Check if the number of required successful loops have passed # rospy.loginfo("X: " + str(self._vehicle.get_velocity_x())) # rospy.loginfo("Y: " + str(self._vehicle.get_velocity_y())) # rospy.loginfo("Z: " + str(self._vehicle.get_velocity_z())) # rospy.loginfo("Total: " + str(abs(math.sqrt(math.pow(self._vehicle.get_velocity_x(), 2) + math.pow(self._vehicle.get_velocity_y(), 2) + math.pow(self._vehicle.get_velocity_z(), 2))))) if loops > 10: # Disarm self._vehicle.disarm() break else: # Increment number of loops loops += 1 else: # Reset loops loops = 0 # Update message header msg.header = Header(stamp=rospy.get_rostime()) # Publish message self._vel_pub.publish(msg) # Sleep to maintain appropriate update frequency self._rate.sleep() # Only perform if controller is still running if self.is_running(): # Set mode to INITIAL self.set_mode(Control_Mode.INITIAL)
def getTwistInBaseFrame(self, twistWC, header): """Returns a TwistWithCovarianceStamped in base frame""" # Create the stamped object twistS = TwistStamped() twistS.header = header twistS.twist = twistWC.twist twistS_base = self.transformTwist(self.base_frame_id, twistS) twistWC_out = TwistWithCovarianceStamped() twistWC_out.header = twistS_base.header twistWC_out.twist.twist = twistS_base.twist twistWC_out.twist.covariance = twistWC.covariance return twistWC_out
def navigate(self): print("navigate") rate = self.rospy.Rate(20) # 10hz msg = TwistStamped() msg.header = Header() msg.header.frame_id = "base_footprint" msg.header.stamp = rospy.Time.now() while 1: msg.twist.linear.x = self.x msg.twist.linear.y = self.y msg.twist.linear.z = self.z msg.twist.angular.z = self.yaw self.pub.publish(msg) # it publish it to the hardware ( the motors) print "Published velocity to auto x:" + str(self.x) + " y:" + str(self.y) + " z:" + str(self.z) + " yaw:" + str(self.yaw) rate.sleep()
def do_transform_twist(twist, transform): transform = copy.deepcopy(transform) kdl_tf = transform_to_kdl(transform) linear = kdl_tf * PyKDL.Vector(twist.twist.linear.x, twist.twist.linear.y, twist.twist.linear.z) angular = kdl_tf * PyKDL.Vector(twist.twist.linear.x, twist.twist.linear.y, twist.twist.linear.z) res = TwistStamped() res.twist.linear.x = linear[0] res.twist.linear.y = linear[1] res.twist.linear.z = linear[2] res.twist.angular.x = angular[0] res.twist.angular.y = angular[1] res.twist.angular.z = angular[2] res.header = transform.header return res
def twist_cb(self, twist): # get teleop twist message twist_stamped = TwistStamped() twist_stamped.twist = twist if not ( twist.linear.x == 0 and twist.linear.y == 0 and twist.linear.z == 0 and twist.angular.x == 0 and twist.angular.y == 0 and twist.angular.z == 0 ): # give it a header header = Header() header.stamp = rospy.Time.now() twist_stamped.header = header # publish the command self.teleop_stamped_pub.publish(twist_stamped)
def twistUpdate(self): if self.curr_state: x_des, v_des, R_des, w_des = self.getGoalState(self.curr_state) e_x, e_v, e_R, e_w = self.getErrors(self.curr_state) twist_cmd = TwistStamped() twist_cmd.header = Header() twist_cmd.header.stamp = rospy.Time.now() twist_cmd.twist.linear.x = -self.k_x * e_x[0] - self.k_v * e_v[0] + v_des[0] twist_cmd.twist.linear.y = -self.k_x * e_x[1] - self.k_v * e_v[1] + v_des[1] twist_cmd.twist.linear.z = -self.k_x * e_x[2] - self.k_v * e_v[2] + v_des[2] twist_cmd.twist.angular.x = -self.k_R * e_R[0] - self.k_w * e_w[0] + w_des[0] twist_cmd.twist.angular.y = -self.k_R * e_R[1] - self.k_w * e_w[1] + w_des[1] twist_cmd.twist.angular.z = -self.k_R * e_R[2] - self.k_w * e_w[2] + w_des[2] self.twist_pub.publish(twist_cmd) self.publishGoal(self.curr_state) return
def navigate(self): rate = self.rospy.Rate(10) # 10hz msg = TwistStamped() msg.header = Header() msg.header.frame_id = "base_footprint" msg.header.stamp = rospy.Time.now() while 1: msg.twist.linear.x = self.x msg.twist.linear.y = self.y msg.twist.linear.z = self.z # For demo purposes we will lock yaw/heading to north. #yaw_degrees = 0 # North #yaw = radians(yaw_degrees) #quaternion = quaternion_from_euler(0, 0, yaw) #msg.pose.orientation = Quaternion(*quaternion) self.pub.publish(msg) rate.sleep()
def Driver(): global rate,pub,start_time,states_in, x_est, P_est,eye,Q,Rk,status if status: #===============# # Predict # #===============# dy = Quad(0) xhat = x_est + dy/rate F = eye + Quad_Jacobian(x_est)/rate Phat = F*P_est*F.T + Q/rate New_measurement = CheckInnovation(xhat,states_in,Phat) if New_measurement: # then use the measurement #==============# # Update # #==============# deg2rad = np.pi/180 # measurement zk = np.matrix([[states_in.x],[-states_in.y],[-states_in.z],[states_in.u],[-states_in.v],[-states_in.w],[states_in.phi*deg2rad],[states_in.theta*deg2rad],[states_in.psi*deg2rad],[states_in.p*deg2rad],[states_in.q*deg2rad],[states_in.r*deg2rad]]) # residual/innovation yk = zk - xhat # residual/innovation Covariance Sk = Phat + Rk # Kalman Gain Kk = Phat*np.linalg.inv(Sk) x_est = xhat + Kk*yk P_est = (eye-Kk)*Phat else: x_est = xhat P_est = Phat #=================================================# # Check if Estimate is actually within Room # #=================================================# if np.linalg.norm(x_est[0:3]) >6 or (x_est != x_est).all(): x_est = np.asmatrix(np.zeros((12,1))) x_est[0:3] = np.matrix([[0],[0],[-2]]) P_est = 3*np.asmatrix(np.eye(12)) #--------------# # Make Message # #--------------# cortex = Cortex() cortex.Obj = [States()]*1 states = States() states.name = states_in.name states.visible = states_in.visible global h h.seq = h.seq + 1 h.stamp = rospy.Time.now() h.frame_id = 'cortex' cortex.header = h rad2deg = 180/np.pi # Invert y and z position and velocity from NED to Cartesian states.x = x_est[0] states.y = -x_est[1] states.z = -x_est[2] states.u = x_est[3] states.v = -x_est[4] states.w = -x_est[5] states.phi = rad2deg*x_est[6] states.theta = rad2deg*x_est[7] states.psi = rad2deg*x_est[8] states.p = rad2deg*x_est[9] states.q = rad2deg*x_est[10] states.r = rad2deg*x_est[11] cortex.Obj[0] = states global acc acc.header = h cov = TwistStamped() cov.header = h cov.twist.linear.x = x_est[0] cov.twist.linear.y = -x_est[1] cov.twist.linear.z = -x_est[2] cov.twist.angular.x = P_est[0,0] cov.twist.angular.y = P_est[1,1] cov.twist.angular.z = P_est[2,2] #---------# # Publish # #---------# pub_cov.publish(cov) pub_acc.publish(acc) pub.publish(cortex)
def spin_once(self): '''Read data from device and publishes ROS messages.''' # common header h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id # create messages and default values imu_msg = Imu() imu_msg.orientation_covariance = (-1., )*9 imu_msg.angular_velocity_covariance = (-1., )*9 imu_msg.linear_acceleration_covariance = (-1., )*9 pub_imu = False gps_msg = NavSatFix() xgps_msg = GPSFix() pub_gps = False vel_msg = TwistStamped() pub_vel = False mag_msg = Vector3Stamped() pub_mag = False temp_msg = Float32() pub_temp = False press_msg = Float32() pub_press = False anin1_msg = UInt16() pub_anin1 = False anin2_msg = UInt16() pub_anin2 = False pub_diag = False def fill_from_raw(raw_data): '''Fill messages with information from 'raw' MTData block.''' # don't publish raw imu data anymore # TODO find what to do with that pass def fill_from_rawgps(rawgps_data): '''Fill messages with information from 'rawgps' MTData block.''' global pub_hps, xgps_msg, gps_msg if rawgps_data['bGPS']<self.old_bGPS: pub_gps = True # LLA xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT']*1e-7 xgps_msg.longitude = gps_msg.longitude = rawgps_data['LON']*1e-7 xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT']*1e-3 # NED vel # TODO? # Accuracy # 2 is there to go from std_dev to 95% interval xgps_msg.err_horz = 2*rawgps_data['Hacc']*1e-3 xgps_msg.err_vert = 2*rawgps_data['Vacc']*1e-3 self.old_bGPS = rawgps_data['bGPS'] def fill_from_Temp(temp): '''Fill messages with information from 'temperature' MTData block.''' global pub_temp, temp_msg pub_temp = True temp_msg.data = temp def fill_from_Calib(imu_data): '''Fill messages with information from 'calibrated' MTData block.''' global pub_imu, imu_msg, pub_vel, vel_msg, pub_mag, mag_msg try: pub_imu = True imu_msg.angular_velocity.x = imu_data['gyrX'] imu_msg.angular_velocity.y = imu_data['gyrY'] imu_msg.angular_velocity.z = imu_data['gyrZ'] imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0., radians(0.025), 0., 0., 0., radians(0.025)) pub_vel = True vel_msg.twist.angular.x = imu_data['gyrX'] vel_msg.twist.angular.y = imu_data['gyrY'] vel_msg.twist.angular.z = imu_data['gyrZ'] except KeyError: pass try: pub_imu = True imu_msg.linear_acceleration.x = imu_data['accX'] imu_msg.linear_acceleration.y = imu_data['accY'] imu_msg.linear_acceleration.z = imu_data['accZ'] imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0., 0.0004, 0., 0., 0., 0.0004) except KeyError: pass try: pub_mag = True mag_msg.vector.x = imu_data['magX'] mag_msg.vector.y = imu_data['magY'] mag_msg.vector.z = imu_data['magZ'] except KeyError: pass def fill_from_Vel(velocity_data): '''Fill messages with information from 'velocity' MTData block.''' global pub_vel, vel_msg pub_vel = True vel_msg.twist.linear.x = velocity_data['Vel_X'] vel_msg.twist.linear.y = velocity_data['Vel_Y'] vel_msg.twist.linear.z = velocity_data['Vel_Z'] def fill_from_Orient(orient_data): '''Fill messages with information from 'orientation' MTData block.''' global pub_imu, imu_msg pub_imu = True if orient.has_key('quaternion'): w, x, y, z = orient['quaternion'] elif orient.has_key('roll'): # FIXME check that Euler angles are in radians x, y, z, w = quaternion_from_euler(radians(orient['roll']), radians(orient['pitch']), radians(orient['yaw'])) elif orient.has_key('matrix'): m = identity_matrix() m[:3,:3] = orient['matrix'] x, y, z, w = quaternion_from_matrix(m) imu_msg.orientation.x = x imu_msg.orientation.y = y imu_msg.orientation.z = z imu_msg.orientation.w = w imu_msg.orientation_covariance = (radians(1.), 0., 0., 0., radians(1.), 0., 0., 0., radians(9.)) def fill_from_Pos(position_data): '''Fill messages with information from 'position' MTData block.''' global pub_gps, xgps_msg, gps_msg pub_gps = True xgps_msg.latitude = gps_msg.latitude = position_data['Lat'] xgps_msg.longitude = gps_msg.longitude = position_data['Lon'] xgps_msg.altitude = gps_msg.altitude = position_data['Alt'] def fill_from_Stat(status): '''Fill messages with information from 'status' MTData block.''' global pub_diag, pub_gps, gps_msg, xgps_msg pub_diag = True if status & 0b0001: self.stest_stat.level = DiagnosticStatus.OK self.stest_stat.message = "Ok" else: self.stest_stat.level = DiagnosticStatus.ERROR self.stest_stat.message = "Failed" if status & 0b0010: self.xkf_stat.level = DiagnosticStatus.OK self.xkf_stat.message = "Valid" else: self.xkf_stat.level = DiagnosticStatus.WARN self.xkf_stat.message = "Invalid" if status & 0b0100: self.gps_stat.level = DiagnosticStatus.OK self.gps_stat.message = "Ok" gps_msg.status.status = NavSatStatus.STATUS_FIX xgps_msg.status.status = GPSStatus.STATUS_FIX gps_msg.status.service = NavSatStatus.SERVICE_GPS xgps_msg.status.position_source = 0b01101001 xgps_msg.status.motion_source = 0b01101010 xgps_msg.status.orientation_source = 0b01101010 else: self.gps_stat.level = DiagnosticStatus.WARN self.gps_stat.message = "No fix" gps_msg.status.status = NavSatStatus.STATUS_NO_FIX xgps_msg.status.status = GPSStatus.STATUS_NO_FIX gps_msg.status.service = 0 xgps_msg.status.position_source = 0b01101000 xgps_msg.status.motion_source = 0b01101000 xgps_msg.status.orientation_source = 0b01101000 def fill_from_Auxiliary(aux_data): '''Fill messages with information from 'Auxiliary' MTData block.''' global pub_anin1, pub_anin2, anin1_msg, anin2_msg try: anin1_msg.data = o['Ain_1'] pub_anin1 = True except KeyError: pass try: anin2_msg.data = o['Ain_2'] pub_anin2 = True except KeyError: pass def fill_from_Temperature(o): '''Fill messages with information from 'Temperature' MTData2 block.''' global pub_temp, temp_msg pub_temp = True temp_msg.data = o['Temp'] def fill_from_Timestamp(o): '''Fill messages with information from 'Timestamp' MTData2 block.''' global h try: # put timestamp from gps UTC time if available y, m, d, hr, mi, s, ns, f = o['Year'], o['Month'], o['Day'],\ o['Hour'], o['Minute'], o['Second'], o['ns'], o['Flags'] if f&0x4: secs = time.mktime((y, m, d, hr, mi, s, 0, 0, 0)) h.stamp.secs = secs h.stamp.nsecs = ns except KeyError: pass # TODO find what to do with other kind of information pass def fill_from_Orientation_Data(o): '''Fill messages with information from 'Orientation Data' MTData2 block.''' global pub_imu, imu_msg pub_imu = True try: x, y, z, w = o['Q1'], o['Q2'], o['Q3'], o['Q0'] except KeyError: pass try: # FIXME check that Euler angles are in radians x, y, z, w = quaternion_from_euler(radians(o['Roll']), radians(o['Pitch']), radians(o['Yaw'])) except KeyError: pass try: a, b, c, d, e, f, g, h, i = o['a'], o['b'], o['c'], o['d'],\ o['e'], o['f'], o['g'], o['h'], o['i'] m = identity_matrix() m[:3,:3] = ((a, b, c), (d, e, f), (g, h, i)) x, y, z, w = quaternion_from_matrix(m) except KeyError: pass imu_msg.orientation.x = x imu_msg.orientation.y = y imu_msg.orientation.z = z imu_msg.orientation.w = w imu_msg.orientation_covariance = (radians(1.), 0., 0., 0., radians(1.), 0., 0., 0., radians(9.)) def fill_from_Pressure(o): '''Fill messages with information from 'Pressure' MTData2 block.''' global pub_press, press_msg press_msg.data = o['Pressure'] def fill_from_Acceleration(o): '''Fill messages with information from 'Acceleration' MTData2 block.''' global pub_imu, imu_msg pub_imu = True # FIXME not sure we should treat all in that same way try: x, y, z = o['Delta v.x'], o['Delta v.y'], o['Delta v.z'] except KeyError: pass try: x, y, z = o['freeAccX'], o['freeAccY'], o['freeAccZ'] except KeyError: pass try: x, y, z = o['accX'], o['accY'], o['accZ'] except KeyError: pass imu_msg.linear_acceleration.x = x imu_msg.linear_acceleration.y = y imu_msg.linear_acceleration.z = z imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0., 0.0004, 0., 0., 0., 0.0004) def fill_from_Position(o): '''Fill messages with information from 'Position' MTData2 block.''' global pub_gps, xgps_msg, gps_msg # TODO publish ECEF try: xgps_msg.latitude = gps_msg.latitude = o['lat'] xgps_msg.longitude = gps_msg.longitude = o['lon'] pub_gps = True alt = o.get('altMsl', o.get('altEllipsoid', 0)) xgps_msg.altitude = gps_msg.altitude = alt except KeyError: pass def fill_from_Angular_Velocity(o): '''Fill messages with information from 'Angular Velocity' MTData2 block.''' global pub_imu, imu_msg, pub_vel, vel_msg try: imu_msg.angular_velocity.x = o['gyrX'] imu_msg.angular_velocity.y = o['gyrY'] imu_msg.angular_velocity.z = o['gyrZ'] imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0., radians(0.025), 0., 0., 0., radians(0.025)) pub_imu = True vel_msg.twist.angular.x = o['gyrX'] vel_msg.twist.angular.y = o['gyrY'] vel_msg.twist.angular.z = o['gyrZ'] pub_vel = True except KeyError: pass # TODO decide what to do with 'Delta q' def fill_from_GPS(o): '''Fill messages with information from 'GPS' MTData2 block.''' global pub_gps, h, xgps_msg try: # DOP xgps_msg.gdop = o['gDOP'] xgps_msg.pdop = o['pDOP'] xgps_msg.hdop = o['hDOP'] xgps_msg.vdop = o['vDOP'] xgps_msg.tdop = o['tDOP'] pub_gps = True except KeyError: pass try: # Time UTC y, m, d, hr, mi, s, ns, f = o['year'], o['month'], o['day'],\ o['hour'], o['min'], o['sec'], o['nano'], o['valid'] if f&0x4: secs = time.mktime((y, m, d, hr, mi, s, 0, 0, 0)) h.stamp.secs = secs h.stamp.nsecs = ns except KeyError: pass # TODO publish SV Info def fill_from_SCR(o): '''Fill messages with information from 'SCR' MTData2 block.''' # TODO that's raw information pass def fill_from_Analog_In(o): '''Fill messages with information from 'Analog In' MTData2 block.''' global pub_anin1, pub_anin2, anin1_msg, anin2_msg try: anin1_msg.data = o['analogIn1'] pub_anin1 = True except KeyError: pass try: anin2_msg.data = o['analogIn2'] pub_anin2 = True except KeyError: pass def fill_from_Magnetic(o): '''Fill messages with information from 'Magnetic' MTData2 block.''' global pub_mag, mag_msg mag_msg.vector.x = o['magX'] mag_msg.vector.y = o['magY'] mag_msg.vector.z = o['magZ'] pub_mag = True def fill_from_Velocity(o): '''Fill messages with information from 'Velocity' MTData2 block.''' global pub_vel, vel_msg vel_msg.twist.linear.x = o['velX'] vel_msg.twist.linear.y = o['velY'] vel_msg.twist.linear.z = o['velZ'] pub_vel = True def fill_from_Status(o): '''Fill messages with information from 'Status' MTData2 block.''' try: status = o['StatusByte'] fill_from_Stat(status) except KeyError: pass try: status = o['StatusWord'] fill_from_Stat(status) except KeyError: pass # TODO RSSI def find_handler_name(name): return "fill_from_%s"%(name.replace(" ", "_")) # get data data = self.mt.read_measurement() # fill messages based on available data fields for n, o in data: try: locals()[find_handler_name(n)](o) except KeyError: rospy.logwarn("Unknown MTi data packet: '%s', ignoring."%n) # publish available information if pub_imu: imu_msg.header = h self.imu_pub.publish(imu_msg) if pub_gps: xgps_msg.header = gps_msg.header = h self.gps_pub.publish(gps_msg) self.xgps_pub.publish(xgps_msg) if pub_vel: vel_msg.header = h self.vel_pub.publish(vel_msg) if pub_mag: mag_msg.header = h self.mag_pub.publish(mag_msg) if pub_temp: self.temp_pub.publish(temp_msg) if pub_press: self.press_pub.publish(press_msg) if pub_anin1: self.analog_in1_pub.publish(anin1_msg) if pub_anin2: self.analog_in2_pub.publish(anin2_msg) if pub_diag: self.diag_msg.header = h self.diag_pub.publish(self.diag_msg) # publish string representation self.str_pub.publish(str(data))
def spin_once(self): def quat_from_orient(orient): '''Build a quaternion from orientation data.''' try: w, x, y, z = orient['quaternion'] return (x, y, z, w) except KeyError: pass try: return quaternion_from_euler(pi*orient['roll']/180., pi*orient['pitch']/180, pi*orient['yaw']/180.) except KeyError: pass try: m = identity_matrix() m[:3,:3] = orient['matrix'] return quaternion_from_matrix(m) except KeyError: pass # get data data = self.mt.read_measurement() # common header h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id # get data (None if not present) temp = data.get('Temp') # float raw_data = data.get('RAW') imu_data = data.get('Calib') orient_data = data.get('Orient') velocity_data = data.get('Velocity') position_data = data.get('Position') rawgps_data = data.get('RAWGPS') status = data.get('Status') # int sample = data.get('Sample') # TODO by @demmeln: use sample counter for timestamp # correction. Watch out for counter wrap. # create messages and default values imu_msg = Imu() imu_msg.orientation_covariance = (-1., )*9 imu_msg.angular_velocity_covariance = (-1., )*9 imu_msg.linear_acceleration_covariance = (-1., )*9 pub_imu = False gps_msg = NavSatFix() xgps_msg = GPSFix() pub_gps = False vel_msg = TwistStamped() pub_vel = False mag_msg = Vector3Stamped() pub_mag = False temp_msg = Float32() pub_temp = False # fill information where it's due # start by raw information that can be overriden if raw_data: # TODO warn about data not calibrated pub_imu = True pub_vel = True pub_mag = True pub_temp = True # acceleration imu_msg.linear_acceleration.x = raw_data['accX'] imu_msg.linear_acceleration.y = raw_data['accY'] imu_msg.linear_acceleration.z = raw_data['accZ'] imu_msg.linear_acceleration_covariance = (0., )*9 # gyroscopes imu_msg.angular_velocity.x = raw_data['gyrX'] imu_msg.angular_velocity.y = raw_data['gyrY'] imu_msg.angular_velocity.z = raw_data['gyrZ'] imu_msg.angular_velocity_covariance = (0., )*9 vel_msg.twist.angular.x = raw_data['gyrX'] vel_msg.twist.angular.y = raw_data['gyrY'] vel_msg.twist.angular.z = raw_data['gyrZ'] # magnetometer mag_msg.vector.x = raw_data['magX'] mag_msg.vector.y = raw_data['magY'] mag_msg.vector.z = raw_data['magZ'] # temperature # 2-complement decoding and 1/256 resolution x = raw_data['temp'] if x&0x8000: temp_msg.data = (x - 1<<16)/256. else: temp_msg.data = x/256. if rawgps_data: if rawgps_data['bGPS']<self.old_bGPS: pub_gps = True # LLA xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT']*1e-7 xgps_msg.longitude = gps_msg.longitude = rawgps_data['LON']*1e-7 xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT']*1e-3 # NED vel # TODO? # Accuracy # 2 is there to go from std_dev to 95% interval xgps_msg.err_horz = 2*rawgps_data['Hacc']*1e-3 xgps_msg.err_vert = 2*rawgps_data['Vacc']*1e-3 self.old_bGPS = rawgps_data['bGPS'] if temp is not None: pub_temp = True temp_msg.data = temp if imu_data: try: imu_msg.angular_velocity.x = imu_data['gyrX'] imu_msg.angular_velocity.y = imu_data['gyrY'] imu_msg.angular_velocity.z = imu_data['gyrZ'] imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0., radians(0.025), 0., 0., 0., radians(0.025)) pub_imu = True vel_msg.twist.angular.x = imu_data['gyrX'] vel_msg.twist.angular.y = imu_data['gyrY'] vel_msg.twist.angular.z = imu_data['gyrZ'] pub_vel = True except KeyError: pass try: imu_msg.linear_acceleration.x = imu_data['accX'] imu_msg.linear_acceleration.y = imu_data['accY'] imu_msg.linear_acceleration.z = imu_data['accZ'] imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0., 0.0004, 0., 0., 0., 0.0004) pub_imu = True except KeyError: pass try: mag_msg.vector.x = imu_data['magX'] mag_msg.vector.y = imu_data['magY'] mag_msg.vector.z = imu_data['magZ'] pub_mag = True except KeyError: pass if velocity_data: pub_vel = True vel_msg.twist.linear.x = velocity_data['Vel_X'] vel_msg.twist.linear.y = velocity_data['Vel_Y'] vel_msg.twist.linear.z = velocity_data['Vel_Z'] if orient_data: pub_imu = True orient_quat = quat_from_orient(orient_data) imu_msg.orientation.x = orient_quat[0] imu_msg.orientation.y = orient_quat[1] imu_msg.orientation.z = orient_quat[2] imu_msg.orientation.w = orient_quat[3] imu_msg.orientation_covariance = (radians(1.), 0., 0., 0., radians(1.), 0., 0., 0., radians(9.)) if position_data: pub_gps = True xgps_msg.latitude = gps_msg.latitude = position_data['Lat'] xgps_msg.longitude = gps_msg.longitude = position_data['Lon'] xgps_msg.altitude = gps_msg.altitude = position_data['Alt'] if status is not None: if status & 0b0001: self.stest_stat.level = DiagnosticStatus.OK self.stest_stat.message = "Ok" else: self.stest_stat.level = DiagnosticStatus.ERROR self.stest_stat.message = "Failed" if status & 0b0010: self.xkf_stat.level = DiagnosticStatus.OK self.xkf_stat.message = "Valid" else: self.xkf_stat.level = DiagnosticStatus.WARN self.xkf_stat.message = "Invalid" if status & 0b0100: self.gps_stat.level = DiagnosticStatus.OK self.gps_stat.message = "Ok" else: self.gps_stat.level = DiagnosticStatus.WARN self.gps_stat.message = "No fix" self.diag_msg.header = h self.diag_pub.publish(self.diag_msg) if pub_gps: if status & 0b0100: gps_msg.status.status = NavSatStatus.STATUS_FIX xgps_msg.status.status = GPSStatus.STATUS_FIX gps_msg.status.service = NavSatStatus.SERVICE_GPS xgps_msg.status.position_source = 0b01101001 xgps_msg.status.motion_source = 0b01101010 xgps_msg.status.orientation_source = 0b01101010 else: gps_msg.status.status = NavSatStatus.STATUS_NO_FIX xgps_msg.status.status = GPSStatus.STATUS_NO_FIX gps_msg.status.service = 0 xgps_msg.status.position_source = 0b01101000 xgps_msg.status.motion_source = 0b01101000 xgps_msg.status.orientation_source = 0b01101000 # publish available information if pub_imu: imu_msg.header = h self.imu_pub.publish(imu_msg) if pub_gps: xgps_msg.header = gps_msg.header = h self.gps_pub.publish(gps_msg) self.xgps_pub.publish(xgps_msg) if pub_vel: vel_msg.header = h self.vel_pub.publish(vel_msg) if pub_mag: mag_msg.header = h self.mag_pub.publish(mag_msg) if pub_temp: self.temp_pub.publish(temp_msg)
def copy_vel(self, vel): copied_vel = TwistStamped() copied_vel.header= vel.header return copied_vel
def onTwistStamped(self,msg): out_msg = TwistStamped() out_msg.header = rospy.time.now() out_msg.twist = msg self.twist_pub.publish(out_msg)