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)