def talker(): print 'in talker' pub = rospy.Publisher('GPS', NavSatFix) rospy.init_node('GPStalker') while not rospy.is_shutdown(): #Assuming that parse will return these values time.sleep(1) parse() msg = NavSatFix() Fix = NavSatStatus() Fix.status = GPS.mode Fix.service = GPS.numSat msg.header.stamp = rospy.Time.now() msg.status = Fix msg.latitude = GPS.lat msg.longitude = GPS.lon msg.altitude = GPS.alt msg.position_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0) msg.position_covariance_type = 0 #covariance_type = 0 unknown # = 1 approximated # = 2 diagonal known # = 3 known pub.publish(msg)
def got_position(xx, yy, aa, stamp): global EP fix = NavSatFix() fix.header.stamp = stamp fix.header.frame_id = "sim_gps" fix.status.status = 0 fix.status.service = 1 utm_e = base_xx + base_utm_e + xx + CONFIG["SIM_ERROR"] * cos((yy + time()) / EP) utm_n = base_yy + base_utm_n + yy + CONFIG["SIM_ERROR"] * cos((utm_e / 3.14) / EP) (lon, lat) = conv(utm_e, utm_n, inverse=True) fix.latitude = lat fix.longitude = lon fix.altitude = 1.3 # print "got_position: %f %f" % (lat, lon) gps.publish(fix) pos.publish(Pose2D(utm_e, utm_n, aa)) tfBr.sendTransform( (utm_e - base_xx, utm_n - base_yy, 0), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/map", "/gps", )
def talker(): UDP_IP = "192.168.0.107" UDP_PORT = 9002 sock = socket.socket(socket.AF_INET, # Internet socket.SOCK_DGRAM) # UDP sock.bind((UDP_IP, UDP_PORT)) pub = rospy.Publisher('gps/fix', NavSatFix, queue_size=10) rospy.init_node('test_relay', anonymous=True) rate = rospy.Rate(100) # 10hz while not rospy.is_shutdown(): data, addr = sock.recvfrom(1024) # buffer size is 1024 bytes print "received message:", data data = re.sub("[A-Za-z]","",str(data)) result = str(data).split(":") print result rospy.loginfo(result) msg = NavSatFix() msg.header = Header() msg.latitude = float(result[1]) msg.longitude = float(result[2]) #rospy.loginfo(msg) pub.publish(msg)
def joy_callback(msg): if msg.buttons[12] == 1: gps_goal = NavSatFix() gps_goal.latitude = 33.726526 gps_goal.longitude = -83.299984 gps_put.publish(gps_goal) if msg.buttons[14] == 1: client.cancel_all_goals()
def publish_gps(self): if self.drone.positionGPS!=None: gps_data = NavSatFix() gps_data.latitude = self.drone.positionGPS[0] gps_data.longitude = self.drone.positionGPS[1]#maybe reversed? gps_data.altitude = self.drone.positionGPS[2] gps_data.header.frame_id = self.unique_id self.gps_pub.publish(gps_data)
def getMsg(): pub_navsat = rospy.Publisher('/rtklib/rover', NavSatFix) pub_pose = rospy.Publisher('/rtklib/pose', PoseStamped) #Initialize the RTKLIB ROS node rospy.init_node('rtklib_messages', anonymous=True) #Define the publishing frequency of the node rate = rospy.Rate(10) #Create a socket sock = socket.socket() #Get the address of the local host host = socket.gethostname() #Connect to the RTKRCV server that is bound to port xxxx port = 5801 sock.connect((host,port)) e2 = 6.69437999014e-3 a = 6378137.0 while not rospy.is_shutdown(): navsat = NavSatFix() ecef_xyz = Point() ecef_pose = Pose() ecef_stampedPose = PoseStamped() ecef_stampedPose = navsat.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = sock.recv(1024) #Split the message msg = msgStr.split() navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2)) ecef_xyz.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0) ecef_xyz.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0) ecef_xyz.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0) ecef_pose.position = ecef_xyz ecef_stampedPose.pose = ecef_pose pub_navsat.publish(navsat) pub_pose.publish(ecef_stampedPose) rate.sleep()
def default(self, ci='unused'): gps = NavSatFix() gps.header = self.get_ros_header() gps.latitude = self.data['x'] gps.longitude = self.data['y'] gps.altitude = self.data['z'] self.publish(gps)
def recieve_gps(self, data ): msg = NavSatFix() msg.latitude = data.vector.x msg.longitude = data.vector.y msg.altitude = data.vector.z msg.header.stamp = data.header.stamp msg.header.frame_id = data.header.frame_id self.publisher['gps'].publish(msg)
def default(self, ci='unused'): """ Publish the data of the gps sensor as a custom ROS NavSatFix message """ gps = NavSatFix() gps.header = self.get_ros_header() # TODO ros.org/doc/api/sensor_msgs/html/msg/NavSatFix.html gps.latitude = self.data['x'] gps.longitude = self.data['y'] gps.altitude = self.data['z'] self.publish(pose)
def address2geo(address): gurl = 'https://maps.googleapis.com/maps/api/geocode/xml?address='+address res = urllib.urlopen(gurl).read() ans = NavSatFix() i1 = res.find('lat') i2 = res.find('/lat',i1) ans.latitude = float(res[i1+4:i2-1]) i1 = res.find('lng') i2 = res.find('/lng',i1) ans.longitude = float(res[i1+4:i2-1]) return ans
def gps_fix_sim(): pub=rospy.Publisher('sensor_msgs/NavSatFix', NavSatFix, queue_size=10) rospy.init_node('gps_sim', anonymous=True) rate = rospy.Rate(2) while not rospy.is_shutdown(): gps_fix = NavSatFix() gps_fix.latitude = 36.6 gps_fix.longitude = -121.1 gps_fix.altitude = 1.0 pub.publish(gps_fix) rate.sleep()
def pub_gps(msg_type, msg, bridge): pub = bridge.get_ros_pub("gps", NavSatFix, queue_size=1) fix = NavSatFix() fix.header.stamp = bridge.project_ap_time(msg) fix.header.frame_id = 'base_footprint' fix.latitude = msg.lat/1e07 fix.longitude = msg.lon/1e07 # NOTE: absolute (MSL) altitude fix.altitude = msg.alt/1e03 fix.status.status = NavSatStatus.STATUS_FIX fix.status.service = NavSatStatus.SERVICE_GPS fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN pub.publish(fix)
def bestpos_handler(self, bestpos): navsat = NavSatFix() # TODO: The timestamp here should come from SPAN, not the ROS system time. navsat.header.stamp = rospy.Time.now() navsat.header.frame_id = self.odom_frame # Assume GPS - this isn't exposed navsat.status.service = NavSatStatus.SERVICE_GPS position_type_to_status = { BESTPOS.POSITION_TYPE_NONE: NavSatStatus.STATUS_NO_FIX, BESTPOS.POSITION_TYPE_FIXED: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_FIXEDHEIGHT: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_FLOATCONV: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_WIDELANE: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_NARROWLANE: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_DOPPLER_VELOCITY: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_SINGLE: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_WAAS: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_PROPAGATED: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_OMNISTAR: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_L1_FLOAT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_IONOFREE_FLOAT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_NARROW_FLOAT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_L1_INT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_WIDE_INT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_NARROW_INT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_RTK_DIRECT_INS: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_SBAS: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_INS_PSRSP: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_RTKFLOAT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_RTKFIXED: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_OMNISTAR: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_OMNISTAR_HP: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_OMNISTAR_XP: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_OMNISTAR_HP: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_OMNISTAR_XP: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_PPP: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_INS_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_INS_PPP: NavSatStatus.STATUS_SBAS_FIX, } navsat.status.status = position_type_to_status.get(bestpos.position_type, NavSatStatus.STATUS_NO_FIX) # Position in degrees. navsat.latitude = bestpos.latitude navsat.longitude = bestpos.longitude # Altitude in metres. navsat.altitude = bestpos.altitude navsat.position_covariance[0] = pow(2, bestpos.latitude_std) navsat.position_covariance[4] = pow(2, bestpos.longitude_std) navsat.position_covariance[8] = pow(2, bestpos.altitude_std) navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN # Ship ito self.pub_navsatfix.publish(navsat)
def convert_gps_to_pose(self, coords): # create NavSatFix msg nav_msg = NavSatFix() nav_msg.header.stamp = rospy.get_rostime() nav_msg.header.frame_id = 'odom' # x is longitude, y is latitude nav_msg.longitude = coords['x'] nav_msg.latitude = coords['y'] # convert using /latlong_goal_node/AddLatLongGoal service # returns PoseStamped response = self.latlong_service(nav_msg) rospy.loginfo("Converted NavSatFix to PoseStamped -> (%f, %f)" % (response.goal_xy.pose.position.x, response.goal_xy.pose.position.y)) return response.goal_xy
def calc_lat_long(init_pos,distance,brng): ''' * calculate the latitude and longitude given distance and bearing * distnace input in [m] brng in [rad] ''' R = 6371.0 * 1000 p2 = NavSatFix() lat1 = init_pos.latitude*PI/180.0 lon1 = init_pos.longitude*PI/180.0 p2.latitude = (asin( sin(lat1)*cos(distance/R) + cos(lat1)*sin(distance/R)*cos(brng))) p2.longitude = (lon1 + atan2(sin(brng)*sin(distance/R)*cos(lat1),cos(distance/R)-sin(lat1)*sin(p2.latitude)))*180 / PI p2.latitude = p2.latitude *180 / PI return p2
def testAutoOriginFromNavSatFix(self): rospy.init_node('test_initialize_origin') nsf_pub = rospy.Publisher('fix', NavSatFix, queue_size=2) origin_sub = self.subscribeToOrigin() self.test_stamp = True nsf_msg = NavSatFix() nsf_msg.latitude = swri['latitude'] nsf_msg.longitude = swri['longitude'] nsf_msg.altitude = swri['altitude'] nsf_msg.header.frame_id = "/far_field" nsf_msg.header.stamp = msg_stamp r = rospy.Rate(10.0) while not rospy.is_shutdown(): nsf_pub.publish(nsf_msg) r.sleep()
def test_telemetry_serializer(self): """Tests telemetry serializer.""" # Set up test data. navsat = NavSatFix() navsat.latitude = 38.149 navsat.longitude = -76.432 navsat.altitude = 30.48 compass = Float64(90.0) json = serializers.TelemetrySerializer.from_msg(navsat, compass) altitude_msl = serializers.meters_to_feet(navsat.altitude) # Compare. self.assertEqual(json["latitude"], navsat.latitude) self.assertEqual(json["longitude"], navsat.longitude) self.assertEqual(json["altitude_msl"], altitude_msl) self.assertEqual(json["uas_heading"], compass.data)
def get_directions_service(req): p = xy2geo(init_pos,req.start) orig = "%f,%f"%(p.latitude,p.longitude) p = xy2geo(init_pos,req.goal) dest = "%f,%f"%(p.latitude,p.longitude) print "From ", orig print "To ", dest print "--------" direction_list,status = get_directions(orig,dest) #print status res = Path() for wp in direction_list: point = NavSatFix() point.latitude = float(wp.start_location[0]) point.longitude = float(wp.start_location[1]) pos = geo2xy(init_pos,point) res.poses.append(pos) return res
def convert_to_pose(self, x, y): if not self.latlong_service: rospy.loginfo("Waiting for AddLatLongGoal service...") rospy.wait_for_service('/latlong_goal_node/AddLatlongGoal') rospy.loginfo("AddLatLongGoal service responded") self.latlong_service = rospy.ServiceProxy('/latlong_goal_node/AddLatlongGoal', AddLatlongGoal) # create NavSatFix msg nav_msg = NavSatFix() nav_msg.header.stamp = rospy.get_rostime() nav_msg.header.frame_id = 'odom' # x is longitude, y is latitude nav_msg.longitude = float(x) nav_msg.latitude = float(y) # convert using /latlong_goal_node/AddLatLongGoal service # returns PoseStamped response = self.latlong_service(nav_msg) rospy.loginfo("Converted NavSatFix to PoseStamped -> (%f, %f)" % (response.goal_xy.pose.position.x, response.goal_xy.pose.position.y)) return response.goal_xy.pose.position.x, response.goal_xy.pose.position.y
def parse_gps(self, data): ''' Given raw data from Jaguar GPS socket, parse and return NavSatFix message. If bad/incomplete data, return None ''' # use regex to parse gpgga_hit = re.search('^\$(GPGGA.+?)\r\n', data) gprmc_hit = re.search('^\$(GPRMC.+?)\r\n', data) if gprmc_hit: # Get estimated heading (not part of standard ROS navsatfix message) gprmc = gprmc_hit.group(0).split(",") try: heading = float(gprmc[8]) except: heading = float("NaN") else: while heading < -180.0: heading += 360.0 while heading > 180.0: heading -= 360.0 finally: self.current_est_heading = heading if gpgga_hit: gpgga = gpgga_hit.group(0).split(",") nav_msg = NavSatFix() # Set header information time = gpgga[1] hrs = float(time[0:1]) mins = float(time[2:3]) secs = float(time[4:5]) nav_msg.header.stamp = rospy.Time.from_sec(hrs * 3600 + mins * 60 + secs) nav_msg.header.frame_id = "gps" # Set GPS Status status = NavSatStatus() status.status = -1 if int(gpgga[6]) == 0 else 0 nav_msg.status = status # Set longitude and latitude lat_str = gpgga[2] lon_str = gpgga[4] lat_degs = float(lat_str[:2]) + (float(lat_str[2:]) / 60.0) lon_degs = float(lon_str[:3]) + (float(lon_str[3:]) / 60.0) nav_msg.latitude = -1 * lat_degs if gpgga[3] == "S" else lat_degs nav_msg.longitude = -1 * lon_degs if gpgga[5] == "W" else lon_degs # Set altitude (Positive is above the WGS 84 ellipsoid) try: nav_msg.altitude = float(gpgga[9]) except: nav_msg.altitude = float("NaN") # Set covariance type to unknown nav_msg.position_covariance_type = 0 return nav_msg else: return None
def test_telemetry_serializer(self): """Tests telemetry serializer.""" # Set up test data. navsat = NavSatFix() navsat.latitude = 38.149 navsat.longitude = -76.432 altitude = Altitude() altitude.amsl = 30.48 pose_stamped = PoseStamped() pose_stamped.pose.orientation.w = 1.0 data = serializers.TelemetrySerializer.from_msg(navsat, altitude, pose_stamped) altitude_msl = serializers.meters_to_feet(altitude.amsl) # Compare. self.assertEqual(data["latitude"], navsat.latitude) self.assertEqual(data["longitude"], navsat.longitude) self.assertEqual(data["altitude_msl"], altitude_msl) self.assertEqual(data["uas_heading"], 90.0)
def receiveGpsOverUdp(): pub = rospy.Publisher('/bozobox/gps/fix', NavSatFix) rospy.init_node('gps_over_udp_receiver') while not rospy.is_shutdown(): data, addr = sock.recvfrom(1024) # buffer size is 1024 bytes #print "received message: >"+data+'<' dataArray = data.split(','); fix = NavSatFix() fix.header.stamp.secs = int(dataArray[0]) fix.header.stamp.nsecs = int(dataArray[1]) fix.latitude = float(dataArray[2]) fix.longitude = float(dataArray[3]) fix.status.status = int(dataArray[4]) print fix.latitude print fix.longitude print fix.status.status pub.publish(fix)
def callback(self): #Publish data at 1 Hz rate = rospy.Rate(1) #Define the RTKRCV server ports corresponding to each of the processing #Modes port = 5801 sock = socket.socket() host = socket.gethostname() sock.connect((host, port)) #WGS84 Parameters e2 = 6.69437999014e-3 a = 6378137.0 #At a rate of 1 hz, create an rtklib message for each socket and publish #the data over while not rospy.is_shutdown(): #Create and RTKLIB and a NavSatFix data structure rtk = rtklib() navsat = NavSatFix() #Set the data structure header time to the current system time navsat.header.stamp = rospy.Time.now() rtk.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = socket.recv(1024) #Split the message msg = msgStr.split() #Save the latitude, longitude and ellipsoid height navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) #Save the position covariance navsat.position_covariance = [float(msg[7]),float(msg[10]),float(msg[12]),float(msg[10]),float(msg[8]),float(msg[11]),float(msg[12]),float(msg[11]),float(msg[9])] navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN #Compute the radius of curvature in the N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2)) #Compute and store the ECEF position rtk.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0) rtk.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0) rtk.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0) rtk.delay = float(msg[13]) rtk.ftest = float(msg[14]) #Store the NavSatFix data rtk.state = navsat pubs[i].publish(rtk) rate.sleep()
def callback(sensorstr): data = sensorstr.data if data[0] == 'Z': data = data.replace("Z","").replace("\n","").replace("\r","") data_list = data.split(',') if len(data_list) == 4: try: ##data_list structure: lat, lon, heading, vel float_list = [float(i) for i in data_list] poseGPS = NavSatFix() poseGPS.header.frame_id = "world" poseGPS.header.stamp = rospy.Time.now() poseGPS.status.status = 0 poseGPS.status.service = 1 poseGPS.latitude = float_list[0] poseGPS.longitude = float_list[1] poseGPS.altitude = 0.0 poseGPS.position_covariance = [3.24, 0, 0, 0, 3,24, 0, 0, 0, 0] poseGPS.position_covariance_type = 2 try: pubGPS.publish(poseGPS) log = "GPS Pose Data: %f %f Publish at Time: %s" % (float_list[0], float_list[1], rospy.get_time()) except: log = "poseGPS Publisher Error" except: log = "GPS Data Error! Data : %s" % data rospy.loginfo(log) rospy.loginfo(log)
def ExtFix2Fix(data): fix = NavSatFix() fix.header = data.header fix.status.status = data.status.status fix.status.service = data.status.position_source fix.latitude = data.latitude fix.longitude = data.longitude fix.altitude = data.altitude fix.position_covariance = data.position_covariance fix.position_covariance_type = data.position_covariance_type return fix
def talker(): pub = rospy.Publisher("/gps/fix", NavSatFix, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) while not rospy.is_shutdown(): #gps config gpsd.next() os.system('clear') gps_data = NavSatFix() #status of gps # gps_data.status.status = 0 # gps_data.status.service = 1 #time gps_data.header.stamp.secs = round(time.time()) #position gps_data.latitude = gpsd.fix.latitude gps_data.longitude = gpsd.fix.longitude gps_data.altitude = 0 # gps_data.position_covariance_type = 0 #publication ros pub.publish(gps_data) rate.sleep()
def talker(): # Subscribe to Vicon messages viconTopic = rospy.get_param('topic') rospy.Subscriber(viconTopic, TransformStamped, callback) # Start a publisher for the GPS messages pub = rospy.Publisher('GPS/position', NavSatFix) # FIXME # Start the node rospy.init_node('talker') # Populate the NavSatFix message from the parameter server statusMsg = NavSatStatus() statusMsg.status = rospy.get_param('status', -1) statusMsg.service = rospy.get_param('service', 1) fixMsg = NavSatFix() fixMsg.header.stamp = rospy.Time.now() fixMsg.header.frame_id = "/world" fixMsg.status = statusMsg fixMsg.position_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] #position could be modified with some gaussian noise added to it and then calculate the covariance matrix. fixMsg.position_covariance_type = rospy.get_param('position_covariance_type', 0); while not rospy.is_shutdown(): [fixMsg.longitude, fixMsg.latitude, fixMsg.altitude] = c.xyz2gps([parentFrameLong, parentFrameLat, parentFrameAlt], latestViconMsg.transform.translation.x, latestViconMsg.transform.translation.y, latestViconMsg.transform.translation.z, parentFrameAngle) statusMsg.status = rospy.get_param('status', statusMsg.status) statusMsg.service = rospy.get_param('service', statusMsg.service) # put the sigma and calculate the cov matrix here #rospy.loginfo([fixMsg.longitude, fixMsg.latitude, fixMsg.altitude]) pub.publish(fixMsg) rospy.sleep(0.1)
def publish(self, data): msg = NavSatFix() msg.header.frame_id = self._frameId msg.header.stamp = rospy.get_rostime() msg.latitude = data.getLat() msg.longitude = data.getLng() msg.altitude = data.getMeters() if data.getFix() == 1: msg.status.status = 0 else: msg.status.status = -1 msg.position_covariance_type = 1 msg.position_covariance[0] = data.getHDOP() * data.getHDOP() msg.position_covariance[4] = data.getHDOP() * data.getHDOP() msg.position_covariance[8] = 4 * data.getHDOP() * data.getHDOP() msg.status.service = 1 self._pub.publish(msg)
def callback(self): rate = rospy.Rate(10) ports = [5801,5802,5803] sockets = [] pubs = [self.static_pub, self.rtkStatic_pub, self.rtkDynamic_pub] for i in ports: sock = socket.socket() host = socket.gethostname() sock.connect((host, i)) sockets.append(sock) e2 = 6.69437999014e-3 a = 6378137.0 while not rospy.is_shutdown(): for i in range(len(sockets)): navsat = NavSatFix() ecef_xyz = Point() ecef_pose = Pose() #ecef_stampedPose = PoseStamped() navsat.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = sockets[i].recv(1024) #Split the message msg = msgStr.split() navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) navsat.position_covariance = [float(msg[7]),float(msg[10]),float(msg[12]),float(msg[10]),float(msg[8]),float(msg[11]),float(msg[12]),float(msg[11]),float(msg[9])] navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2)) ecef_xyz.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0) ecef_xyz.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0) ecef_xyz.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0) ecef_pose.position = ecef_xyz #ecef_stampedPose.pose = ecef_pose pubs[i].publish(navsat) rate.sleep()
def subCB(msg_in): global pub msg_out = NavSatFix() msg_out.header = msg_in.header msg_out.status.status = NavSatStatus.STATUS_FIX # TODO - fix this msg_out.status.service = NavSatStatus.SERVICE_GPS msg_out.latitude = msg_in.LLA.x msg_out.longitude = msg_in.LLA.y msg_out.altitude = msg_in.LLA.z msg_out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED msg_out.position_covariance[1] = msg_in.PosUncerainty pub.publish(msg_out)
def __init__(self, *args): super(MavrosQuad, self).__init__(*args) self.altitude = Altitude() self.extended_state = ExtendedState() self.global_position = NavSatFix() self.home_position = HomePosition() self.local_position = PoseStamped() self.mission_wp = WaypointList() self.state = State() self.mav_type = None self.local_velocity = TwistStamped() self.gazebo_load_name = 'rigid_body_load_1_vehicle::rb_link' self.gazebo_load_pose = Pose() self.gazebo_load_twist = Twist() self.gazebo_quad_name = 'rigid_body_load_1_vehicle::base_link_0' self.gazebo_quad_pose = Pose() self.gazebo_quad_twist = Twist() self.gazebo_imu_name = 'rigid_body_load_1_vehicle::iris_0/imu_link' self.gazebo_imu_pose = Pose() self.gazebo_imu_twist = Twist() self.q_input = np.array([0, 0, 0, 1]) self.T_input = 0 self.offboard_position_active = True self.offboard_attitude_active = False self.offboard_load_active = False self.sub_topics_ready = { key: False for key in [ 'alt', 'ext_state', 'gazebo', 'global_pos', 'home_pos', 'local_pos', 'local_vel', 'mission_wp', 'state', 'imu' ] } # ROS services service_timeout = 30 rospy.loginfo("waiting for ROS services") try: rospy.wait_for_service('mavros/param/get', service_timeout) rospy.wait_for_service('mavros/cmd/arming', service_timeout) rospy.wait_for_service('mavros/mission/push', service_timeout) rospy.wait_for_service('mavros/mission/clear', service_timeout) rospy.wait_for_service('mavros/set_mode', service_timeout) rospy.loginfo("ROS services are up") except rospy.ROSException: rospy.logerr("failed to connect to services") self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet) self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode) self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear', WaypointClear) self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push', WaypointPush) # ROS subscribers self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude, self.altitude_callback) self.ext_state_sub = rospy.Subscriber('mavros/extended_state', ExtendedState, self.extended_state_callback) self.global_pos_sub = rospy.Subscriber('mavros/global_position/global', NavSatFix, self.global_position_callback) self.home_pos_sub = rospy.Subscriber('mavros/home_position/home', HomePosition, self.home_position_callback) self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped, self.local_position_callback) self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints', WaypointList, self.mission_wp_callback) self.state_sub = rospy.Subscriber('mavros/state', State, self.state_callback) self.local_vel_sub = rospy.Subscriber( 'mavros/local_position/velocity_local', TwistStamped, self.local_velocity_callback) self.imu_sub = rospy.Subscriber('/mavros/imu/data', Imu, self.imu_callback) self.gazebo_sub = rospy.Subscriber('/gazebo/link_states', LinkStates, self.gazebo_callback) # ROS Publishers # Attitude self.att = AttitudeTarget() self.att_setpoint_pub = rospy.Publisher('mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1) # send setpoints in seperate thread to better prevent failsafe self.att_thread = Thread(target=self.send_att, args=()) self.att_thread.daemon = True self.att_thread.start() # Pose self.pos = PoseStamped() self.pos_setpoint_pub = rospy.Publisher( 'mavros/setpoint_position/local', PoseStamped, queue_size=1) # send setpoints in seperate thread to better prevent failsafe self.pos_thread = Thread(target=self.send_pos, args=()) self.pos_thread.daemon = True self.pos_thread.start()
def Start(): rospy.init_node('cubigolf_localization', anonymous=True) #Setup the publishers imu_pub = rospy.Publisher("/cubigolf/imu", Imu, queue_size= 10) navsat_pub = rospy.Publisher("/cubigolf/gps", NavSatFix, queue_size=10) magneto_pub = rospy.Publisher("/cubigolf/magneticfield", MagneticField, queue_size=10) odom_pub = rospy.Publisher("/cubigolf/odom", Odometry, queue_size=10) freq = 100 r = rospy.Rate(freq) # 10hz imu = Imu() gps = NavSatFix() odom = Odometry() magneto = MagneticField() wheelspeed = Float32() steeringangle = Float32() vel_oldx = vel_oldy = angle_old = 0 #Start deserializing the code coming in on the serial port myDeco = UartPLCDeserializer('/dev/ttyUSB0') deserializerThread = threading.Thread(target=myDeco.Main, args=()) deserializerThread.start() while not rospy.is_shutdown(): #Populate IMU imu.header.stamp = rospy.Time.now() imu.orientation.x = myDeco.variables.imu_quatX['value'] imu.orientation.y = myDeco.variables.imu_quatY['value'] imu.orientation.z = myDeco.variables.imu_quatZ['value'] imu.orientation.w = myDeco.variables.imu_quatW['value'] imu.angular_velocity.x = myDeco.variables.imu_gyroX_rads['value'] imu.angular_velocity.y = myDeco.variables.imu_gyroY_rads['value'] imu.angular_velocity.z = myDeco.variables.imu_gyroZ_rads['value'] imu.linear_acceleration.x = myDeco.variables.imu_accX_ms2['value'] imu.linear_acceleration.y = myDeco.variables.imu_accY_ms2['value'] imu.linear_acceleration.z = myDeco.variables.imu_accZ_ms2['value'] imu_pub.publish(imu) #Populate the magneto message magneto.header.stamp = rospy.Time.now() magneto.magnetic_field.x = myDeco.variables.imu_magX_ut['value'] magneto.magnetic_field.y = myDeco.variables.imu_magY_ut['value'] magneto.magnetic_field.z = myDeco.variables.imu_magZ_ut['value'] magneto_pub.publish(magneto) #Populate navfix message gps.header.stamp = rospy.Time.now() gps.latitude = myDeco.variables.gps_lat_dms['value'] gps.longitude = myDeco.variables.gps_long_dms['value'] gps.altitude = myDeco.variables.gps_altitude_m['value'] print gps.latitude, gps.longitude, gps.altitude #NMEA 0 = Invalid;1 = GPS fix, 2 = DGPS fix #ROS NavSatStatus #int8 STATUS_NO_FIX = -1 # unable to fix position #int8 STATUS_FIX = 0 # unaugmented fix #int8 STATUS_SBAS_FIX = 1 # with satellite-based augmentation #int8 STATUS_GBAS_FIX = 2 # with ground-based augmentation if myDeco.variables.gps_fix['value'] == 0: gps.status.status = NavSatStatus.STATUS_NO_FIX elif myDeco.variables.gps_fix['value'] == 1: gps.status.status = NavSatStatus.STATUS_FIX elif myDeco.variables.gps_fix['value'] == 2: gps.status.status = NavSatStatus.STATUS_SBAS_FIX # TODO Check FIX assumptions navsat_pub.publish(gps) #TODO Define an odometry frame for an ackermann platfrom odom.child_frame_id="base_link" odom.twist.twist.linear.x = 0 odom.twist.twist.linear.y = 0 odom.child_frame_id = "odom" odom.pose.pose.position.x = 0 odom.pose.pose.position.y = 0 odom.pose.pose.orientation.x = 0 odom.pose.pose.orientation.y = 0 odom.pose.pose.orientation.z = 0 odom.pose.pose.orientation.w = 1 odom_pub.publish(odom) r.sleep()
def __init__(self, this_uav, NUM_UAV, D_GAIN): print 'init' print 'this uav = ' + str(this_uav) print 'num uav = ' + str(NUM_UAV) print 'dir_path = ' + str(os.path.dirname(os.path.realpath(__file__))) print 'cwd = ' + str(os.getcwd()) global cur_pose global cur_state global cur_vel global cur_globalPose global command cur_pose = [PoseStamped() for i in range(NUM_UAV)] cur_globalPose = [NavSatFix() for i in range(NUM_UAV)] cur_state = [State() for i in range(NUM_UAV)] cur_vel = [TwistStamped() for i in range(NUM_UAV)] command = Float64MultiArray() command.data = [0, 0, 0, -1] des_pose = PoseStamped() des_vel = TwistStamped() des_vel.header.frame_id = "map" status = Int8() status.data = -1 convergence = 0 t = 0 #time at start of rotation ttR = 20 #time to rotate comDisk = 0 #40 #size of communication disk between drones #create pid controllers for R and Theta (Kp, Ki, Kd) cmdR = pid.PID(50, 0, 0) cmdTheta = pid.PID(50, 25, 0) rospy.init_node('offboard_test' + str(this_uav), anonymous=True) rate = rospy.Rate(100) #Hz mode_proxy = rospy.ServiceProxy( 'mavros' + str(this_uav + 1) + '/set_mode', SetMode) arm_proxy = rospy.ServiceProxy( 'mavros' + str(this_uav + 1) + '/cmd/arming', CommandBool) paramPull_proxy = rospy.ServiceProxy( 'mavros' + str(this_uav + 1) + '/param/pull', ParamPull) paramGet_proxy = rospy.ServiceProxy( 'mavros' + str(this_uav + 1) + '/param/get', ParamGet) paramSet_proxy = rospy.ServiceProxy( 'mavros' + str(this_uav + 1) + '/param/set', ParamSet) print('paramPull - \n' + str(paramPull_proxy(True))) print('paramGet MAV_TYPE - \n' + str(paramGet_proxy("MAV_TYPE"))) print( '______________________________________________________________________________' ) #generating subscribers to ALL drones in the swarm for i in range(NUM_UAV): exec('def position_cb' + str(i) + '(msg): cur_pose[' + str(i) + '] = msg') exec('def globalPosition_cb' + str(i) + '(msg): cur_globalPose[' + str(i) + '] = msg') exec('def velocity_cb' + str(i) + '(msg): cur_vel[' + str(i) + '] = msg') exec('def state_cb' + str(i) + '(msg): cur_state[' + str(i) + '] = msg') rospy.Subscriber('/mavros' + str(i + 1) + '/local_position/pose', PoseStamped, callback=eval('position_cb' + str(i))) rospy.Subscriber('/mavros' + str(i + 1) + '/global_position/global', NavSatFix, callback=eval('globalPosition_cb' + str(i))) rospy.Subscriber('/mavros' + str(i + 1) + '/state', State, callback=eval('state_cb' + str(i))) rospy.Subscriber('/mavros' + str(i + 1) + '/local_position/velocity', TwistStamped, callback=eval('velocity_cb' + str(i))) #suscribe to sequencer rospy.Subscriber('/sequencer/command', Float64MultiArray, callback=self.command_cb) #publish status to sequencer status_pub = rospy.Publisher('/sequencer/status' + str(this_uav), Int8, queue_size=10) #publish position setpoints to FCU pose_pub = rospy.Publisher('/mavros' + str(this_uav + 1) + '/setpoint_position/local', PoseStamped, queue_size=10) #publish velocity setpoints to FCU vel_pub = rospy.Publisher('/mavros' + str(this_uav + 1) + '/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10) #2nd subscriber to uav-state [temporary] rospy.Subscriber('/mavros' + str(this_uav + 1) + '/state', State, callback=self.state_cb) #assigning identities for drone ahead and behind in formation #only relevant to rotations in cmd 4 plus_uav = (this_uav + 1) % NUM_UAV minus_uav = (this_uav - 1) % NUM_UAV #GOTO initial holding pattern command = [0,0,25,0] des_pose = cur_pose[this_uav] des_pose.pose.position.x = 10 * math.sin( (this_uav * 2 * math.pi) / NUM_UAV) des_pose.pose.position.y = 10 * math.cos( (this_uav * 2 * math.pi) / NUM_UAV) des_pose.pose.position.z = 20 pose_pub.publish(des_pose) status_pub.publish(status) #....fix....double subscribe to status, check and set status in callback? print cur_state[this_uav] print '---------------arming-----------------------' print '----WAITING FOR STANDBY STATUS---' while True: pose_pub.publish(des_pose) if cur_state[this_uav].system_status == 3: print cur_state[this_uav] break print 'INITIAL POSITION' print cur_globalPose[this_uav] while cur_state[this_uav].mode != 'OFFBOARD' and not cur_state[ this_uav].armed: print 'intial arming' mode_sent = False success = False pose_pub.publish(des_pose) while not mode_sent: rospy.wait_for_service('mavros' + str(this_uav + 1) + '/set_mode', timeout=None) try: mode_sent = mode_proxy(1, 'OFFBOARD') except rospy.ServiceException as exc: print exc rate.sleep() while not success: rospy.wait_for_service('mavros' + str(this_uav + 1) + '/cmd/arming', timeout=None) try: success = arm_proxy(True) except rospy.ServiceException as exc: print exc rate.sleep() print 'mode_sent - ' + str(mode_sent) print 'arm_sent - ' + str(success) print 'armed?' rate.sleep() print cur_state[this_uav] #wait for sequencer to connect to /sequencer/status# nc = status_pub.get_num_connections() print 'num_connections = ' + str(nc) sys.stdout.flush() while nc == 0: nc = status_pub.get_num_connections() rate.sleep() print 'num_connections = ' + str(nc) print 'FLAG' print rospy.is_shutdown() sys.stdout.flush() rate.sleep() print rospy.is_shutdown() #MAIN LOOP print 'Loop INIT Time - ' + str(time.clock()) while not rospy.is_shutdown(): #''' #hopefully temporary hack while cur_state[this_uav].mode != 'OFFBOARD' and not cur_state[ this_uav].armed: print '------------------rearming----------------------------' mode_sent = False success = False pose_pub.publish(des_pose) while not mode_sent: rospy.wait_for_service('mavros' + str(this_uav + 1) + '/set_mode', timeout=None) try: mode_sent = mode_proxy(1, 'OFFBOARD') except rospy.ServiceException as exc: print exc while not success: rospy.wait_for_service('mavros' + str(this_uav + 1) + '/cmd/arming', timeout=None) try: success = arm_proxy(True) except rospy.ServiceException as exc: print exc #temp end #''' if command.data[3] > -1 and command.data[3] < 4: des_pose.pose.position.x = command.data[ 0] + command.data[2] * math.sin( (this_uav * 2 * math.pi) / NUM_UAV) des_pose.pose.position.y = command.data[ 1] + command.data[2] * math.cos( (this_uav * 2 * math.pi) / NUM_UAV) des_pose.pose.position.z = 25 convergence = math.sqrt( math.pow( des_pose.pose.position.x - cur_pose[this_uav].pose.position.x, 2) + math.pow( des_pose.pose.position.y - cur_pose[this_uav].pose.position.y, 2) + math.pow( des_pose.pose.position.z - cur_pose[this_uav].pose.position.z, 2)) if convergence < .5 and status != Int8(command.data[3]): status = Int8(command.data[3]) print 'Status Set - ' + str(status) + ' time - ' + str( time.clock()) print 'Current Pose - ' + str(cur_pose[this_uav].pose) status_pub.publish(status) pose_pub.publish(des_pose) if command.data[3] == 4: #timing on rotation if t == 0: t = time.clock() print t if time.clock() > t + ttR: status = Int8(command.data[3]) print 'Status Set - ' + str(status) + ' time - ' + str( time.clock()) status_pub.publish(status) ttR = 1000000000000 r = math.sqrt( math.pow(cur_pose[this_uav].pose.position.x, 2) + math.pow(cur_pose[this_uav].pose.position.y, 2)) theta = math.atan2(cur_pose[this_uav].pose.position.y, cur_pose[this_uav].pose.position.x) % ( 2 * math.pi) #from [-pi,pi] -> [0, 2pi] theta_plus = math.atan2( cur_pose[plus_uav].pose.position.y, cur_pose[plus_uav].pose.position.x) % (2 * math.pi) theta_minus = math.atan2( cur_pose[minus_uav].pose.position.y, cur_pose[minus_uav].pose.position.x) % (2 * math.pi) #deal with wrap around if theta_minus < theta: #yea looks backwards dtheta_minus = (theta_minus + 2 * math.pi) - theta else: dtheta_minus = theta_minus - theta if theta < theta_plus: #...again...backwards dtheta_plus = (theta + 2 * math.pi) - theta_plus else: dtheta_plus = theta - theta_plus if comDisk: #do stuff distPlus = math.sqrt( math.pow( cur_pose[this_uav].pose.position.x - cur_pose[plus_uav].pose.position.x, 2) + math.pow( cur_pose[this_uav].pose.position.y - cur_pose[plus_uav].pose.position.y, 2)) distMinus = math.sqrt( math.pow( cur_pose[this_uav].pose.position.x - cur_pose[minus_uav].pose.position.x, 2) + math.pow( cur_pose[this_uav].pose.position.y - cur_pose[minus_uav].pose.position.y, 2)) if distPlus > comDisk or distMinus > comDisk: print 'comDisk' print ' distPlus - ' + str(distPlus) print ' distMinus - ' + str(distMinus) if distPlus > comDisk and distMinus > comDisk: dtheta_plus = dtheta_minus = 2 elif distPlus > comDisk: dtheta_plus = 2 dtheta_minus = 0 elif distMinus > comDisk: dtheta_minus = 0 dtheta_plus = .5 thetaError = (dtheta_minus - dtheta_plus) rError = command.data[2] - r thetaDot = cmdTheta.update(thetaError) + 5 rDot = cmdR.update(rError) if thetaDot < 0: thetaDot = 0 des_vel.twist.linear.x = (rDot * math.cos(theta) - r * thetaDot * math.sin(theta)) * 2 des_vel.twist.linear.y = (rDot * math.sin(theta) + r * thetaDot * math.cos(theta)) * 2 des_vel.twist.linear.z = ( 25 - cur_pose[this_uav].pose.position.z) * .5 vel_pub.publish(des_vel) print theta if command.data[3] == 5: config = np.loadtxt('/simulation/config0_0.txt') status = Int8(command.data[3]) print 'Status Set - ' + str(status) + ' time - ' + str( time.clock()) status_pub.publish(status) if command.data[3] > 5: des_pose.pose.position.x = config[int(command.data[3] - 6), (4 * this_uav)] des_pose.pose.position.y = config[int(command.data[3] - 6), (4 * this_uav) + 1] des_pose.pose.position.z = 20 + (2 * this_uav) convergence = math.sqrt( math.pow( des_pose.pose.position.x - cur_pose[this_uav].pose.position.x, 2) + math.pow( des_pose.pose.position.y - cur_pose[this_uav].pose.position.y, 2) + math.pow( des_pose.pose.position.z - cur_pose[this_uav].pose.position.z, 2)) if convergence < .5 and status != Int8(command.data[3]): status = Int8(command.data[3]) print 'Status Set - ' + str(status) + ' time - ' + str( time.clock()) print 'Current Pose - ' + str(cur_pose[this_uav].pose) print 'Cmd Pose - (' + str( config[int(command.data[3] - 6), (4 * this_uav)]) + ',' + str( config[int(command.data[3] - 6), (4 * this_uav) + 1]) + ')' status_pub.publish(status) pose_pub.publish(des_pose) rate.sleep()
def navigation_handler(self, data): """ Rebroadcasts navigation data in the following formats: 1) /odom => /base footprint transform (if enabled, as per REP 105) 2) Odometry message, with parent/child IDs as in #1 3) NavSatFix message, for systems which are knowledgeable about GPS stuff 4) IMU messages """ rospy.logdebug("Navigation received") # If we don't have a fix, don't publish anything... if self.nav_status.status == NavSatStatus.STATUS_NO_FIX: return # Changing from NED from the Applanix to ENU in ROS # Roll is still roll, since it's WRT to the x axis of the vehicle # Pitch is -ve since axis goes the other way (+y to right vs left) # Yaw (or heading) in Applanix is clockwise starting with North # In ROS it's counterclockwise startin with East orient = PyKDL.Rotation.RPY(RAD(data.roll), RAD(-data.pitch), RAD(90 - data.heading)).GetQuaternion() # UTM conversion utm_pos = geodesy.utm.fromLatLong(data.latitude, data.longitude) # Initialize starting point if we haven't yet if not self.init and self.zero_start: self.origin.x = utm_pos.easting self.origin.y = utm_pos.northing self.origin.z = data.altitude self.init = True origin_param = { "east": self.origin.x, "north": self.origin.y, "alt": self.origin.z, } rospy.set_param('/gps_origin', origin_param) # Publish origin reference for others to know about p = Pose() p.position.x = self.origin.x p.position.y = self.origin.y p.position.z = self.origin.z self.pub_origin.publish(p) # # Odometry # TODO: Work out these covariances properly from DOP # odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = self.odom_frame odom.child_frame_id = self.base_frame odom.pose.pose.position.x = utm_pos.easting - self.origin.x odom.pose.pose.position.y = utm_pos.northing - self.origin.y odom.pose.pose.position.z = data.altitude - self.origin.z odom.pose.pose.orientation = Quaternion(*orient) odom.pose.covariance = POSE_COVAR # Twist is relative to /reference frame or /vehicle frame and # NED to ENU conversion is needed here too odom.twist.twist.linear.x = data.east_vel odom.twist.twist.linear.y = data.north_vel odom.twist.twist.linear.z = -data.down_vel odom.twist.twist.angular.x = RAD(data.ang_rate_long) odom.twist.twist.angular.y = RAD(-data.ang_rate_trans) odom.twist.twist.angular.z = RAD(-data.ang_rate_down) odom.twist.covariance = TWIST_COVAR self.pub_odom.publish(odom) t_tf = odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z q_tf = Quaternion(*orient).x, Quaternion(*orient).y, Quaternion( *orient).z, Quaternion(*orient).w # # Odometry transform (if required) # if self.publish_tf: self.tf_broadcast.sendTransform(t_tf, q_tf, odom.header.stamp, odom.child_frame_id, odom.header.frame_id) # # NavSatFix # TODO: Work out these covariances properly from DOP # navsat = NavSatFix() navsat.header.stamp = rospy.Time.now() navsat.header.frame_id = self.odom_frame navsat.status = self.nav_status navsat.latitude = data.latitude navsat.longitude = data.longitude navsat.altitude = data.altitude navsat.position_covariance = NAVSAT_COVAR navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN self.pub_navsatfix.publish(navsat) # # IMU # TODO: Work out these covariances properly # imu = Imu() imu.header.stamp = rospy.Time.now() imu.header.frame_id = self.base_frame # Orientation imu.orientation = Quaternion(*orient) imu.orientation_covariance = IMU_ORIENT_COVAR # Angular rates imu.angular_velocity.x = RAD(data.ang_rate_long) imu.angular_velocity.y = RAD(-data.ang_rate_trans) imu.angular_velocity.z = RAD(-data.ang_rate_down) imu.angular_velocity_covariance = IMU_VEL_COVAR # Linear acceleration imu.linear_acceleration.x = data.long_accel imu.linear_acceleration.y = data.trans_accel imu.linear_acceleration.z = data.down_accel imu.linear_acceleration_covariance = IMU_ACCEL_COVAR self.pub_imu.publish(imu) pass
def parse_novatelINSPVA(insHeader, insString): #print "++++instring:",insString gnssWeek = insString[0] # GNSS week secondsFromWeek = insString[1] # Seconds from week start latitude = insString[2] # Latitude (WGS84) longitude = insString[3] # Longitude (WGS84) heightMSL = insString[4] # Ellipsoidal Height (WGS84) [m] velcnsY = float( insString[5] ) # Velocity in northerly direction [m/s] (negative for south) velcnsX = float( insString[6] ) # Velocity in easterly direction [m/s] (negative for west) velcnsZ = float(insString[7]) # Velocity in upward direction [m/s] cnsYaw = float(insString[8]) * ( 3.14159265 / 180 ) # yaw/azimuth - Right-handed rotation from local level around Z-axis -- changed from degress to radians (pi/180 deg) cnsRoll = float(insString[9]) * ( 3.14159265 / 180 ) # roll - (neg) Right-handed rotation from local level around y-axis -- changed from degress to radians (pi/180 deg) cnsPitch = float(insString[10]) * ( 3.14159265 / 180 ) # pitch -Right-handed rotation from local level around x-axis -- changed from degress to radians (pi/180 deg) inertialStatus = insString[11].split('*')[0] # Inertial status #print "inertialStatus",inertialStatus fix_msg = NavSatFix() fix_msg.header.stamp = rospy.get_rostime() fix_msg.header.frame_id = 'cns5000_frame' fix_msg.latitude = float(latitude) fix_msg.longitude = float(longitude) fix_msg.altitude = float(heightMSL) fix_msg.position_covariance = [ 0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 999.0 ] if (insHeader[4] == "FINESTEERING"): fix_msg.position_covariance_type = 2 else: fix_msg.position_covariance_type = 0 #print "lat, long, alt:" + str(fix_msg.latitude)+ " , "+ str(fix_msg.longitude)+" , " + str(fix_msg.altitude) global curTime global curRoll global curYaw global curPitch global lastRoll global lastYaw global lastPitch global curvelcnsX global curvelcnsY global curvelcnsZ global lastvelcnsX global lastvelcnsY global lastvelcnsZ #cns to imu coordinate sytem conversion --> cnsX = -imuY, cnsY = imuX, cnsZ = imuZ #imuY comes in as negative of value --> cancels out negative conversion imuYaw = -1 * cnsPitch + 1.57079632679 #rotate 90 deg, or pi/2 radians #print(imuYaw) imuPitch = cnsRoll imuRoll = cnsYaw lastYaw = curYaw curYaw = imuYaw lastPitch = curPitch curPitch = imuPitch lastRoll = curRoll curRoll = imuRoll lastvelcnsX = curvelcnsX curvelcnsX = velcnsX lastvelcnsY = curvelcnsY curvelcnsY = velcnsY lastvelcnsZ = curvelcnsZ curvelcnsZ = velcnsZ lastTime = curTime curTime = rospy.Time.now() deltime = (curTime - lastTime).to_sec() if (deltime == 0): deltime = 0.00000001 imu_msg = Imu() imu_msg.header.stamp = curTime imu_msg.header.frame_id = 'cns5000_frame' imu_msg.linear_acceleration.x = (curvelcnsY - lastvelcnsY) / deltime #*.05/pow(2,15) imu_msg.linear_acceleration.y = ( curvelcnsY - lastvelcnsY) / deltime #*-1#*.05/pow(2,15) imu_msg.linear_acceleration.z = (curvelcnsY - lastvelcnsY) / deltime #*.05/pow(2,15) imu_msg.linear_acceleration_covariance = [ 0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1 ] #imu_msg.orientation_covariance.x = float(angcnsY) #euler = Vector3(curRoll, curPitch, curYaw) #euler = vector_norm(euler) quaternion = tf.transformations.quaternion_from_euler(0, 0, curYaw) #print(curRoll, curPitch, curYaw) #type(pose) = geometry_msgs.msg.Pose # DML Next two lines additions to normalize the quaternion quat = numpy.array( [quaternion[0], quaternion[1], quaternion[2], quaternion[3]]) quaternion[0], quaternion[1], quaternion[2], quaternion[ 3] = quat / numpy.sqrt(numpy.dot(quat, quat)) imu_msg.orientation.x = quaternion[0] imu_msg.orientation.y = quaternion[1] imu_msg.orientation.z = quaternion[2] imu_msg.orientation.w = quaternion[3] imu_msg.orientation_covariance = [ 0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1 ] imu_msg.angular_velocity.x = (curPitch - lastPitch) / deltime imu_msg.angular_velocity.y = (curRoll - lastRoll) / deltime imu_msg.angular_velocity.z = (curYaw - lastYaw) / deltime imu_msg.angular_velocity_covariance = [ 0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1 ] #velx = float(velEast)*.05/pow(2,15) #vely = float(velNorth)*.05/pow(2,15) #velz = float(velUp)*.05/pow(2,15) #imu_msg = Vector3(velx,vely,velz,pitch,roll) #imu_msg.orientation.w = 0.01 retrnList = [imu_msg, fix_msg] return retrnList
ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_VELECEF, 1) ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_POSECEF, 1) ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_RAW, 1) ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_SFRB, 1) ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_SVSI, 1) ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_ALM, 1) ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_EPH, 1) ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_TIMEGPS, 5) ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_CLOCK, 5) while not rospy.is_shutdown(): msg = ubl.receive_message() if msg is None: pass if msg.name() == "NAV_POSLLH": outstr = str(msg).split(",")[1:] outstr = "".join(outstr) components = search(" Longitude={:d} Latitude={:d} Height={:d}", outstr) h = Header() h.stamp = rospy.Time.now() navmsg = NavSatFix() navmsg.header = h navmsg.latitude = float(components[1]) / 10000000.0 navmsg.longitude = float(components[0]) / 10000000.0 navmsg.altitude = float(components[2]) / 10000000.0 publisher.publish(navmsg)
import rospy from sensor_msgs.msg import NavSatFix import sys if len(sys.argv) < 4: print('usage: {} lat lng alt'.format(sys.argv[0])) exit() lat = float(sys.argv[1]) lng = float(sys.argv[2]) alt = float(sys.argv[3]) rospy.init_node("target_gps_broadcaster") pub = rospy.Publisher('target_gps_fix', NavSatFix, queue_size=1) rospy.sleep(1) fix = NavSatFix() fix.header.stamp = rospy.Time.now() fix.header.frame_id = 'gps_frame' fix.latitude = lat fix.longitude = lng fix.altitude = alt fix.status.status = 0 fix.status.service = 1 pub.publish(fix)
def callback(self): rate = rospy.Rate(10) ports = [5801, 5802, 5803] sockets = [] pubs = [self.static_pub, self.rtkStatic_pub, self.rtkDynamic_pub] for i in ports: sock = socket.socket() host = socket.gethostname() sock.connect((host, i)) sockets.append(sock) e2 = 6.69437999014e-3 a = 6378137.0 while not rospy.is_shutdown(): for i in range(len(sockets)): navsat = NavSatFix() ecef_xyz = Point() ecef_pose = Pose() #ecef_stampedPose = PoseStamped() navsat.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = sockets[i].recv(1024) #Split the message msg = msgStr.split() navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) navsat.position_covariance = [ float(msg[7]), float(msg[10]), float(msg[12]), float(msg[10]), float(msg[8]), float(msg[11]), float(msg[12]), float(msg[11]), float(msg[9]) ] navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN N = 1.0 * a / np.sqrt( 1 - e2 * (np.sin(float(msg[2]) * np.pi / 180.0)**2)) ecef_xyz.x = (N + float(msg[4])) * np.cos( float(msg[2]) * np.pi / 180.0) * np.cos( float(msg[3]) * np.pi / 180.0) ecef_xyz.y = (N + float(msg[4])) * np.cos( float(msg[2]) * np.pi / 180.0) * np.sin( float(msg[3]) * np.pi / 180.0) ecef_xyz.z = (N * (1 - e2) + float(msg[4])) * np.sin( float(msg[2]) * np.pi / 180.0) ecef_pose.position = ecef_xyz #ecef_stampedPose.pose = ecef_pose pubs[i].publish(navsat) rate.sleep()
def __init__(self, default_port='/dev/ttyUSB1'): """ @param default_port: default serial port to use for establishing a connection to the UM7 IMU sensor. This will be overridden by ~port param if available. """ rospy.init_node('imu_um7') self.port = rospy.get_param('~port', default_port) self.frame_id = rospy.get_param('~frame_id', "/imu") self.gps_frame_id = rospy.get_param('~gps_frame_id', "/gps") self.throttle_rate = rospy.get_param('~throttle_rate', 10000) self.reset_mag = rospy.get_param('~reset_mag', False) self.reset_accel = rospy.get_param('~reset_accel', False) self.mag_zero_x = rospy.get_param('~mag_zero_x', False) self.mag_zero_y = rospy.get_param('~mag_zero_y', False) self.mag_zero_z = rospy.get_param('~mag_zero_z', False) rospy.loginfo("serial port: %s" % (self.port)) self.link = rospy.get_param('~link', 'imu_link') rospy.loginfo("tf link: {}".format(self.link)) self.imu_data = Imu() self.imu_data = Imu(header=rospy.Header(frame_id=self.link)) # These covariance calculations are based on those bound in the existing # UM7 ROS package at: # https://github.com/ros-drivers/um7 linear_acceleration_stdev = rospy.get_param( "~linear_acceleration_stdev", 4.0 * 1e-3 * 9.80665) angular_velocity_stdev = rospy.get_param("~angular_velocity_stdev", np.deg2rad(0.06)) linear_acceleration_cov = linear_acceleration_stdev * linear_acceleration_stdev angular_velocity_cov = angular_velocity_stdev * angular_velocity_stdev # From the UM7 datasheet for the dynamic accuracy from the EKF. orientation_x_stdev = rospy.get_param("~orientation_x_stdev", np.deg2rad(3.0)) orientation_y_stdev = rospy.get_param("~orientation_y_stdev", np.deg2rad(3.0)) orientation_z_stdev = rospy.get_param("~orientation_z_stdev", np.deg2rad(5.0)) orientation_x_covar = orientation_x_stdev * orientation_x_stdev orientation_y_covar = orientation_y_stdev * orientation_y_stdev orientation_z_covar = orientation_z_stdev * orientation_z_stdev self.imu_data.orientation_covariance = [ orientation_x_covar, 0, 0, 0, orientation_y_covar, 0, 0, 0, orientation_z_covar ] self.imu_data.angular_velocity_covariance = [ angular_velocity_cov, 0, 0, 0, angular_velocity_cov, 0, 0, 0, angular_velocity_cov ] self.imu_data.linear_acceleration_covariance = [ linear_acceleration_cov, 0, 0, 0, linear_acceleration_cov, 0, 0, 0, linear_acceleration_cov ] # Set up the imu data message self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=1) # Set up the Roll-Pitch-Yaw (rpy) message self.rpy_data = Vector3Stamped() self.rpy_pub = rospy.Publisher('imu/rpy', Vector3Stamped, queue_size=1) # Set up the magnometer message self.mag_data = Vector3Stamped() self.mag_pub = rospy.Publisher('imu/mag', Vector3Stamped, queue_size=1) # what data to get from the UM7 self.statevars = [ 'health', 'roll', 'pitch', 'yaw', 'mag_proc_x', 'mag_proc_y', 'mag_proc_z', 'accel_proc_x', 'accel_proc_y', 'accel_proc_z', 'gyro_proc_x', 'gyro_proc_y', 'gyro_proc_z', 'quat_a', 'quat_b', 'quat_c', 'quat_d', 'gps_latitude', 'gps_longitude', 'gps_altitude' ] # Masks for parsing out the health data from the IMU self.NUM_SATS_USED = 0b11111100000000000000000000000000 self.HDOP = 0b00000011111111110000000000000000 # Not used as of 05/23/19 self.NUM_SATS_IN_VIEW = 0b00000000000000001111110000000000 # Set up the GPS NavSatFix publisher self.fix_data = NavSatFix() self.fix_pub = rospy.Publisher('fix', NavSatFix, queue_size=1) imu_connected = False while not rospy.is_shutdown() and not imu_connected: try: self.driver = um7.UM7('s', self.port, self.statevars, baud=115200) imu_connected = True rospy.loginfo("Imu initialization completed") self.received = -1 if self.reset_mag: self.driver.set_mag_reference() if self.reset_accel: cmd_seq.append(Um6Drv.CMD_SET_ACCEL_REF) if self.mag_zero_x and self.mag_zero_y and self.mag_zero_z: rospy.loginfo("Magnetometer calibration: %.3f %.3f %.3f", self.mag_zero_x, self.mag_zero_y, self.mag_zero_z) except SerialException: rospy.logwarn( "Serial error communicating with IMU. Will retry in 2.0 seconds." ) rospy.sleep(2.0)
def __init__(self): # Initializing ros node with name position_control rospy.init_node('position_controller', log_level=rospy.DEBUG) # Creating an instance of NavSatFix message and edrone_cmd message self.location = NavSatFix() self.drone_cmd = edrone_cmd() # Created a flag for changing the setpoints self.targets_achieved = 0 # Counter for csv file self.csv_counter = 0 # Counter Check if obstacle was deteceted self.obstacle_count = 0 # Gripper check vaiable self.gripper_data = False # A multiplication factor to increase number of waypoints proportional to distance between final and initial self.stride = 1 / 300.0 # Hardcoded initial target point """ Building 1: lat: 18.9990965928, long: 72.0000664814, alt: 10.75 Building 2: lat: 18.9990965925, long: 71.9999050292, alt: 22.2 Building 3: lat: 18.9993675932, long: 72.0000569892, alt: 10.7 """ # 0:takeoff,1:transverse,2:landing,3:takeoff W/P,4:transverse W/P,5:landing W/P self.states = 0 # List of target buildings self.buiding_locations = [] # Initial Longitude , latitude and altitude self.targets = [ [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], ] # Variable to store scanned waypoints self.scanned_target = [0.0, 0.0, 0.0] # Marker ids self.marker_id = 0 # To store bottom range finder values self.range_finder_bottom = 0 # Variable for top range finder sensor data # [1] right, [2] back, [3] left, [0,4] front w.r.t eyantra logo self.range_finder_top_list = [0.0, 0.0, 0.0, 0.0, 0.0] # Weights for left right sensor values self.weights_lr = [1.3, -1.3] # Counter for number of landings made self.bottom_count = 0 # Offset altitude self.offset_alt = 3.00 # Safety distances self.safe_dist_lat = 21.555 / 105292.0089353767 self.safe_dist_long = 6 / 110692.0702932625 # To store the x and y co-ordinates in meters self.err_x_m = 0.0 self.err_y_m = 0.0 # Number of waypoints initialized to -1 self.n = -1 # List to store waypoints Points calculated self.points = [] # Check if height attained self.start_to_check_for_obstacles = False # Kp, Ki and Kd found out experimentally using PID tune self.Kp = [88318, 88318, 1082 * 0.06] self.Ki = [0.008 * 10, 0.008 * 10, 0.0 * 0.008] self.Kd = [6606000, 6606000, 4476 * 0.3] # Output, Error ,Cummulative Error and Previous Error of the PID equation in the form [Long, Lat, Alt] self.error = [0.0, 0.0, 0.0] self.ouput = [0.0, 0.0, 0.0] self.cummulative_error = [0.0, 0.0, 0.0] self.previous_error = [0.0, 0.0, 0.0] self.max_cummulative_error = [1e-3, 1e-3, 100] self.max_error = 0.00038256625782 self.assigned_error = 0.000118256625782 / 2 self.throttle = 0 self.base_pwm = 1500 # ---------------------------------------------------------------------------------------------------------- # Allowed errors in long.,and lat. self.allowed_lon_error = 0.0000047487 / 6 self.allowed_lat_error = 0.000004517 / 6 # Checking if we have to scan or Land self.scan = False # Variable representing if the box has been grabbed self.box_grabbed = 0 # Constraining the no of times location_error has been called self.count = 0 # Time in which PID algorithm runs self.pid_break_time = 0.060 # in seconds self.x = 0 # Publishing servo-control messaages and altitude,longitude,latitude and zero error on errors /drone_command, /alt_error, /long_error, /lat_error, and current marker id self.drone_pub = rospy.Publisher('/drone_command', edrone_cmd, queue_size=1) self.alt_error = rospy.Publisher('/alt_error', Float32, queue_size=1) self.long_error = rospy.Publisher('/long_error', Float32, queue_size=1) self.lat_error = rospy.Publisher('/lat_error', Float32, queue_size=1) self.zero_error = rospy.Publisher('/zero_error', Float32, queue_size=1) self.curr_m_id = rospy.Publisher('/edrone/curr_marker_id', Int32, queue_size=1) self.alt_diff = rospy.Publisher("/alt_diff", Float32, queue_size=1) # ----------------------------------------------------------------------------------------------------------- # Subscribers for gps co-ordinates, and pid_tune GUI, gripper,rangefinder, custom location message and x,y errors in meters rospy.Subscriber('/edrone/gps', NavSatFix, self.gps_callback) rospy.Subscriber('/pid_tuning_altitude', PidTune, self.altitude_set_pid) rospy.Subscriber('/pid_tuning_roll', PidTune, self.long_set_pid) rospy.Subscriber('/pid_tuning_pitch', PidTune, self.lat_set_pid) rospy.Subscriber('/edrone/location_custom', location_custom, self.scanQR) rospy.Subscriber('/edrone/range_finder_bottom', LaserScan, self.range_bottom) rospy.Subscriber('/edrone/range_finder_top', LaserScan, self.range_top) rospy.Subscriber('/edrone/gripper_check', String, self.gripper_callback) rospy.Subscriber('/edrone/err_x_m', Float32, self.handle_x_m_err) rospy.Subscriber('/edrone/err_y_m', Float32, self.handle_y_m_err)
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('Vel') position_data = data.get('Pos') rawgps_data = data.get('RAWGPS') status = data.get('Stat') # int # 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: x = imu_data['gyrX'] y = imu_data['gyrY'] z = imu_data['gyrZ'] v = numpy.array([x, y, z]) v = v.dot(self.R) imu_msg.angular_velocity.x = v[0] imu_msg.angular_velocity.y = v[1] imu_msg.angular_velocity.z = v[2] 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 = v[0] vel_msg.twist.angular.y = v[1] vel_msg.twist.angular.z = v[2] pub_vel = True except KeyError: pass try: x = imu_data['accX'] y = imu_data['accY'] z = imu_data['accZ'] v = numpy.array([x, y, z]) v = v.dot(self.R) imu_msg.linear_acceleration.x = v[0] imu_msg.linear_acceleration.y = v[1] imu_msg.linear_acceleration.z = v[2] imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0., 0.0004, 0., 0., 0., 0.0004) pub_imu = True except KeyError: pass try: x = imu_data['magX'] y = imu_data['magY'] z = imu_data['magZ'] v = numpy.array([x, y, z]) v = v.dot(self.R) mag_msg.vector.x = v[0] mag_msg.vector.y = v[1] mag_msg.vector.z = v[2] 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 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 add_sentence(self, nmea_string, frame_id, timestamp=None): """Public method to provide a new NMEA sentence to the driver. Args: nmea_string (str): NMEA sentence in string form. frame_id (str): TF frame ID of the GPS receiver. timestamp(rospy.Time, optional): Time the sentence was received. If timestamp is not specified, the current time is used. Returns: bool: True if the NMEA string is successfully processed, False if there is an error. """ if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False # else: # rospy.logwarn("Valid sentence was: %s" % repr(nmea_string)) parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id if not self.use_GNSS_time: current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED data = parsed_sentence['GGA'] if self.use_GNSS_time: if math.isnan(data['utc_time'][0]): rospy.logwarn("Time in the NMEA sentence is NOT valid") return False current_fix.header.stamp = rospy.Time(data['utc_time'][0], data['utc_time'][1]) fix_type = data['fix_type'] if not (fix_type in self.gps_qualities): fix_type = -1 gps_qual = self.gps_qualities[fix_type] default_epe = gps_qual[0] current_fix.status.status = gps_qual[1] current_fix.position_covariance_type = gps_qual[2] if gps_qual > 0: self.valid_fix = True else: self.valid_fix = False current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude # use default epe std_dev unless we've received a GST sentence with # epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = default_epe * 2 hdop = data['hdop'] current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2 current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2 current_fix.position_covariance[8] = (2 * hdop * self.alt_std_dev)**2 # FIXME self.fix_pub.publish(current_fix) if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time): current_time_ref.time_ref = rospy.Time(data['utc_time'][0], data['utc_time'][1]) self.last_valid_fix_time = current_time_ref self.time_ref_pub.publish(current_time_ref) elif not self.use_RMC and 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] # Only report VTG data when you've received a valid GGA fix as # well. if self.valid_fix: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * math.sin( data['true_course']) current_vel.twist.linear.y = data['speed'] * math.cos( data['true_course']) self.vel_pub.publish(current_vel) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] if self.use_GNSS_time: if math.isnan(data['utc_time'][0]): rospy.logwarn("Time in the NMEA sentence is NOT valid") return False current_fix.header.stamp = rospy.Time(data['utc_time'][0], data['utc_time'][1]) # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time): current_time_ref.time_ref = rospy.Time( data['utc_time'][0], data['utc_time'][1]) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide # it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) elif 'GST' in parsed_sentence: data = parsed_sentence['GST'] # Use receiver-provided error estimate if available self.using_receiver_epe = True self.lon_std_dev = data['lon_std_dev'] self.lat_std_dev = data['lat_std_dev'] self.alt_std_dev = data['alt_std_dev'] elif 'HDT' in parsed_sentence: data = parsed_sentence['HDT'] if data['heading']: current_heading = QuaternionStamped() current_heading.header.stamp = current_time current_heading.header.frame_id = frame_id q = quaternion_from_euler(0, 0, math.radians(data['heading'])) current_heading.quaternion.x = q[0] current_heading.quaternion.y = q[1] current_heading.quaternion.z = q[2] current_heading.quaternion.w = q[3] self.heading_pub.publish(current_heading) elif 'SHR' in parsed_sentence: data = parsed_sentence['SHR'] if data['heading']: current_attitude = PoseWithCovarianceStamped() if self.use_GNSS_time: if math.isnan(data['utc_time'][0]): rospy.logwarn("Time in the NMEA sentence is NOT valid") return False current_attitude.header.stamp = rospy.Time( data['utc_time'][0], data['utc_time'][1]) else: current_attitude.header.stamp = current_time current_attitude.header.frame_id = frame_id current_attitude.pose.pose.position.x = math.radians( data['roll']) current_attitude.pose.pose.position.y = math.radians( data['pitch']) current_attitude.pose.pose.position.z = math.radians( data['heading']) q = quaternion_from_euler(math.radians(data['roll']), math.radians(data['pitch']), math.radians(data['heading'])) current_attitude.pose.pose.orientation.x = q[0] current_attitude.pose.pose.orientation.y = q[1] current_attitude.pose.pose.orientation.z = q[2] current_attitude.pose.pose.orientation.w = q[3] current_attitude.pose.covariance[0] = data['gps_quality'] current_attitude.pose.covariance[1] = data['imu_state'] current_attitude.pose.covariance[21] = math.radians( data['roll_accuracy']) current_attitude.pose.covariance[28] = math.radians( data['pitch_accuracy']) current_attitude.pose.covariance[35] = math.radians( data['heading_accuracy']) self.attitude_pub.publish(current_attitude) else: return False
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED data = parsed_sentence['GGA'] fix_type = data['fix_type'] if not (fix_type in self.gps_qualities): fix_type = -1 gps_qual = self.gps_qualities[fix_type] default_epe = gps_qual[0]; current_fix.status.status = gps_qual[1] current_fix.position_covariance_type = gps_qual[2]; if gps_qual > 0: self.valid_fix = True else: self.valid_fix = False current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude # use default epe std_dev unless we've received a GST sentence with epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = default_epe * 2 hdop = data['hdop'] current_fix.position_covariance[0] = (hdop * self.lon_std_dev) ** 2 current_fix.position_covariance[4] = (hdop * self.lat_std_dev) ** 2 current_fix.position_covariance[8] = (2 * hdop * self.alt_std_dev) ** 2 # FIXME self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) self.last_valid_fix_time = current_time_ref self.time_ref_pub.publish(current_time_ref) elif not self.use_RMC and 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] # Only report VTG data when you've received a valid GGA fix as well. if self.valid_fix: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) elif self.use_RMC and 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) self.time_ref_pub.publish(current_time_ref) if data['fix_valid']: # Publish velocity current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) # Publish true course current_heading = QuaternionStamped() current_heading.header.stamp = current_time current_heading.header.frame_id = frame_id q = quaternion_from_euler(0, 0, data['true_course']) current_heading.quaternion.x = q[0] current_heading.quaternion.y = q[1] current_heading.quaternion.z = q[2] current_heading.quaternion.w = q[3] self.heading_pub.publish(current_heading) elif 'GST' in parsed_sentence: data = parsed_sentence['GST'] # Use receiver-provided error estimate if available self.using_receiver_epe = True self.lon_std_dev = data['lon_std_dev'] self.lat_std_dev = data['lat_std_dev'] self.alt_std_dev = data['alt_std_dev'] elif not self.use_RMC and 'HDT' in parsed_sentence: data = parsed_sentence['HDT'] if data['heading']: current_heading = QuaternionStamped() current_heading.header.stamp = current_time current_heading.header.frame_id = frame_id q = quaternion_from_euler(0, 0, math.radians(data['heading'])) current_heading.quaternion.x = q[0] current_heading.quaternion.y = q[1] current_heading.quaternion.z = q[2] current_heading.quaternion.w = q[3] self.heading_pub.publish(current_heading) else: return False
def DataReader(): data = ReadFile(FILE_NAME) # IMU imu_msg = Imu() imu_msg.orientation.x = data[0][1:-1,0] imu_msg.orientation.y = data[0][1:-1,1] imu_msg.orientation.z = data[0][1:-1,2] imu_msg.orientation.w = data[0][1:-1,3] imu_msg.linear_acceleration.x=data[0][1:-1,4] imu_msg.linear_acceleration.y=data[0][1:-1,5] imu_msg.linear_acceleration.z=data[0][1:-1,6] imu_msg.angular_velocity.x=data[0][1:-1,7] imu_msg.angular_velocity.y=data[0][1:-1,8] imu_msg.angular_velocity.z=data[0][1:-1,9] imu_time = data[0][1:-1,10] imu_length = len(imu_msg.orientation.x) imu_roll_angle=np.zeros(imu_length,dtype=float) imu_pitch_angle=np.zeros(imu_length,dtype=float) imu_yaw_angle=np.zeros(imu_length,dtype=float) for i in range(imu_length): x = imu_msg.orientation.x[i] y = imu_msg.orientation.y[i] z = imu_msg.orientation.z[i] w = imu_msg.orientation.w[i] imu_roll_angle[i] = math.atan2(2 * (y*z + w*x), w*w - x*x - y*y + z*z)*180/PI imu_pitch_angle[i] = math.asin(-2 * (x*z - w*y))*180/PI imu_yaw_angle[i] = math.atan2(2 * (x*y + w*z), w*w + x*x - y*y - z*z)*180/PI # vehicle_state vehicle_msg =Odometry() vehicle_msg.pose.pose.position.x =data[1][1:-1,0] vehicle_msg.pose.pose.position.y =data[1][1:-1,1] vehicle_msg.pose.pose.position.z =data[1][1:-1,2] vehicle_msg.pose.pose.orientation.x = data[1][1:-1,3] vehicle_msg.pose.pose.orientation.y = data[1][1:-1,4] vehicle_msg.pose.pose.orientation.z = data[1][1:-1,5] vehicle_msg.pose.pose.orientation.w = data[1][1:-1,6] vehicle_msg.twist.twist.linear.x= data[1][1:-1,7] vehicle_msg.twist.twist.linear.y= data[1][1:-1,8] vehicle_msg.twist.twist.linear.z= data[1][1:-1,9] vehicle_msg.twist.twist.angular.x= data[1][1:-1,10] vehicle_msg.twist.twist.angular.y= data[1][1:-1,11] vehicle_msg.twist.twist.angular.z= data[1][1:-1,12] vehicle_time = data[1][1:-1,13] odom_length = len(vehicle_msg.pose.pose.orientation.x) odom_roll_angle=np.zeros(odom_length,dtype=float) odom_pitch_angle=np.zeros(odom_length,dtype=float) odom_yaw_angle=np.zeros(odom_length,dtype=float) for i in range(odom_length): x = vehicle_msg.pose.pose.orientation.x[i] y = vehicle_msg.pose.pose.orientation.y[i] z = vehicle_msg.pose.pose.orientation.z[i] w = vehicle_msg.pose.pose.orientation.w[i] odom_roll_angle[i] = math.atan2(2 * (y*z + w*x), w*w - x*x - y*y + z*z)*180/PI odom_pitch_angle[i] = math.asin(-2 * (x*z - w*y))*180/PI odom_yaw_angle[i] = math.atan2(2 * (x*y + w*z), w*w + x*x - y*y - z*z)*180/PI # GPS pos_gps = NavSatFix() pos_gps.latitude = data[2][1:-1,1] pos_gps.longitude = data[2][1:-1,2] pos_gps.altitude = data[2][1:-1,3] gps_time = data[2][1:-1,4] # chassis chassis_msg_Steer = data[3][1:-1,0] chassis_msg_Throttle = data[3][1:-1,1] chassis_msg_Brake = data[3][1:-1,2] chassis_msg_Wheel = data[3][1:-1,3] chassis_msg_Speed = data[3][1:-1,4] chassis_msg_BucketAngle = data[3][1:-1,5] chassis_msg_EngineSpeed = data[3][1:-1,6] chassis_msg_EngineTorque = data[3][1:-1,7] chassis_time = data[3][1:-1,8] plt.figure(1) loc='center' font_dict={'fontsize': 20,\ 'fontweight' : 8.2,\ 'verticalalignment': 'baseline',\ 'horizontalalignment': loc} plt.title('trajectory',fontdict=font_dict,loc=loc) plt.plot(pos_gps.latitude,pos_gps.longitude, color='cyan', label='desired_trajectory') plt.xlabel('latitude') plt.ylabel('longitude') plt.figure(2) loc='center' font_dict={'fontsize': 20,\ 'fontweight' : 8.2,\ 'verticalalignment': 'baseline',\ 'horizontalalignment': loc} plt.title('Pitch Angle Comparison',fontdict=font_dict,loc=loc) plt.plot(vehicle_time,odom_pitch_angle, color='cyan', label='pitch angle from odom') plt.plot(imu_time,-imu_pitch_angle, color='r', label='pitch angle from imu') plt.legend() plt.ylabel('pitch angle/degree') plt.xlabel('time/s') plt.figure(3) loc='center' font_dict={'fontsize': 20,\ 'fontweight' : 8.2,\ 'verticalalignment': 'baseline',\ 'horizontalalignment': loc} plt.title('trajectory',fontdict=font_dict,loc=loc) plt.plot(vehicle_msg.pose.pose.position.x ,vehicle_msg.pose.pose.position.y, color='cyan', label='desired_trajectory') plt.xlabel('X') plt.ylabel('Y') plt.show()
import rospy import math import geopy from geometry_msgs.msg import PoseStamped, Pose2D from geopy.distance import VincentyDistance, vincenty from sensor_msgs.msg import NavSatFix # global local_target local_target = Pose2D() position_global = NavSatFix() pose = PoseStamped() def global_to_local(data): # TODO: refactor global local_target # local_target = Pose2D() lat = data.x lon = data.y print data d = math.hypot(pose.pose.position.x, pose.pose.position.y) bearing = math.degrees( math.atan2(-pose.pose.position.x, -pose.pose.position.y)) if bearing < 0: bearing += 360 cur = geopy.Point(position_global.latitude, position_global.longitude) origin = VincentyDistance(meters=d).destination(cur, bearing)
from sensor_msgs.msg import NavSatFix, BatteryState from inspector_software_uav.srv import * from uav_abstraction_layer.srv import * from uav_abstraction_layer.msg import * import PAL_clients as pal import geo # Global variables stop_flag = False auto_download = False loaded_mission = False flight_status = UInt8() current_pos = PointStamped() current_gps_pos = NavSatFix() battery_state = BatteryState() current_vel = Vector3Stamped() ual_state = UInt8() last_ual_state = UInt8() adl_state = String() ## Json files with mission information mission_status_file = os.path.expanduser("~") + '/catkin_ws/src/inspector_software_uav/json_files/mission_status.json' mission_waypoints_file = os.path.expanduser("~") + '/catkin_ws/src/inspector_software_uav/json_files/mission_wps.json' mission_data_file = os.path.expanduser("~") + '/catkin_ws/src/inspector_software_uav/json_files/mission_data.json' print mission_waypoints_file ## PAL srv Clients ##
def setUp(self): self.altitude = Altitude() self.extended_state = ExtendedState() self.global_position = NavSatFix() self.imu_data = Imu() self.home_position = HomePosition() self.local_position = PoseStamped() self.mission_wp = WaypointList() self.state = State() self.mav_type = None self.sub_topics_ready = { key: False for key in [ 'alt', 'ext_state', 'global_pos', 'home_pos', 'local_pos', 'mission_wp', 'state', 'imu' ] } # ROS services service_timeout = 30 rospy.loginfo("waiting for ROS services") try: rospy.wait_for_service('mavros/param/get', service_timeout) rospy.wait_for_service('mavros/cmd/arming', service_timeout) rospy.wait_for_service('mavros/mission/push', service_timeout) rospy.wait_for_service('mavros/mission/clear', service_timeout) rospy.wait_for_service('mavros/set_mode', service_timeout) rospy.loginfo("ROS services are up") except rospy.ROSException: self.fail("failed to connect to services") self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet) self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode) self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear', WaypointClear) self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push', WaypointPush) # ROS subscribers self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude, self.altitude_callback) self.ext_state_sub = rospy.Subscriber('mavros/extended_state', ExtendedState, self.extended_state_callback) self.global_pos_sub = rospy.Subscriber('mavros/global_position/global', NavSatFix, self.global_position_callback) self.imu_data_sub = rospy.Subscriber('mavros/imu/data', Imu, self.imu_data_callback) self.home_pos_sub = rospy.Subscriber('mavros/home_position/home', HomePosition, self.home_position_callback) self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped, self.local_position_callback) self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints', WaypointList, self.mission_wp_callback) self.state_sub = rospy.Subscriber('mavros/state', State, self.state_callback)
def waterlinked(base_url): rospy.init_node('waterlinked', anonymous=True) rate = rospy.Rate(40) # 40hz raw_pub = rospy.Publisher('/waterlinked/acoustic_position/raw', PointStamped, queue_size=120) filtered_pub = \ rospy.Publisher('/waterlinked/acoustic_position/filtered', PointStamped, queue_size=120) global_pub = rospy.Publisher('/waterlinked/global_position', NavSatFix, queue_size=120) def publish_raw(data_raw): raw_point = PointStamped() raw_point.header.frame_id = "/map" raw_point.header.stamp = rospy.Time.now() raw_point.point.x = data_raw['x'] raw_point.point.y = data_raw['y'] raw_point.point.z = data_raw['z'] raw_pub.publish(raw_point) def publish_filtered(data_filtered): filtered_point = PointStamped() filtered_point.header.frame_id = "/map" filtered_point.header.stamp = rospy.Time.now() filtered_point.point.x = data_filtered['x'] filtered_point.point.y = data_filtered['y'] filtered_point.point.z = data_filtered['z'] filtered_pub.publish(filtered_point) last_raw, last_filtered = None, None while not rospy.is_shutdown(): data_raw = get_acoustic_position_raw(base_url) if last_raw is None: publish_raw(data_raw) last_raw = data_raw else: dist = (data_raw['x'] - last_raw['x']) ** 2 +\ (data_raw['y'] - last_raw['y']) ** 2 +\ (data_raw['z'] - last_raw['z']) ** 2 if dist > 1e-10: publish_raw(data_raw) last_raw = data_raw data_filtered = get_acoustic_position_filtered(base_url) if last_filtered is None: publish_filtered(data_filtered) last_filtered = data_filtered else: dist = (data_filtered['x'] - last_filtered['x']) ** 2 +\ (data_filtered['y'] - last_filtered['y']) ** 2 +\ (data_filtered['z'] - last_filtered['z']) ** 2 if dist > 1e-10: publish_filtered(data_filtered) last_filtered = data_filtered data_global = get_global_position(base_url) global_point = NavSatFix() global_point.header.stamp = rospy.Time.now() global_point.header.frame_id = "/map" global_point.latitude = data_global['lat'] global_point.longitude = data_global['lon'] global_pub.publish(global_point) rate.sleep()
#!/usr/bin/env python import rospy from std_msgs.msg import Int8 from sensor_msgs.msg import NavSatFix from sklearn.cluster import MeanShift #from threading import Thread lat_offset = 26.19 lon_offset = 91.69 gps_coordinates = [] box_coordinates = [] box_gps_msg = NavSatFix() obj_found_1 = 56 def obj_number_cb1(data_temp): print("obj data updated") global obj_found_1 obj_found_1 = int(data_temp.data) print(obj_found_1) def gps_cb1(data): global obj_found_1, lat_offset, lon_offset print("gps called") print("^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ", obj_found_1) if (obj_found_1 == 1): print("*****************object found ******************") gps_coordinates.append((int((data.latitude - lat_offset) * 10000000), int((data.longitude - lon_offset) * 1000000)))
def control(): pass def commuciation_check(): pass def takeoff(height): pass if __name__ == '__main__': self_position = NavSatFix() neighbour_position = gpsPosition() rospy.init_node('px4ros_leader_control', anonymous=True) pub = rospy.Publisher('serial_data_send', String, queue_size=20) rate = rospy.Rate(10) # 10hz #subscribe info from mavros rospy.Subscriber("/mavros/global_position/raw/fix", NavSatFix, callback1) #subscribe info from leader_control_decode rospy.Subscriber("neighbour_position", gpsPosition, callback2) while not rospy.is_shutdown(): #pub.publish(globalPosition) #rospy.loginfo("%s",globalPosition)
def talker(): global global_g, deg_rad, Heading_HPD ser.flush() pub_m = rospy.Publisher('/imu/data_raw', Imu, queue_size=100) pub = rospy.Publisher('comb', comb, queue_size=100) pub_n = rospy.Publisher('comb_now', comb, queue_size=100) pub_g = rospy.Publisher('gps', comb, queue_size=100) pub_o = rospy.Publisher('/gps/fix', NavSatFix, queue_size=100) rospy.init_node('comb_node', anonymous=True) rate = rospy.Rate(100) # 10hz imuMsg = Imu() combMsg = comb() gpsMsg = comb() gpsmeanMsg = NavSatFix() seq = 0 while not rospy.is_shutdown(): # $GTIMU,GPSWeek,GPSTime,GyroX,GyroY,GyroZ,AccX,AccY,AccZ,Tpr*cs<CR><LF> if ser.read() != '$': continue line0 = ser.readline() print line0 cs = line0[-4:-2] print cs for s_tmp in line0[6:-8]: if s_tmp.isalpha(): continue try: cs1 = int(cs, 16) except: continue cs2 = checksum(line0) print("cs1,cs2", cs1, cs2) if cs1 != cs2: continue if line0[0:5] == "GTIMU": line = line0.replace("GTIMU,", "") line = line.replace("\r\n", "") if "\x00" in line: continue if not string.find('*', line): continue line = line.replace("*", ",") words = string.split(line, ",") if len(words) != 10: continue GyroX = string.atof(words[2]) GyroY = string.atof(words[3]) GyroZ = string.atof(words[4]) AccX = string.atof(words[5]) AccY = string.atof(words[6]) AccZ = string.atof(words[7]) imuMsg.linear_acceleration.x = AccX * global_g imuMsg.linear_acceleration.y = AccY * global_g imuMsg.linear_acceleration.z = AccZ * global_g imuMsg.linear_acceleration_covariance[0] = 0.0001 imuMsg.linear_acceleration_covariance[4] = 0.0001 imuMsg.linear_acceleration_covariance[8] = 0.0001 imuMsg.angular_velocity.x = GyroX * deg_rad imuMsg.angular_velocity.y = GyroY * deg_rad imuMsg.angular_velocity.z = GyroZ * deg_rad imuMsg.angular_velocity_covariance[0] = 0.0001 imuMsg.angular_velocity_covariance[4] = 0.0001 imuMsg.angular_velocity_covariance[8] = 0.0001 q = quaternion_from_euler(0, 0, 0) imuMsg.orientation.x = q[0] imuMsg.orientation.y = q[1] imuMsg.orientation.z = q[2] imuMsg.orientation.w = q[3] imuMsg.header.stamp = rospy.Time.now() imuMsg.header.frame_id = 'imu' imuMsg.header.seq = seq seq = seq + 1 pub_m.publish(imuMsg) rate.sleep() elif line0[0:5] == "GPFPD": line = line0.replace("GPFPD,", "") line = line.replace("\r\n", "") if ("\x00" in line): continue if (not string.find('*', line)): continue line = line.replace("*", ",") words = string.split(line, ",") if len(words) != 16: continue GPSWeek = string.atoi(words[0]) GPSTime = string.atof(words[1]) Heading = string.atof(words[2]) Pitch = string.atof(words[3]) Roll = string.atof(words[4]) Latitude = string.atof(words[5]) Longitude = string.atof(words[6]) Altitude = string.atof(words[7]) Ve = string.atof(words[8]) Vn = string.atof(words[9]) Vu = string.atof(words[10]) Baseline = string.atof(words[11]) NSV1 = string.atoi(words[12]) NSV2 = string.atoi(words[13]) Status = words[14] if Status == '03': Vne = math.sqrt(Vn * Vn + Ve * Ve) if (Vne > 0.3 and Ve < 0): Heading = math.acos(Vn / Vne) else: Heading = 2 * 3.141592658 - math.acos(Vn / Vne) combMsg.GPSWeek = GPSWeek combMsg.GPSTime = GPSTime combMsg.Heading = Heading combMsg.Pitch = Pitch combMsg.Roll = Roll combMsg.Latitude = Latitude combMsg.Longitude = Longitude combMsg.Altitude = Altitude combMsg.Ve = Ve combMsg.Vn = Vn combMsg.Vu = Vu combMsg.Baseline = Baseline combMsg.NSV1 = NSV1 combMsg.NSV2 = NSV2 combMsg.Status = Status combMsg.header.stamp = rospy.Time.now() combMsg.header.frame_id = 'gps' combMsg.header.seq = seq pub_n.publish(combMsg) print("pub comb_now") if Status == '03' or Status == '04' or Status == '05' or Status == '08' or Status == '0B': pub.publish(combMsg) print("pub comb") seq += 1 rate.sleep() # $GPHPD, GPSWeek, GPSTime, Heading, Pitch, Track, Latitude, Longitude, Altitude, Ve , Vn, Vu,Baseline, NSV1, NSV2*cs<CR><LF> elif line0[0:5] == "GPHPD": line = line0.replace("GPHPD,", "") line = line.replace("\r\n", "") if ("\x00" in line): continue if (not string.find('*', line)): continue line = line.replace("*", ",") words = string.split(line, ",") if len(words) != 16: continue Status = words[14] GPSWeek = string.atoi(words[0]) GPSTime = string.atof(words[1]) Heading_HPD = string.atof(words[2]) Heading_HPD = 360 - Heading_HPD Pitch = string.atof(words[3]) Track = string.atof(words[4]) Latitude = string.atof(words[5]) Longitude = string.atof(words[6]) Altitude = string.atof(words[7]) Ve = string.atof(words[8]) Vn = string.atof(words[9]) Vu = string.atof(words[10]) Baseline = string.atof(words[11]) NSV1 = string.atoi(words[12]) NSV2 = string.atoi(words[13]) gpsMsg.GPSWeek = GPSWeek gpsMsg.GPSTime = GPSTime gpsMsg.Heading = Heading_HPD gpsMsg.Pitch = Pitch gpsMsg.Roll = Track gpsMsg.Latitude = Latitude gpsMsg.Longitude = Longitude gpsMsg.Altitude = Altitude gpsMsg.Ve = Ve gpsMsg.Vn = Vn gpsMsg.Vu = Vu gpsMsg.Baseline = Baseline gpsMsg.NSV1 = NSV1 gpsMsg.NSV2 = NSV2 gpsMsg.Status = Status gpsMsg.header.stamp = rospy.Time.now() gpsMsg.header.frame_id = 'gps' gpsMsg.header.seq = seq pub_g.publish(gpsMsg) print("pub gps") gpsmeanMsg.header.stamp = rospy.Time.now() gpsmeanMsg.header.frame_id = 'gps' gpsmeanMsg.header.seq = seq if Status == '05': gpsmeanMsg.status.status = 2 covariance_xyz = 0.05**2 elif Status == '03': gpsmeanMsg.status.status = 0 covariance_xyz = 5**2 else: gpsmeanMsg.status.status = -1 covariance_xyz = 99999 gpsmeanMsg.status.service = 1 gpsmeanMsg.latitude = Latitude gpsmeanMsg.longitude = Longitude gpsmeanMsg.altitude = Latitude gpsmeanMsg.position_covariance = [ covariance_xyz, 0, 0, 0, covariance_xyz, 0, 0, 0, covariance_xyz ] gpsmeanMsg.position_covariance_type = 3 pub_o.publish(gpsmeanMsg) print("pub gps_mean") seq += 1 rate.sleep() else: continue
def bestpos_handler(self, bestpos): navsat = NavSatFix() # TODO: The timestamp here should come from SPAN, not the ROS system time. navsat.header.stamp = rospy.Time.now() navsat.header.frame_id = self.odom_frame # Assume GPS - this isn't exposed navsat.status.service = NavSatStatus.SERVICE_GPS position_type_to_status = { BESTPOS.POSITION_TYPE_NONE: NavSatStatus.STATUS_NO_FIX, BESTPOS.POSITION_TYPE_FIXED: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_FIXEDHEIGHT: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_FLOATCONV: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_WIDELANE: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_NARROWLANE: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_DOPPLER_VELOCITY: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_SINGLE: NavSatStatus.STATUS_FIX, BESTPOS.POSITION_TYPE_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_WAAS: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_PROPAGATED: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_OMNISTAR: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_L1_FLOAT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_IONOFREE_FLOAT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_NARROW_FLOAT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_L1_INT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_WIDE_INT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_NARROW_INT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_RTK_DIRECT_INS: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_SBAS: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_INS_PSRSP: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_RTKFLOAT: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_RTKFIXED: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_OMNISTAR: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_OMNISTAR_HP: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_INS_OMNISTAR_XP: NavSatStatus.STATUS_GBAS_FIX, BESTPOS.POSITION_TYPE_OMNISTAR_HP: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_OMNISTAR_XP: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_PPP: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_INS_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX, BESTPOS.POSITION_TYPE_INS_PPP: NavSatStatus.STATUS_SBAS_FIX, } navsat.status.status = position_type_to_status.get( bestpos.position_type, NavSatStatus.STATUS_NO_FIX) # Position in degrees. navsat.latitude = bestpos.latitude navsat.longitude = bestpos.longitude # Altitude in metres. navsat.altitude = bestpos.altitude navsat.position_covariance[0] = pow(bestpos.latitude_std, 2) navsat.position_covariance[4] = pow(bestpos.longitude_std, 2) navsat.position_covariance[8] = pow(bestpos.altitude_std, 2) navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN # Ship ito self.pub_navsatfix.publish(navsat)
def setUp(self): print('@ctrl_server@ setUp') self.pos = PoseStamped() self.altitude = Altitude() self.extended_state = ExtendedState() self.global_position = NavSatFix() self.home_position = HomePosition() self.local_position = PoseStamped() self.misson_wp = WaypointList() self.state = State() self.scan = LaserScan() self.mav_type = None self.ready = False # Target offset radius self.radius = 0.25 self.sub_topics_ready = { key: False for key in [ 'alt', 'ext_state', 'global_pos', 'home_pos', 'local_pos', 'mission_wp', 'state', 'scan' ] } # ROS services service_timeout = 60 rospy.loginfo("waiting for ROS services") try: rospy.wait_for_service('mavros/param/get', service_timeout) rospy.wait_for_service('mavros/cmd/arming', service_timeout) rospy.wait_for_service('mavros/mission/push', service_timeout) rospy.wait_for_service('mavros/mission/clear', service_timeout) rospy.wait_for_service('mavros/set_mode', service_timeout) rospy.loginfo("ROS services are up") except rospy.ROSException: print("@ctrl_server@ failed to connect to services") self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet) self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode) self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear', WaypointClear) self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push', WaypointPush) # ROS subscribers self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude, self.altitude_callback) self.ext_state_sub = rospy.Subscriber('mavros/extended_state', ExtendedState, self.extended_state_callback) self.global_pos_sub = rospy.Subscriber('mavros/global_position/global', NavSatFix, self.global_position_callback) self.home_pos_sub = rospy.Subscriber('mavros/home_position/home', HomePosition, self.home_position_callback) self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped, self.local_position_callback) self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints', WaypointList, self.mission_wp_callback) self.state_sub = rospy.Subscriber('mavros/state', State, self.state_callback) self.get_scan_srv = rospy.Subscriber('/laser/scan', LaserScan, self.scan_callback) # ROS publisers self.pos_setpoint_pub = rospy.Publisher( 'mavros/setpoint_position/local', PoseStamped, queue_size=1) # send setpoints in seperate thread self.pos_thread = Thread(target=self.send_pos, args=()) self.pos_thread.daemon = True self.pos_thread.start() print('@ctrl_server@ setUp over') pass
def add_sentence(self, nmea_string, frame_id, timestamp=None): """Public method to provide a new NMEA sentence to the driver. Args: nmea_string (str): NMEA sentence in string form. frame_id (str): TF frame ID of the GPS receiver. timestamp(rospy.Time, optional): Time the sentence was received. If timestamp is not specified, the current time is used. Returns: bool: True if the NMEA string is successfully processed, False if there is an error. """ if not check_nmea_checksum(nmea_string): #rospy.logwarn("Received a sentence with an invalid checksum. " + # "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: #rospy.logdebug( # "Failed to parse NMEA sentence. Sentence was: %s" % # nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id if not self.use_GNSS_time: current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: data = parsed_sentence['GGA'] if self.use_GNSS_time: if math.isnan(data['utc_time'][0]): rospy.logwarn("Time in the NMEA sentence is NOT valid") return False current_fix.header.stamp = rospy.Time(data['utc_time'][0], data['utc_time'][1]) gps_qual = data['fix_type'] if gps_qual == 0: current_fix.status.status = NavSatStatus.STATUS_NO_FIX elif gps_qual == 1: current_fix.status.status = NavSatStatus.STATUS_FIX elif gps_qual == 2: current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX elif gps_qual in (4, 5): current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX elif gps_qual == 9: # Support specifically for NOVATEL OEM4 recievers which report WAAS fix as 9 # http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/ current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX if gps_qual > 0: self.valid_fix = True else: self.valid_fix = False current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude hdop = data['hdop'] current_fix.position_covariance[0] = hdop ** 2 current_fix.position_covariance[4] = hdop ** 2 current_fix.position_covariance[8] = (2 * hdop) ** 2 # FIXME current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude self.fix_pub.publish(current_fix) if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time): current_time_ref.time_ref = rospy.Time( data['utc_time'][0], data['utc_time'][1]) self.last_valid_fix_time = current_time_ref self.time_ref_pub.publish(current_time_ref) elif not self.use_RMC and 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] # Only report VTG data when you've received a valid GGA fix as # well. if self.valid_fix: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * math.cos(data['true_course']) self.vel_pub.publish(current_vel) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] if self.use_GNSS_time: if math.isnan(data['utc_time'][0]): #rospy.logwarn("Time in the NMEA sentence is NOT valid") return False current_fix.header.stamp = rospy.Time(data['utc_time'][0], data['utc_time'][1]) # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time): current_time_ref.time_ref = rospy.Time( data['utc_time'][0], data['utc_time'][1]) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide # it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) else: return False
def __init__(self, namespace='sbg_raw_data_simulator'): """ Initialize the publishers and subscribes to necessary channels `self.gps` mimics the gps data of the sbg `self.imu` mimics the imu data of the sbg Raw imu simulator parameters in: eufs_description/sensors/imu.urdf.xacro Raw gps simulator parameters in: eufs_description/sensors/gps.urdf.xacro Parameters: _imu_hz: the hz at which to broadcast the imu data, note that this has implications for accuracy as the noise models are affected by sample rate! _gps_hz: the hz at which to broadcast the gps data, has no accuracy implications Highly recommended to read the wiki https://www.wiki.ed.ac.uk/display/EUF/SBG+Sensor+Simulation to understand why the noise is produced the way it is. """ rospy.init_node("sbg_raw_data_simulator", anonymous=True) # Create persistent messages self.imu_msg = Imu() self.gps_msg = NavSatFix() # Create bias error accumulator, only relevant for the imu self.imu_angular_velocity_bias = [0, 0, 0] # Don't publish anything if not received messages yet self.got_imu = False self.got_gps = False # Latitude & longitude to meters # Approximations about Edinburgh 55.950191, -3.187566 # Values represent distance you have to travel to equal 1 meter. self.latitude_approx = 0.000018 self.longitude_approx = 0.000016 # Publish at fixed rates self.IMU_PUBLISH_RATE_HZ = rospy.get_param("~imu_hz", default=200) self.GPS_PUBLISH_RATE_HZ = rospy.get_param("~gps_hz", default=5) self.gps = rospy.Publisher("/imu/nav_sat_fix", NavSatFix, queue_size=1) self.imu = rospy.Publisher("/imu/data", Imu, queue_size=1) rospy.Timer( rospy.Duration(1.0 / self.IMU_PUBLISH_RATE_HZ), lambda x: self.imu.publish(self.imu_msg) if self.got_imu and self.got_gps else None) rospy.Timer( rospy.Duration(1.0 / self.GPS_PUBLISH_RATE_HZ), lambda x: self.gps.publish(self.gps_msg) if self.got_imu and self.got_gps else None) # Set calculated standard distribution information # check https://www.wiki.ed.ac.uk/display/EUF/SBG+Sensor+Simulation # for method and reasons. # Note that we had trouble calculating bias constant so for now we're using # the white noise constant for bias as well. self.imu_angular_velocity_white_noise_std = math.sqrt( 0.00000625 * self.IMU_PUBLISH_RATE_HZ) self.imu_angular_velocity_bias_std = math.sqrt( 1.476 * math.pow(10, -13) / self.IMU_PUBLISH_RATE_HZ) self.imu_linear_acceleration_white_noise_std = math.sqrt( 0.000000312 * self.IMU_PUBLISH_RATE_HZ) self.gps_position_white_noise_std = math.sqrt(39.4) # Subscribe to channels self.imu_sub = rospy.Subscriber("/imu", Imu, self.imu_receiver) self.gps_sub = rospy.Subscriber("/gps", NavSatFix, self.gps_receiver)
class bt_missions: rospy.init_node('aero_pose_python') mavros.set_namespace() rate = rospy.Rate(20) orb_ = PoseStamped() dirct_ = Float64() points_ = PointCloud2() home_ = HomePosition() current_state = State() current_ = NavSatFix() alt_ = Float64() fourcc = cv2.VideoWriter_fourcc('X', 'V', 'I', 'D') out = cv2.VideoWriter('realsense.avi', fourcc, 20.0, (640, 480)) home_set_ = False isContinue = True orb_scale = False target = [[24.982550663278158, 121.57233949235275], [24.982091, 121.571846], [24.981758, 121.571963]] index = 0 current_posx = 0 dist_first = True start_heading = 0 p0 = 0 p1 = 0 brng = 0 dLon = 0 y = 0 x = 0 current_yaw = 0 current_alt = 0 dist = 0 pose_dist = 0 heading = 0 lat = 0 lon = 0 cnt = 0 alt_count = 0 alt_count2 = 0 ah_alt = 0 z1 = 0 z2 = 0 z3 = 0 z4 = 0 scale = 0 ah_flag = False img_count = 0 query_count = 0 pose_flag = True move_flag = False record_img = np.array([]) set_vel_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10) set_v_pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=10) local_pos_pub = rospy.Publisher(mavros.get_topic('setpoint_position', 'local'), PoseStamped, queue_size=10) def __init__(self): self.orb_sub = rospy.Subscriber("/orb_slam2_mono/pose", PoseStamped, self.orb_pose) self.hdg_sub = rospy.Subscriber("/mavros/global_position/compass_hdg", Float64, self.compass) self.rel_alt = rospy.Subscriber("/mavros/global_position/rel_alt", Float64, self.altitude) self.home_sub = rospy.Subscriber("/mavros/home_position/home", HomePosition, self.setHomeGeoPointCB) self.gps_sub = rospy.Subscriber("/mavros/global_position/global", NavSatFix, self.current_position_cb) self.state_sub = rospy.Subscriber(mavros.get_topic('state'), State, self.state_cb) self.img_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.call, buff_size=2**24, queue_size=1) self.point_sub = rospy.Subscriber("/orb_slam2_mono/map_points", PointCloud2, self.pointcloud_cb) #self.mono_sub = rospy.Subscriber("/camera/mono/image_raw", Image, self.mono_cb, buff_size = 2**24, queue_size = 1) self.tree = self.correction #(self.achieve | ((self.achieve_alt | self.up) >> (self.yaw_correction | self.correction) >> (self.get_scale | self.up_scale) >> self.forward)) >> (self.ah | (self.isdetect >> self.change_alt)) def orb_pose(self, pose): bt_missions.orb_ = pose def pointcloud_cb(self, points): bt_missions.points_ = points def compass(self, dirct): bt_missions.dirct_ = dirct bt_missions.heading = bt_missions.dirct_.data def state_cb(self, state): bt_missions.current_state = State() bt_missions.current_state = state def altitude(self, alt): bt_missions.alt_ = alt bt_missions.current_alt = bt_missions.alt_.data rospy.loginfo("Received current altitude : %s" % bt_missions.current_alt) def setHomeGeoPointCB(self, home): bt_missions.home_ = home bt_missions.home_set_ = True rospy.loginfo( "Received Home (WGS84 datum): %lf, %lf, %lf" % (bt_missions.home_.geo.latitude, bt_missions.home_.geo.longitude, bt_missions.home_.geo.altitude)) def current_position_cb(self, current): bt_missions.current_ = current bt_missions.lat = bt_missions.current_.latitude bt_missions.lon = bt_missions.current_.longitude rospy.loginfo("Received current GPS : %lf, %lf" % (bt_missions.lat, bt_missions.lon)) def call(self, callimage): bt_missions.record_img = CvBridge().imgmsg_to_cv2(callimage, "bgr8") bt_missions.out.write(bt_missions.record_img) @condition def ah(self): if bt_missions.ah_flag == True: return False else: return True @condition def isdetect(self): bt_missions.query_count = bt_missions.query_count + 1 img1 = cv2.imread('query/' + str(bt_missions.query_count) + '.PNG', cv2.IMREAD_GRAYSCALE) #queryImage bt_missions.img_count = bt_missions.img_count + 1 cv2.imwrite('image/' + str(bt_missions.img_count) + '.PNG', bt_missions.record_img) img2 = cv2.imread('image/' + str(bt_missions.img_count) + '.PNG', cv2.IMREAD_GRAYSCALE) #trainImage # Initiate SIFT detector sift = cv2.xfeatures2d.SIFT_create() # find the keypoints and descriptors with SIFT kp1, des1 = sift.detectAndCompute(img1, None) kp2, des2 = sift.detectAndCompute(img2, None) # BFMatcher with default params bf = cv2.BFMatcher() t = rospy.get_time() while (rospy.get_time() - t <= 2): matches = bf.knnMatch(des1, des2, k=2) # Apply ratio test good = [] for m, n in matches: if m.distance < 0.55 * n.distance: good.append([m]) print("threshold: %s" % len(good)) if len(good) >= 20: bt_missions.index = bt_missions.index + 1 bt_missions.img_count = 0 return True else: return False @condition def lose_tracking(self): if bt_missions.orb_.pose.position.z == 0: return False else: return True @condition def get_scale(self): if bt_missions.orb_scale == True: return True else: return False @condition def achieve_alt(self): if bt_missions.current_alt > 15: print("altitude enough") return True else: print("altitude not enough") return False @condition def achieve(self): if bt_missions.index == 0 and bt_missions.dist_first == True: bt_missions.dist_first = False #GPS distance bt_missions.p0 = (bt_missions.home_.geo.latitude, bt_missions.home_.geo.longitude) bt_missions.p1 = (bt_missions.target[bt_missions.index][0], bt_missions.target[bt_missions.index][1]) bt_missions.dist = vincenty(bt_missions.p0, bt_missions.p1).meters print("current Gps dist: %s" % bt_missions.dist) #Pose distance bt_missions.pose_dist = bt_missions.scale * bt_missions.dist print("current pose dist: %s" % bt_missions.pose_dist) if bt_missions.pose_dist >= 0.02: return False elif bt_missions.pose_dist == 0: return False else: bt_missions.index = bt_missions.index + 1 elif bt_missions.index == 0 and bt_missions.dist_first == False: bt_missions.p0 = (bt_missions.current_.latitude, bt_missions.current_.longitude) bt_missions.p1 = (bt_missions.target[bt_missions.index][0], bt_missions.target[bt_missions.index][1]) bt_missions.dist = vincenty(bt_missions.p0, bt_missions.p1).meters print("current Gps dist: %s" % bt_missions.dist) #Pose distance bt_missions.pose_dist = bt_missions.scale * bt_missions.dist print("current pose dist: %s" % bt_missions.pose_dist) if bt_missions.pose_dist >= 0.02: return False elif bt_missions.pose_dist == 0: return False else: print("achieve first point") bt_missions.move_flag = False rospy.loginfo("first point's GPS : %lf, %lf, %lf" % (bt_missions.lat, bt_missions.lon, bt_missions.current_alt)) bt_missions.index = bt_missions.index + 1 return True elif bt_missions.index == 1 and bt_missions.dist_first == False: bt_missions.p0 = (bt_missions.current_.latitude, bt_missions.current_.longitude) bt_missions.p1 = (bt_missions.target[bt_missions.index][0], bt_missions.target[bt_missions.index][1]) bt_missions.dist = vincenty(bt_missions.p0, bt_missions.p1).meters print("current Gps dist: %s" % bt_missions.dist) #Pose distance bt_missions.pose_dist = bt_missions.scale * bt_missions.dist print("current pose dist: %s" % bt_missions.pose_dist) if bt_missions.pose_dist >= 0.02: return False else: print("achieve key point") bt_missions.move_flag = False bt_missions.ah_flag = True rospy.loginfo("keypoint point's GPS : %lf, %lf, %lf" % (bt_missions.lat, bt_missions.lon, bt_missions.current_alt)) return True @condition def yaw_correction(self): if bt_missions.index == 0: bt_missions.dLon = (bt_missions.target[bt_missions.index][1] - bt_missions.home_.geo.longitude) bt_missions.y = math.sin(bt_missions.dLon) * math.cos( bt_missions.target[bt_missions.index][0]) bt_missions.x = math.cos(bt_missions.home_.geo.latitude) * math.sin( bt_missions.target[bt_missions.index][0]) - math.sin( bt_missions.home_.geo.latitude) * cos(bt_missions.target[ bt_missions.index][0]) * math.cos(bt_missions.dLon) bt_missions.brng = math.atan2(bt_missions.y, bt_missions.x) bt_missions.brng = math.degrees(bt_missions.brng) bt_missions.brng = (bt_missions.brng + 360) % 360 bt_missions.brng = 360 - bt_missions.brng bt_missions.brng = math.radians(bt_missions.brng) bt_missions.brng = math.degrees(bt_missions.brng) bt_missions.current_yaw = bt_missions.heading print("correction heading: %s" % bt_missions.current_yaw) print("correction brng: %s" % bt_missions.brng) if bt_missions.current_yaw > ( 360 - bt_missions.brng) + 2.0 or bt_missions.current_yaw < ( 360 - bt_missions.brng) - 2.0: return False else: return True else: bt_missions.dLon = (bt_missions.target[bt_missions.index][1] - bt_missions.current_.longitude) bt_missions.y = math.sin(bt_missions.dLon) * math.cos( bt_missions.target[bt_missions.index][0]) bt_missions.x = math.cos(bt_missions.current_.latitude) * math.sin( bt_missions.target[bt_missions.index][0]) - math.sin( bt_missions.current_.latitude) * cos(bt_missions.target[ bt_missions.index][0]) * math.cos(bt_missions.dLon) bt_missions.brng = math.atan2(bt_missions.y, bt_missions.x) bt_missions.brng = math.degrees(bt_missions.brng) bt_missions.brng = (bt_missions.brng + 360) % 360 bt_missions.brng = 360 - bt_missions.brng bt_missions.brng = math.radians(bt_missions.brng) bt_missions.brng = math.degrees(bt_missions.brng) bt_missions.current_yaw = bt_missions.heading print("correction heading: %s" % bt_missions.current_yaw) print("correction brng: %s" % bt_missions.brng) if bt_missions.current_yaw > ( 360 - bt_missions.brng) + 2.0 or bt_missions.current_yaw < ( 360 - bt_missions.brng) - 2.0: return False else: return True @action def up_scale(self): bt_missions.start_heading = bt_missions.heading msg = TwistStamped() msg.header.stamp = rospy.Time.now() msg.twist.angular.x = 0.0 msg.twist.linear.x = 0.0 msg.twist.angular.y = 0.0 msg.twist.linear.y = 0.0 msg.twist.angular.z = 0.0 msg.twist.linear.z = 0.0 t = rospy.get_time() while rospy.get_time() - t <= 2: change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode) response = change_mode(custom_mode="GUIDED") bt_missions.set_vel_pub.publish(msg) bt_missions.rate.sleep() bt_missions.alt_count = bt_missions.current_alt bt_missions.z1 = bt_missions.orb_.pose.position.z print("pose z1 :%s" % bt_missions.z1) msg = TwistStamped() msg.header.stamp = rospy.Time.now() msg.twist.angular.z = 0.0 msg.twist.linear.z = 0.5 while bt_missions.current_alt - bt_missions.alt_count <= 5: change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode) response = change_mode(custom_mode="GUIDED") bt_missions.set_vel_pub.publish(msg) bt_missions.rate.sleep() msg = TwistStamped() msg.header.stamp = rospy.Time.now() msg.twist.angular.x = 0.0 msg.twist.linear.x = 0.0 msg.twist.angular.y = 0.0 msg.twist.linear.y = 0.0 msg.twist.angular.z = 0.0 msg.twist.linear.z = 0.0 t = rospy.get_time() while rospy.get_time() - t <= 2: change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode) response = change_mode(custom_mode="GUIDED") bt_missions.set_vel_pub.publish(msg) bt_missions.rate.sleep() bt_missions.alt_count2 = bt_missions.current_alt print("current altitude : %s" % bt_missions.alt_count2) bt_missions.z2 = bt_missions.orb_.pose.position.z print("pose z2 :%s" % bt_missions.z2) bt_missions.scale = math.fabs(bt_missions.z2 - bt_missions.z1) / ( bt_missions.alt_count2 - bt_missions.alt_count) print("scale one: %s" % bt_missions.scale) bt_missions.orb_scale = True @action def correction(self): #orb lost tracking if bt_missions.orb_.pose.position.z == 0 and bt_missions.orb_.pose.position.y == 0 and bt_missions.orb_.pose.position.x == 0: bt_missions.current_yaw = bt_missions.heading msg = TwistStamped() msg.header.stamp = rospy.Time.now() msg.twist.linear.z = 0 bt_missions.current_yaw = bt_missions.heading if 360 - bt_missions.brng - bt_missions.current_yaw > 0 and 360 - bt_missions.brng - bt_missions.current_yaw < 180: msg.twist.angular.z = -0.1 elif 360 - bt_missions.brng - bt_missions.current_yaw > 0 and 360 - bt_missions.brng - bt_missions.current_yaw >= 180: msg.twist.angular.z = 0.1 elif 360 - bt_missions.brng - bt_missions.current_yaw < 0 and math.fabs( 360 - bt_missions.brng - bt_missions.current_yaw) >= 180: msg.twist.angular.z = -0.1 elif 360 - bt_missions.brng - bt_missions.current_yaw < 0 and math.fabs( 360 - bt_missions.brng - bt_missions.current_yaw) < 180: msg.twist.angular.z = 0.1 bt_missions.current_yaw = bt_missions.heading while (bt_missions.current_yaw > (360 - bt_missions.brng) + 2.0 or bt_missions.current_yaw < (360 - bt_missions.brng) - 2.0) and ( bt_missions.orb_.pose.position.z != 0 and bt_missions.orb_.pose.position.y != 0 and bt_missions.orb_.pose.position.x != 0): change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode) response = change_mode(custom_mode="GUIDED") bt_missions.set_vel_pub.publish(msg) bt_missions.current_yaw = bt_missions.heading bt_missions.rate.sleep() print("first correction heading : %s" % bt_missions.current_yaw) if bt_missions.orb_.pose.position.z == 0 and bt_missions.orb_.pose.position.y == 0 and bt_missions.orb_.pose.position.x == 0: print("orb lost tracking") bt_missions.current_yaw = bt_missions.heading bt_missions.dLon = (bt_missions.target[bt_missions.index - 1][1] - bt_missions.current_.longitude) bt_missions.y = math.sin(bt_missions.dLon) * math.cos( bt_missions.target[bt_missions.index - 1][0]) bt_missions.x = math.cos(bt_missions.current_.latitude) * math.sin( bt_missions.target[bt_missions.index - 1][0]) - math.sin( bt_missions.current_.latitude) * cos(bt_missions.target[ bt_missions.index][0]) * math.cos(bt_missions.dLon) bt_missions.brng = math.atan2(bt_missions.y, bt_missions.x) bt_missions.brng = math.degrees(bt_missions.brng) bt_missions.brng = (bt_missions.brng + 360) % 360 bt_missions.brng = 360 - bt_missions.brng bt_missions.brng = math.radians(bt_missions.brng) bt_missions.brng = math.degrees(bt_missions.brng) msg = TwistStamped() msg.header.stamp = rospy.Time.now() msg.twist.linear.z = 0 bt_missions.current_yaw = bt_missions.heading if 360 - bt_missions.brng - bt_missions.current_yaw > 0 and 360 - bt_missions.brng - bt_missions.current_yaw < 180: msg.twist.angular.z = -0.1 elif 360 - bt_missions.brng - bt_missions.current_yaw > 0 and 360 - bt_missions.brng - bt_missions.current_yaw >= 180: msg.twist.angular.z = 0.1 elif 360 - bt_missions.brng - bt_missions.current_yaw < 0 and math.fabs( 360 - bt_missions.brng - bt_missions.current_yaw) >= 180: msg.twist.angular.z = -0.1 elif 360 - bt_missions.brng - bt_missions.current_yaw < 0 and math.fabs( 360 - bt_missions.brng - bt_missions.current_yaw) < 180: msg.twist.angular.z = 0.1 bt_missions.current_yaw = bt_missions.heading while bt_missions.current_yaw > ( 360 - bt_missions.brng) + 2.0 or bt_missions.current_yaw < ( 360 - bt_missions.brng) - 2.0: change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode) response = change_mode(custom_mode="GUIDED") bt_missions.set_vel_pub.publish(msg) bt_missions.current_yaw = bt_missions.heading bt_missions.rate.sleep() print("orb correction heading : %s" % bt_missions.current_yaw) @action def up(self): msg = TwistStamped() msg.header.stamp = rospy.Time.now() msg.twist.linear.z = 0.8 while bt_missions.current_alt <= 15: change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode) change_mode(custom_mode="GUIDED") bt_missions.set_vel_pub.publish(msg) bt_missions.rate.sleep() print("current altitude : %s" % bt_missions.current_alt) @action def forward(self): bt_missions.move_flag = True #body_frame msg = TwistStamped() msg.header.stamp = rospy.Time.now() msg.twist.angular.z = 0.0 print("forward") vel = PositionTarget() vel.header.stamp = rospy.Time.now() vel.coordinate_frame = PositionTarget.FRAME_BODY_NED vel.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.FORCE + PositionTarget.IGNORE_YAW + PositionTarget.IGNORE_YAW_RATE vel.velocity.x = 0.0 vel.velocity.y = 1.0 vel.velocity.z = 0.0 forward_t = rospy.get_time() while rospy.get_time() - forward_t <= bt_missions.dist * 0.1: change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode) response = change_mode(custom_mode="GUIDED") bt_missions.set_vel_pub.publish(msg) bt_missions.set_v_pub.publish(vel) bt_missions.rate.sleep() @action def change_alt(self): msg = TwistStamped() msg.header.stamp = rospy.Time.now() msg.twist.linear.z = 0.8 while bt_missions.current_alt <= 35: change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode) response = change_mode(custom_mode="GUIDED") bt_missions.set_vel_pub.publish(msg) bt_missions.rate.sleep() print("current altitude : %s" % bt_missions.current_alt) def run(self): while True: if bt_missions.isContinue == False: break bt_count = self.tree.blackboard(1) bt_state = bt_count.tick() print("bt state = %s\n" % bt_state) while bt_count == RUNNING: bt_state = bt_count.tick() print("state = %s\n" % bt_state) assert bt_state == SUCCESS or bt_state == FAILURE
def auto_demo(): rospy.init_node('autonomous_demo') #Requirements pub1 = rospy.Publisher('/gps/fix', NavSatFix, queue_size =10) pub2 = rospy.Publisher('/imu/data', Imu ,queue_size = 10) pub3 = rospy.Publisher('/odometry/wheel', Odometry, queue_size = 10) pub4 = rospy.Publisher('/gps_waypoint_handler/status', String, queue_size = 10) #Autonomous movement #pub5 = rospy.Publisher('/move_base/status',GoalStatusArray,queue_size=10) #Image Detect pub6 = rospy.Publisher('/image_detect_topic',String,queue_size=10) #Image Reach pub7 = rospy.Publisher('/image_reach_topic',String,queue_size=10) rate = rospy.Rate(10) # 10hz count = 0 while not rospy.is_shutdown(): print("0: All sensors are good.") print("1: All sensors except encoder are good.") print("2: Damaged Sensors") print("3: Got Waypoint") print("4: Did not get any waypoint") print("5: Reached to way point") print("6: Did not Reached to way point") print("7: Detected Image") print("8: Did not detect Image") print("9: Reached Image") print("10: Did not reached image") imuMsg = Imu() imuMsg.orientation.x = 5 imuMsg.orientation.y = 5 gpsMsg = NavSatFix() gpsMsg.latitude = 5 gpsMsg.longitude = 5 encoderMsg = Odometry() encoderMsg.pose.pose.position.x = 5 encoderMsg.pose.pose.position.y = 5 wpStatusMsgLow = GoalStatus() wpStatusMsgLow.status = 3 wpStatusArray =[] wpStatusArray.append(wpStatusMsgLow) wpStatusMsg = GoalStatusArray() wpStatusMsg.status_list = wpStatusArray userInput = raw_input() if userInput == "0": #All sensors are good. pub1.publish(gpsMsg) pub2.publish(imuMsg) pub3.publish(encoderMsg) elif userInput == "1": # All sensors except encoder are good. pub1.publish(gpsMsg) pub2.publish(imuMsg) #pub3.publish("0") elif userInput == "2": #Damaged Sensors pub1.publish(gpsMsg) #pub2.publish("0") pub3.publish(encoderMsg) elif userInput == "3": # Got Waypoint pub4.publish("1") #elif userInput == "4": #Did not get any waypoint #pub4.publish("0") elif userInput == "5": #Reached to way point #pub5.publish(wpStatusMsg) pub4.publish("2") #elif userInput == "6": #Did not reached to way point #pub5.publish("0") elif userInput == "7": #Detected Image pub6.publish("1") elif userInput == "8": #Did not Detect Image pub6.publish("0") elif userInput == "9": #Reached Image pub7.publish("1") elif userInput == "10": #Did not reached image pub7.publish("0") else: print("Invalid entry") rate.sleep();
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: data = parsed_sentence['GGA'] gps_qual = data['fix_type'] if gps_qual == 0: current_fix.status.status = NavSatStatus.STATUS_NO_FIX elif gps_qual == 1: current_fix.status.status = NavSatStatus.STATUS_FIX elif gps_qual == 2: current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX elif gps_qual in (4, 5): current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude hdop = data['hdop'] current_fix.position_covariance[0] = hdop**2 current_fix.position_covariance[4] = hdop**2 current_fix.position_covariance[8] = (2 * hdop)**2 # FIXME current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) else: return False
#!/usr/bin/env python import rospy from geometry_msgs.msg import Twist from sensor_msgs.msg import NavSatFix from math import * from sensor_msgs.msg import Imu from tf.transformations import euler_from_quaternion ob1 = Twist() ob2 = NavSatFix() ob3 = NavSatFix() def arduino_map(x, in_min, in_max, out_min, out_max): return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min def cartesian_distance(lat1, lon1, lat2, lon2): return sqrt((lat2 - lat1) ** 2 + (lon2 - lon1) ** 2) def haversine(lat1, lon1, lat2, lon2): # distance between latitudes # and longitudes dLat = (lat2 - lat1) * pi / 180.0 dLon = (lon2 - lon1) * pi / 180.0 # convert to radians lat1 = lat1 * pi / 180.0 lat2 = lat2 * pi / 180.0