def parse_takmsg_route(self): try: msg_type = self.takmsg_tree.getroot().attrib['type'] if msg_type == 'b-m-r': robot_path = Path() robot_path.header.stamp = rospy.Time.now() robot_path.header.frame_id = 'utm' waypoints = self.takmsg_tree.findall("./detail/link") for wp in waypoints: pnt_str = wp.attrib['point'].split(",") (lat,lon) = (float(pnt_str[0]),float(pnt_str[1])) (zone,crnt_utm_e,crnt_utm_n) = LLtoUTM(23, lat, lon) rospy.loginfo("waypoint is: %f, %f" %(crnt_utm_e,crnt_utm_n)) wp_pose = PoseStamped() wp_pose.header = robot_path.header wp_pose.pose.position.x = float(crnt_utm_e) wp_pose.pose.position.y = float(crnt_utm_n) wp_pose.pose.orientation.w = 1 robot_path.poses.append(wp_pose) self.path_pub.publish(robot_path) # if 'husky1' in msg_callsign: rospy.loginfo("============> got route\n ") except Exception as e: rospy.logwarn("\n\n----- Recieved ATAK Message and have an error of: "+ str(e) + '\n\n')
def update_utm(self, current_latlong): #rospy.loginfo("Current Lat, Long is: (%f,%f)" %((current_latlong.latitude),(current_latlong.longitude))) self.current_utm = LLtoUTM(23, current_latlong.latitude, current_latlong.longitude) self.curLat = current_latlong.latitude self.curLong = current_latlong.longitude
def parse_takmsg_air(self): fiveline = self.takmsg_tree.find("./detail/fiveline") #rospy.loginfo("fiveline:%s" %fiveline) if not (fiveline in (-1, None)): target_num = fiveline.attrib['fiveline_target_number'] # If this is a goto location then publish it as a go to goal. # Assumes utm is the global frame. if ('U99999' == target_num): try: this_uid = self.takmsg_tree.get("uid") lat = self.takmsg_tree.find("./point").attrib['lat'] lon = self.takmsg_tree.find("./point").attrib['lon'] except Exception, e: rospy.logwarn( "----- Recieved ATAK Message and it is not a move to command -----" + str(e)) (zone, crnt_utm_e, crnt_utm_n) = LLtoUTM(23, float(lat), float(lon)) goal_pose_stamped = PoseStamped() goal_pose_stamped.header.stamp = rospy.Time.now() goal_pose_stamped.header.frame_id = 'utm' goal_pose_stamped.pose.position.x = crnt_utm_e goal_pose_stamped.pose.position.y = crnt_utm_n msg = self.tf1_listener.transformPose(self.global_frame, goal_pose_stamped) msg.pose.position.z = 5.0 self.uav_pub.publish(msg) rospy.loginfo( "----- Recieved ATAK Message from UID: %s, saying move to lat/lon of %s, %s and map location %s, %s" % (this_uid, lat, lon, crnt_utm_e, crnt_utm_n))
def parse_takmsg_goto(self): try: msg_root = self.takmsg_tree.getroot() if not(msg_root in (-1, None)): msg_uid = msg_root.attrib['uid'] robot_uid = self.robot_name + '_goto' #if (robot_uid == msg_uid): rospy.loginfo("Mesage UID: %s",msg_uid) if (self.robot_msg_uid == msg_uid): lat = float(self.takmsg_tree.find("./point").attrib['lat']) lon = float(self.takmsg_tree.find("./point").attrib['lon']) detail = self.takmsg_tree.find("./detail").find("./remarks").text.split(',') desired_orientation = detail[2] desired_altitude = detail[3] (zone,crnt_utm_e,crnt_utm_n) = LLtoUTM(23, lat, lon) msg_pose = PoseStamped() msg_pose.header.stamp = rospy.Time.now() msg_pose.header.frame_id = 'utm' msg_pose.pose.position.x = crnt_utm_e msg_pose.pose.position.y = crnt_utm_n if (desired_altitude != 'NA'): print(type(desired_orientation),desired_orientation) msg_pose.pose.position.z = float(desired_altitude) if (desired_orientation != 'NA'): orientation_quat = tf.transformations.quaternion_from_euler(0,0,float(desired_orientation)/180*3.14) msg_pose.pose.orientation.x = orientation_quat[0] msg_pose.pose.orientation.y = orientation_quat[1] msg_pose.pose.orientation.z = orientation_quat[2] msg_pose.pose.orientation.w = orientation_quat[3] else: msg_pose.pose.orientation.z = 0.0516597603525 msg_pose.pose.orientation.w = 0.998664743125 msg_pose2 = self.tf1_listener.transformPose(self.map_frame, msg_pose) msg = PoseDescriptionStamped() msg.header.stamp = msg_pose.header.stamp msg.header.frame_id = self.map_frame msg.pose.pose = msg_pose2.pose msg.pose.description.data = robot_uid self.goal_pub.publish(msg) rospy.loginfo("\n\n----- Recieved ATAK Message from UID: %s, saying move to lat/lon of %s, %s heading: %s altitude: %s\n\n" %(msg_uid,lat,lon,desired_orientation,desired_altitude)) return (lat,lon,desired_orientation,desired_altitude) return 'na' # Not a message for the robot except Exception as e: rospy.logwarn("\n\n----- Recieved ATAK Message and have an error of: "+ str(e) + '\n\n') return 'na' # Cant parse the ATAK message
def makeWaypointsIntoGoals(self, filename): log_directory = rospy.get_param("~log_directory", "~/") + "waypoints2Goals_log.csv" # Load and parse waypoints from file. wps = self.loadWaypoints(filename) goals = [] for waypointLat, waypointLong, search_duration, rest_duration in wps: goal_pose = Pose() (wpZone, wpEasting, wpNorthing) = LLtoUTM(23, waypointLat, waypointLong) goal_pose.position = deepcopy(self.initial_pose.pose.pose.position) goal_pose.position.x = (wpEasting - self.initial_utm[1] ) # REP103 says x is east and y is north goal_pose.position.y = (wpNorthing - self.initial_utm[2]) goals.append((goal_pose, search_duration, rest_duration)) return goals
def makeWaypointsIntoGoals(self, filename): time.sleep(1) #while (not self.initial_utm): # rospy.loginfo("Waiting for initial position") # Load and parse waypoints from file. wps = self.loadWaypoints(filename) goalstemp=[] for waypointLat,waypointLong,search_duration,rest_duration,unused,identity in wps: goal_pose=Pose() (wpZone,wpEasting,wpNorthing)=LLtoUTM(23, waypointLat,waypointLong) goal_pose.position=deepcopy(self.initial_pose.pose.pose.position) goal_pose.position.x=(wpEasting - self.initial_utm[1]) # REP103 says x is east and y is north goal_pose.position.y=(wpNorthing - self.initial_utm[2]) goal_pose.orientation.x,goal_pose.orientation.y,goal_pose.orientation.z,goal_pose.orientation.w=[0,0,0,1] #(wpNorthing - self.initial_utm[2]) self.vis_pub.publish( make_waypoint_viz(goal_pose,identity[-2:],int(identity[-2:]) )) rospy.loginfo("x,y for waypoint#: %s in odom frame: (%.4f, %.4f)"%(identity[-2:],goal_pose.position.x, goal_pose.position.y)) goalstemp.append((goal_pose, search_duration, rest_duration)) return goalstemp
print "GPS", gps_flag print "Current", lat, lon print "Dest", destLat, destLon print "DISTNACE:", distance print "desBearing_hav:", desBearing print "desBearing_utm:", utm_bearing print "curBearing:", robot_heading print "lineFollowing", rightFollow_flag #print "Distance", distance elif len(controller.buttons) > 1 and controller.buttons[2] == 0: prevX = 0 #Nav ... autonomous switch ##FIX distance, desBearing = haversine(lat, lon, destLat, destLon) ##CONVERSTION (originZone, destX, destY) = LLtoUTM(23, destLat, destLon) ##UTM utm_distance, utm_bearing = haversineUtm(utm_xcoord, utm_ycoord, destX, destY) if autonomous_flag and gps_flag == False and gpsSwitchDistance > distance: gps_flag = True print "GPS ON" #Nav ... reach destination if waypoint_flag and gps_flag and distance < 1: #reach waypoint (close enough) print "Reached Waypoint!!!" destination_flag = True waypoint_flag = False ##Autonomous Mode if len(controller.buttons ) > 1 and controller.buttons[1] == 1 and prevB == 0:
def parse_takmsg_grnd(self): # etree = ET.tostring(self.takmsg_tree, 'utf-8') # rospy.loginfo("takmsg_tree:%s" %etree) try: fiveline = self.takmsg_tree.find("./detail/fiveline") #rospy.loginfo("fiveline:%s" %fiveline) if not (fiveline in (-1, None)): target_num = fiveline.attrib['fiveline_target_number'] # If this is a goto location then publish it as a go to goal. # Assumes utm is the global frame. if ('G99999' == target_num): this_uid = self.takmsg_tree.get("uid") lat = self.takmsg_tree.find("./point").attrib['lat'] lon = self.takmsg_tree.find("./point").attrib['lon'] (zone, crnt_utm_e, crnt_utm_n) = LLtoUTM(23, float(lat), float(lon)) goal_pose_stamped = PoseStamped() goal_pose_stamped.header.stamp = rospy.Time.now() goal_pose_stamped.header.frame_id = 'utm' goal_pose_stamped.pose.position.x = crnt_utm_e goal_pose_stamped.pose.position.y = crnt_utm_n goal_pose_stamped = self.tf1_listener.transformPose( 'husky/map', goal_pose_stamped) goal_pose_stamped.pose.orientation.z = 0.0985357937255 goal_pose_stamped.pose.orientation.w = 0.995133507302 msg = GotoRegionActionGoal() msg.header.stamp = rospy.Time.now() msg.goal_id.stamp = rospy.Time.now() msg.goal_id.id = "ATAK GOTO" msg.goal.region_center.header.stamp = msg.header.stamp msg.goal.region_center.header.frame_id = "husky/map" msg.goal.region_center.pose = goal_pose_stamped.pose msg.goal.radius = 3.75 msg.goal.angle_threshold = 3.108 self.grnd_pub.publish(msg) marker = Marker() marker.header.frame_id = "husky/map" msg.header.stamp = rospy.Time.now() marker.ns = "husky" marker.id = 0 marker.type = Marker.SPHERE marker.action = Marker.ADD marker.pose = goal_pose_stamped.pose marker.scale.x = 0.5 marker.scale.y = 0.5 marker.scale.z = 0.5 marker.color.a = 1.0 marker.color.r = 1.0 self.vis_pub.publish(marker) rospy.loginfo( "----- Recieved ATAK Message from UID: %s, saying move to lat/lon of %s, %s and map location %s, %s" % (this_uid, lat, lon, msg.goal.region_center.pose.position.x, msg.goal.region_center.pose.position.y)) except Exception, e: rospy.logwarn( "----- Recieved ATAK Message and have an error of: " + str(e))
def update_utm_cb(self, msg): #rospy.loginfo("Current Lat, Long is: (%f,%f)" %((msg.latitude),(msg.longitude))) self.current_utm = LLtoUTM(23, msg.latitude,msg.longitude)