def pos_set():
global state_id, extend_set_id
pub = rospy.Publisher('/mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
rospy.init_node('UAV_setpoint')
rospy.Subscriber('/mavros/state', State, set_id)
rospy.Subscriber('/mavros/extended_state', ExtendedState, extend_set_id)
rate = rospy.Rate(15)
frame_id = 1
extend_state_id = "FLYING"
state_id = 1
zdown = 0
zhover = 0.45
while not rospy.is_shutdown():
rospy.loginfo(str(frame_id))
if state_id is 1:
pos = PoseStamped()
pos.header.stamp = rospy.Time.now()
pos.pose.position.x = 0
pos.pose.position.y = 0
pos.pose.position.z = 0
quat = qfe(0, 0, pi / 2)
pos.pose.orientation.w = quat[3]
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pub.publish(pos)
elif 1 < frame_id < 150:
pos = PoseStamped()
pos.header.stamp = rospy.Time.now()
pos.pose.position.x = 0
pos.pose.position.y = 0
pos.pose.position.z = zhover
quat = qfe(0, 0, pi / 2)
pos.pose.orientation.w = quat[3]
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pub.publish(pos)
else:
if extend_state_id is 'FLYING':
zdown -= 1
pos = PoseStamped()
pos.header.stamp = rospy.Time.now()
pos.pose.position.x = 0
pos.pose.position.y = 0
pos.pose.position.z = zdown
quat = qfe(0, 0, pi / 2)
pos.pose.orientation.w = quat[3]
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pub.publish(pos)
if extend_state_id is 'LANDED':
rospy.set_param('mavsafety', 'disarm')
break
if state_id is 2:
frame_id += 1
rate.sleep()
def pos_set():
global state_id, extend_set_id
pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10)
rospy.init_node('UAV_setpoint')
rospy.Subscriber('/mavros/state',State,set_id)
rospy.Subscriber('/mavros/extended_state',ExtendedState,extend_set_id)
rate = rospy.Rate(15)
frame_id = 1
extend_state_id = "FLYING"
state_id = 1
zdown = 0
zhover = 0.45
while not rospy.is_shutdown():
rospy.loginfo(str(frame_id))
if state_id is 1:
pos = PoseStamped()
pos.header.stamp = rospy.Time.now()
pos.pose.position.x=0
pos.pose.position.y=0
pos.pose.position.z=0
quat = qfe(0,0,pi/2)
pos.pose.orientation.w = quat[3]
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pub.publish(pos)
elif 1 < frame_id < 150:
pos = PoseStamped()
pos.header.stamp = rospy.Time.now()
pos.pose.position.x=0
pos.pose.position.y=0
pos.pose.position.z=zhover
quat = qfe(0,0,pi/2)
pos.pose.orientation.w = quat[3]
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pub.publish(pos)
else:
if extend_state_id is 'FLYING':
zdown -= 1
pos = PoseStamped()
pos.header.stamp = rospy.Time.now()
pos.pose.position.x=0
pos.pose.position.y=0
pos.pose.position.z=zdown
quat = qfe(0,0,pi/2)
pos.pose.orientation.w = quat[3]
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pub.publish(pos)
if extend_state_id is 'LANDED':
rospy.set_param('mavsafety','disarm')
break
if state_id is 2:
frame_id += 1
rate.sleep()
def goal_rotate(self):
destination = MoveBaseGoal()
destination.target_pose.header.frame_id = self.field_frame_id
destination.target_pose.header.stamp = rospy.Time.now()
destination.target_pose.pose.position.x = -0.54
destination.target_pose.pose.position.y = 2
rotate = 0
connected_to_move_base = False
dur = rospy.Duration(1.0)
while not connected_to_move_base:
# Wait for the server for a while
connected_to_move_base = self.move_base_client.wait_for_server(dur)
# Check to make sure ROS is ok still
if rospy.is_shutdown(): return
# Update the user on the status of this process
msg = "Path Planner: waiting on move_base."
rospy.loginfo(msg)
while not rospy.is_shutdown():
rotate += 1.57
rospy.loginfo("Sending rotation %.2f" % rotate)
quat = qfe(0, 0, rotate)
destination.target_pose.pose.orientation.x = quat[0]
destination.target_pose.pose.orientation.y = quat[1]
destination.target_pose.pose.orientation.z = quat[2]
destination.target_pose.pose.orientation.w = quat[3]
self.move_base_client.send_goal(destination)
self.move_base_client.wait_for_result()
rospy.loginfo("Get result from goal")
rospy.Rate(1).sleep()
def do_qualification(self):
"""
Executes a qualification run.
"""
for waypoint in self.path:
if rospy.is_shutdown(): return
# Cancel any current goals
self.move_base_client.cancel_all_goals()
# Push the goal to the actionlib server
destination = MoveBaseGoal()
destination.target_pose.header.frame_id = "odom"
destination.target_pose.header.stamp = rospy.Time.now()
# Set the target location
destination.target_pose.pose.position.x = waypoint[0]
destination.target_pose.pose.position.y = waypoint[1]
# Set the heading
quat = qfe(0, 0, waypoint[2])
destination.target_pose.pose.orientation.x = quat[0]
destination.target_pose.pose.orientation.y = quat[1]
destination.target_pose.pose.orientation.z = quat[2]
destination.target_pose.pose.orientation.w = quat[3]
# Send the desired destination to the actionlib server
rospy.loginfo("Sending waypoint (%f, %f)@%f" % tuple(waypoint))
self.move_base_client.send_goal(destination)
# Wait for the goal to finish
duration = rospy.Duration(1.0)
for x in range(60):
self.move_base_client.wait_for_result(duration)
if rospy.is_shutdown(): return
def odometry_cb(self):
if rospy.is_shutdown():
return
self.odometry_timer = threading.Timer(self.publish_rate,
self.odometry_cb)
self.odometry_timer.start()
if not self.location_initilized or not self.heading_initilized:
return
msg = Odometry()
# Header
msg.header.stamp = self.filter_time
msg.header.frame_id = self.output_frame
# Position
msg.pose.pose.position = Vector3(self.ekf.getEasting(),
self.ekf.getNorthing(), 0)
quat = qfe(0, 0, self.ekf.getYaw())
msg.pose.pose.orientation.x = quat[0]
msg.pose.pose.orientation.y = quat[1]
msg.pose.pose.orientation.z = quat[2]
msg.pose.pose.orientation.w = quat[3]
msg.pose.covariance[0] = self.ekf.P[0, 0]
msg.pose.covariance[7] = self.ekf.P[1, 1]
msg.pose.covariance[15] = self.ekf.P[2, 2]
msg.twist.twist.linear = Vector3(self.v, 0, 0)
msg.twist.twist.angular = Vector3(0, 0, self.w)
self.odom_pub.publish(msg)
if self.output_tf:
br = tf.TransformBroadcaster()
br.sendTransform((self.ekf.getEasting(), self.ekf.getNorthing(), 0),
qfe(0, 0, self.ekf.getYaw()),
self.filter_time,
"/base_footprint",
self.output_frame)
if self.publish_states:
self.states_pub.publish(States(self.ekf.getStateList(),
self.ekf.getPList()))
return
def _handle_start_following(self):
#make sure server is available before sending goal
QApplication.setOverrideCursor(QCursor(Qt.WaitCursor))
if (not self._client.wait_for_server(rospy.Duration.from_sec(10))):
QApplication.restoreOverrideCursor()
msg_box = QMessageBox()
msg_box.setText("Timeout while looking for path following control server.")
msg_box.exec_()
return
QApplication.restoreOverrideCursor()
#get path file to load
filename = QFileDialog.getOpenFileName(self, self.tr('Load Path from File'), '.', self.tr('Path File {.txt} (*.txt)'))
if filename[0] != '':
try:
handle = open(filename[0])
except IOError as e:
qWarning(str(e))
return
# load path from file
path_reader=csv.reader(open(filename[0],'rb'),delimiter=';')
# parse path file and convert to numbers
temp = [row[0].split() for row in path_reader]
lines= [[float(num) for num in row] for row in temp]
# first line of file is radius
radius = lines[0]
# remove radius
del(lines[0])
now = rospy.Time.now()
goal = auxos_messages.msg.PlanThenFollowDubinsPathGoal()
goal.path.header.stamp = now
goal.path.header.frame_id = self.path_frame_id # TODO: fix this!!!
for line in lines:
pose_msg = PoseStamped()
pose_msg.header.stamp = now
pose_msg.header.frame_id = goal.path.header.frame_id
pose_msg.pose.position.x = line[0]
pose_msg.pose.position.y = line[1]
quat = qfe(0, 0, psi2theta(line[2]))
pose_msg.pose.orientation.x = quat[0]
pose_msg.pose.orientation.y = quat[1]
pose_msg.pose.orientation.z = quat[2]
pose_msg.pose.orientation.w = quat[3]
goal.path.poses.append(pose_msg)
self._client.send_goal(goal, self._handle_path_complete, self._handle_active, self._handle_feedback)
print("start following")
def callback(msg):
global pub, rotation
# If it is the base_link -> openni_camera tf rotate
for transform in msg.transforms:
if transform.header.frame_id == '/base_link' and \
transform.child_frame_id == '/openni_camera':
quat = qfe(0,radians(rotation),0)
transform.transform.rotation.x = quat[0]
transform.transform.rotation.y = quat[1]
transform.transform.rotation.z = quat[2]
transform.transform.rotation.w = quat[3]
# Republish the msg
pub.publish(msg)
def callback(msg):
global last_time, yaw, pub, last_yaw, count, pub_pose
now = msg.header.stamp
if last_time == None:
last_time = now
yaw = 0
last_yaw = 0
count = 0
return
count += 1
q = msg.orientation
delta_yaw = efq((q.x, q.y, q.z, q.w))[2] * (now - last_time).to_sec()
# delta_yaw = efq((q.x, q.y, q.z, q.w))[2] * 1.0/200.0
yaw += delta_yaw
last_time = now
if count != 10:
return
count = 0
delta_yaw = last_yaw - yaw
last_yaw = yaw
covar = 1.0/abs(radians(delta_yaw))
print delta_yaw, radians(delta_yaw), covar
q = qfe(0, 0, radians(yaw))
header = msg.header
msg = PoseStamped()
msg.header = header
msg.pose.orientation.x = q[0]
msg.pose.orientation.y = q[1]
msg.pose.orientation.z = q[2]
msg.pose.orientation.w = q[3]
pub_pose.publish(msg)
msg = Imu()
msg.header = header
msg.orientation.x = q[0]
msg.orientation.y = q[1]
msg.orientation.z = q[2]
msg.orientation.w = q[3]
msg.orientation_covariance = [1e+100, 0, 0,
0, 1e+100, 0,
0, 0, covar]
pub.publish(msg)
def vizTheta(self, theta):
msg = PoseStamped()
msg.header.seq = self.count
msg.header.stamp.secs = rospy.get_rostime().secs
msg.header.stamp.nsecs = rospy.get_rostime().nsecs
msg.header.frame_id = self.frame_id
msg.pose.position.x = self.x_bot
msg.pose.position.y = self.y_bot
q = qfe(0, 0, theta)
msg.pose.orientation.x = q[0]
msg.pose.orientation.y = q[1]
msg.pose.orientation.z = q[2]
msg.pose.orientation.w = q[3]
self.count += 1
self.pub_theta_viz.publish(msg)
return
def publish_sim_path_as_pose_array(self, sim_path, frame_id):
"""Publishes the given sim path as a ROS pose array"""
msg = PoseArray()
msg.header.frame_id = frame_id
msg.header.stamp = rospy.Time.now()
if sim_path:
for waypoint in sim_path:
pose = Pose()
pose.position.x = waypoint[1]
pose.position.y = waypoint[2]
quat = qfe(0, 0, waypoint[3])
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
msg.poses.append(pose)
self.pose_array_pub.publish(msg)
def pos_set():
pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10)
rospy.init_node('pos_set_node', anonymous=True)
rate = rospy.Rate(20)
frame_id = 1
while not rospy.is_shutdown():
pos = PoseStamped()
pos.header.stamp = rospy.Time.now()
pos.pose.position.x=0
pos.pose.position.y=0
pos.pose.position.z=-1.5
quat = qfe(0,0,0)
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pos.pose.orientation.w = quat[3]
pub.publish(pos)
frame_id += 1
rate.sleep()
def pos_set():
pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10)
rospy.init_node('UAV_setpoint')
rate = rospy.Rate(15)
frame_id = 1
while not rospy.is_shutdown():
pos = PoseStamped()
pos.header.stamp = rospy.Time.now()
pos.pose.position.x=0
pos.pose.position.y=0
pos.pose.position.z=0.4
quat = qfe(0,0,pi/2)
pos.pose.orientation.w = quat[3]
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pub.publish(pos)
frame_id += 1
rate.sleep()
def step_path_following(self):
"""
Steps the path following system, checking if new waypoints
should be sent, if a timeout has occurred, or if path following
needs to be paused.
"""
# Visualize the data
self.visualize_path(self.path, self.path_status)
# Get the next (or current) waypoint
current_waypoint_index = self.get_next_waypoint_index()
# If the index is None, then we are done path planning
if current_waypoint_index == None:
rospy.loginfo("Path Planner: Done.")
return False
if current_waypoint_index == 0:
self.path_status[current_waypoint_index] = 'visited'
# Get the waypoint and status
current_waypoint = self.path[current_waypoint_index]
current_waypoint_status = self.path_status[current_waypoint_index]
# If the status is visited
print current_waypoint
if current_waypoint_status == 'visited':
# This shouldn't happen...
return True
# If the status is not_visited then we need to push the goal
if current_waypoint_status == 'not_visited':
# Cancel any current goals
#self.move_base_client.cancel_all_goals()
# Set it to visiting
self.path_status[current_waypoint_index] = 'visiting'
# Push the goal to the actionlib server
destination = MoveBaseGoal()
destination.target_pose.header.frame_id = self.field_frame_id
destination.target_pose.header.stamp = rospy.Time.now()
# Set the target location
destination.target_pose.pose.position.x = current_waypoint[0]
destination.target_pose.pose.position.y = current_waypoint[1]
# Calculate the distance
if self.previous_destination == None:
self.current_distance = 5.0
else:
self.current_distance = self.distance(self.previous_destination, destination)
# Set the heading
quat = qfe(0, 0, current_waypoint[2])
destination.target_pose.pose.orientation.x = quat[0]
destination.target_pose.pose.orientation.y = quat[1]
destination.target_pose.pose.orientation.z = quat[2]
destination.target_pose.pose.orientation.w = quat[3]
# Send the desired destination to the actionlib server
rospy.loginfo("Sending waypoint (%f, %f)@%f" % tuple(current_waypoint))
self.current_destination = destination
self.move_base_client.send_goal(destination)
if self.current_distance < self.cut_spacing + 0.1:
rospy.sleep(0.1)
self.move_base_client.cancel_goal()
self.previous_destination = destination
# If the status is visiting, then we just need to monitor the status
temp_state = self.move_base_client.get_goal_status_text()
if current_waypoint_status == 'visiting':
if temp_state in ['ABORTED', 'SUCCEEDED']:
self.path_status[current_waypoint_index] = 'visited'
else:
duration = rospy.Duration(2.0)
from math import floor
count = 0
self.move_base_client.wait_for_result()
if self.timeout == True:
if count == 6+int(self.current_distance*4):
rospy.logwarn("Path Planner: move_base goal timeout occurred, clearing costmaps")
# Cancel the goals
self.move_base_client.cancel_all_goals()
# Clear the cost maps
self.clear_costmaps()
# Wait 1 second
rospy.Rate(1.0).sleep()
# Then reset the current goal
if not self.just_reset:
self.just_reset = True
self.path_status[current_waypoint_index] = 'not_visited'
else:
self.just_reset = False
self.path_status[current_waypoint_index] = 'visited'
return True
self.path_status[current_waypoint_index] = 'visited'
return True
def publish_markers(self, thetaWalker, thetaDiff, thetaA, thetaB):
markers = []
marker = Marker()
marker.header.seq = 1
marker.header.stamp = self.rospy.get_rostime()
marker.header.frame_id = "map"
marker.id = 1
marker.type = 0
marker.action = 0
#translation, rotation = self.tf_listener.lookupTransform("/map", "/odom")
#matrix = self.tf_listener.fromTranslationRotation(translation, rotation)
theta = self.rotMapOdom
rotation = np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]])
translation = np.array([[self.tx], [self.ty]])
pos = np.array([[self.pose_walker.pose.pose.position.x],
[self.pose_walker.pose.pose.position.y]])
new_pos = rotation.dot(pos) + translation
marker.pose.position.x = new_pos[0]
marker.pose.position.y = new_pos[1]
quat = qfe(0, 0, thetaDiff - thetaA)
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.scale.x = 1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 1
marker.color.r = 0
marker.color.g = 1
marker.color.b = 0
markers.append(marker)
marker = Marker()
marker.header.seq = 2
marker.header.stamp = self.rospy.get_rostime()
marker.header.frame_id = "map"
marker.id = 2
marker.type = 0
marker.action = 0
pos = np.array([[self.pose_walker.pose.pose.position.x],
[self.pose_walker.pose.pose.position.y]])
new_pos = rotation.dot(pos) + translation
marker.pose.position.x = new_pos[0]
marker.pose.position.y = new_pos[1]
quat = qfe(0, 0, thetaDiff + thetaB)
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.scale.x = 1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 1
marker.color.r = 0
marker.color.g = 0
marker.color.b = 1
markers.append(marker)
marker = Marker()
marker.header.seq = 3
marker.header.stamp = self.rospy.get_rostime()
marker.header.frame_id = "map"
marker.id = 3
marker.type = 0
marker.action = 0
pos = np.array([[self.pose_walker.pose.pose.position.x],
[self.pose_walker.pose.pose.position.y]])
new_pos = rotation.dot(pos) + translation
marker.pose.position.x = new_pos[0]
marker.pose.position.y = new_pos[1]
quat = qfe(0, 0, thetaDiff)
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.scale.x = 1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 1
marker.color.r = 0
marker.color.g = 1
marker.color.b = 1
markers.append(marker)
self.pub_markers.publish(markers)
return
def visualize_path_as_marker(self, path, path_status):
"""
Publishes visualization Markers to represent the planned path.
Publishes the path as a series of spheres connected by lines.
The color of the spheres is set by the path_status parameter,
which is a list of strings of which the possible values are in
['not_visited', 'visiting', 'visited'].
"""
# Get the time
now = rospy.Time.now()
# If self.path_markers is None, initialize it
if self.path_markers == None:
self.path_markers = MarkerArray()
# # If there are existing markers, delete them
# markers_to_delete = MarkerArray()
# if len(self.path_markers.markers) > 0:
# for marker in self.path_markers.markers:
# marker.action = Marker.DELETE
# markers_to_delete.markers.append(marker)
# self.path_marker_pub.publish(markers_to_delete)
# Clear the path_markers
self.path_markers = MarkerArray()
line_strip_points = []
# Create the waypoint markers
for index, waypoint in enumerate(path):
waypoint_marker = Marker()
waypoint_marker.header.stamp = now
waypoint_marker.header.frame_id = self.field_frame_id
waypoint_marker.ns = "waypoints"
waypoint_marker.id = index
waypoint_marker.type = Marker.ARROW
if index == 0:
waypoint_marker.type = Marker.CUBE
waypoint_marker.action = Marker.MODIFY
waypoint_marker.scale.x = 1
waypoint_marker.scale.y = 1
waypoint_marker.scale.z = 0.25
point = Point(waypoint[0], waypoint[1], 0)
waypoint_marker.pose.position = point
# Store the point for the line_strip marker
line_strip_points.append(point)
# Set the heading of the ARROW
quat = qfe(0, 0, waypoint[2])
waypoint_marker.pose.orientation.x = quat[0]
waypoint_marker.pose.orientation.y = quat[1]
waypoint_marker.pose.orientation.z = quat[2]
waypoint_marker.pose.orientation.w = quat[3]
# Color is based on path_status
status = path_status[index]
if status == 'not_visited':
waypoint_marker.color = ColorRGBA(1,0,0,0.5)
elif status == 'visiting':
waypoint_marker.color = ColorRGBA(0,1,0,0.5)
elif status == 'visited':
waypoint_marker.color = ColorRGBA(0,0,1,0.5)
else:
rospy.err("Invalid path status.")
waypoint_marker.color = ColorRGBA(1,1,1,0.5)
# Put this waypoint Marker in the MarkerArray
self.path_markers.markers.append(waypoint_marker)
# Create the line_strip Marker which connects the waypoints
line_strip = Marker()
line_strip.header.stamp = now
line_strip.header.frame_id = self.field_frame_id
line_strip.ns = "lines"
line_strip.id = 0
line_strip.type = Marker.LINE_STRIP
line_strip.action = Marker.ADD
line_strip.scale.x = 0.1
line_strip.color = ColorRGBA(0,0,1,0.5)
line_strip.points = line_strip_points
self.path_markers.markers.append(line_strip)
# Publish the marker array
self.path_marker_pub.publish(self.path_markers)
def package_odom_var(self, UTMtoPub):
"""
Puts odom var (from UTM N & E) into a ros message and sends to the publishing decider.
-does all the ROS packaging common to all sources
-each individual legend is now
"""
if self.DEBUG:
print('rvizBridge: '+self.tag+': In package_odom_var')
combined_array = MarkerArray()
### Odometry Arrows ####################################################
odom_msg = Odometry()
# odom_msg.header.stamp = rospy.Time(UTMtoPub['time'])
odom_msg.header.stamp = rospy.Time.now()
odom_msg.header.frame_id = "odom" # may need to expand?
odom_msg.pose.pose.position.x = UTMtoPub['E']
odom_msg.pose.pose.position.y = UTMtoPub['N']
odom_msg.pose.pose.position.z = 1.55
# make a quaternion :: came from trial-and-error...
quat = qfe(-pi, -pi, -UTMtoPub['crs']-pi/2)
odom_msg.pose.pose.orientation.x = quat[0]
odom_msg.pose.pose.orientation.y = quat[1]
odom_msg.pose.pose.orientation.z = quat[2]
odom_msg.pose.pose.orientation.w = quat[3]
odom_msg.pose.covariance[0] = UTMtoPub['Nsd']
odom_msg.pose.covariance[7] = UTMtoPub['Esd']
odom_msg.pose.covariance[14] = 0
self.odom_publisher.publish(odom_msg)
### Error Ellipses #####################################################
ell_marker = Marker()
ell_marker.header = odom_msg.header # clarify?
ell_marker.id = 0 # enumerate subsequent markers here
ell_marker.action = Marker.MODIFY # can be ADD, REMOVE, or MODIFY
ell_marker.pose = odom_msg.pose.pose # put at same place as its odom arrow
ell_marker.lifetime = rospy.Duration() # will last forever unless modified
ell_marker.ns = "Error_Ellipses__"+ self.tag
ell_marker.type = Marker.CYLINDER
ell_marker.scale.x = abs(odom_msg.pose.covariance[0]) # not visible unless scaled up
ell_marker.scale.y = abs(odom_msg.pose.covariance[7]) # not visible unless scaled up
ell_marker.scale.z = 0.000001 # We just want a disk
ell_marker.color.r = self.color['r']
ell_marker.color.g = self.color['g']
ell_marker.color.b = self.color['b']
ell_marker.color.a = self.err_ell_opacity
ell_marker.color.a = 0.5
self.ell_publisher.publish(ell_marker)
### Legend #############################################################
legend_marker = Marker()
legend_marker.header = odom_msg.header
legend_marker.id = 0
legend_marker.ns = ''.join(["Error_Ellipses", '__', self.tag, '__', self.tag])
legend_marker.type = Marker.TEXT_VIEW_FACING
legend_marker.text = self.legend_text
legend_marker.action = Marker.MODIFY
legend_marker.pose = odom_msg.pose.pose
legend_marker.pose.position.z = self.legend_text_height # elevate to spread
legend_marker.scale.x = .7
legend_marker.scale.y = .7
legend_marker.scale.z = .7
legend_marker.color.r = self.color['r']
legend_marker.color.g = self.color['g']
legend_marker.color.b = self.color['b']
legend_marker.color.a = .9
self.legend_publisher.publish(legend_marker)
self.cameraFollow_tf(odom_msg)
self.pub_at_position(odom_msg)
def package_odom_var(self, UTMtoPub):
"""
Puts odom var (from UTM N & E) into a ros message and sends to the publishing decider.
-does all the ROS packaging common to all sources
-each individual legend is now
"""
### Odometry Arrows ####################################################
odom_msg = Odometry()
odom_msg.header.stamp = rospy.Time(UTMtoPub['time'])
odom_msg.header.frame_id = "odom" # may need to expand?
odom_msg.pose.pose.position.x = UTMtoPub['E']
odom_msg.pose.pose.position.y = UTMtoPub['N']
odom_msg.pose.pose.position.z = 1.55
# make a quaternion :: came from trial-and-error...
quat = qfe(-pi, -pi, -UTMtoPub['crs']-pi/2)
odom_msg.pose.pose.orientation.x = quat[0]
odom_msg.pose.pose.orientation.y = quat[1]
odom_msg.pose.pose.orientation.z = quat[2]
odom_msg.pose.pose.orientation.w = quat[3]
odom_msg.pose.covariance[0] = UTMtoPub['Nsd']
odom_msg.pose.covariance[7] = UTMtoPub['Esd']
odom_msg.pose.covariance[14] = 0
self.odom_publisher.publish(odom_msg)
### Error Ellipses #####################################################
## pulls from the odom msg --
ell_marker = Marker()
ell_marker.header = odom_msg.header # clarify?
ell_marker.id = 0 # enumerate subsequent markers here
ell_marker.action = Marker.MODIFY # can be ADD, REMOVE, or MODIFY
ell_marker.pose = odom_msg.pose.pose # put at same place as its odom arrow
ell_marker.lifetime = rospy.Duration() # will last forever unless modified
ell_marker.ns = ''.join(["Error_Ellipses", '___', self.sensor_name])
ell_marker.type = Marker.CYLINDER
ell_marker.scale.x = sqrt(odom_msg.pose.covariance[0]) *10 # not visible unless scaled up
ell_marker.scale.y = sqrt(odom_msg.pose.covariance[7]) *10 # not visible unless scaled up
ell_marker.scale.z = 0.000001 # We just want a disk
ell_marker.color.r = self.color['r']
ell_marker.color.g = self.color['g']
ell_marker.color.b = self.color['b']
ell_marker.color.a = 0.95
self.ell_publisher.publish(ell_marker)
### Legend #############################################################
marker = Marker()
marker.header = odom_msg.header
marker.id = 0
marker.ns = self.sensor_name + '_legend'
marker.type = Marker.TEXT_VIEW_FACING
marker.text = self.legend_text
marker.action = Marker.MODIFY
marker.pose = odom_msg.pose.pose
marker.pose.position.z = self.legend_text_height # elevate to spread
marker.scale.x = 0.5
marker.scale.y = 0.5
marker.scale.z = 0.6
marker.color.r = self.color['r']
marker.color.g = self.color['g']
marker.color.b = self.color['b']
marker.color.a = 1.0
self.legend_publisher.publish(marker)
# tell camera tf where the look if master ##############################
if self.ismaster: # update the vehicle mesh position
self.pub_at_position(odom_msg)
self.cameraFollow_tf(odom_msg)
def step_path_following(self):
"""
Steps the path following system, checking if new waypoints
should be sent, if a timeout has occurred, or if path following
needs to be paused.
"""
# Visualize the data
self.visualize_path(self.path, self.path_status)
# Get the next (or current) waypoint
current_waypoint_index = self.get_next_waypoint_index()
# If the index is None, then we are done path planning
if current_waypoint_index == None:
rospy.loginfo("Path Planner: Done.")
return False
if current_waypoint_index == 0:
self.path_status[current_waypoint_index] = 'visited'
# Get the waypoint and status
current_waypoint = self.path[current_waypoint_index]
current_waypoint_status = self.path_status[current_waypoint_index]
# If the status is visited
if current_waypoint_status == 'visited':
# This shouldn't happen...
return True
# If the status is not_visited then we need to push the goal
if current_waypoint_status == 'not_visited':
# Cancel any current goals
self.move_base_client.cancel_all_goals()
# Set it to visiting
self.path_status[current_waypoint_index] = 'visiting'
# Push the goal to the actionlib server
destination = MoveBaseGoal()
destination.target_pose.header.frame_id = self.field_frame_id
destination.target_pose.header.stamp = rospy.Time.now()
# Set the target location
destination.target_pose.pose.position.x = current_waypoint[0]
destination.target_pose.pose.position.y = current_waypoint[1]
# Calculate the distance
if self.previous_destination == None:
self.current_distance = 5.0
else:
self.current_distance = self.distance(self.previous_destination, destination)
# Set the heading
quat = qfe(0, 0, current_waypoint[2])
destination.target_pose.pose.orientation.x = quat[0]
destination.target_pose.pose.orientation.y = quat[1]
destination.target_pose.pose.orientation.z = quat[2]
destination.target_pose.pose.orientation.w = quat[3]
# Send the desired destination to the actionlib server
rospy.loginfo("Sending waypoint (%f, %f)@%f" % tuple(current_waypoint))
self.current_destination = destination
self.move_base_client.send_goal(destination)
self.previous_destination = destination
# If the status is visiting, then we just need to monitor the status
temp_state = self.move_base_client.get_goal_status_text()
if current_waypoint_status == 'visiting':
if temp_state in ['ABORTED', 'SUCCEEDED']:
self.path_status[current_waypoint_index] = 'visited'
else:
duration = rospy.Duration(1.0)
from math import floor
count = 0
while not self.move_base_client.wait_for_result(duration) and count != 6+int(self.current_distance*4):
if rospy.is_shutdown(): return False
count += 1
if count == 6+int(self.current_distance*4):
rospy.logwarn("Path Planner: move_base goal timeout occurred, clearing costmaps")
# Cancel the goals
self.move_base_client.cancel_all_goals()
# Clear the cost maps
self.clear_costmaps()
# Wait 1 second
rospy.Rate(1.0).sleep()
# Then reset the current goal
if not self.just_reset:
self.just_reset = True
self.path_status[current_waypoint_index] = 'not_visited'
else:
self.just_reset = False
self.path_status[current_waypoint_index] = 'visited'
return True
self.path_status[current_waypoint_index] = 'visited'
return True
def pos_set():
global state_id
pub = rospy.Publisher('/mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
rospy.init_node('UAV_setpoint')
rospy.Subscriber('/mavros/state', State, set_id)
rate = rospy.Rate(15)
frame_id = 1
state_id = 1
while not rospy.is_shutdown():
if state_id is 1:
pos = PoseStamped()
pos.header.stamp = rospy.Time.now()
pos.pose.position.x = 0
pos.pose.position.y = 0
pos.pose.position.z = 0
quat = qfe(0, 0, pi / 2)
pos.pose.orientation.w = quat[3]
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pub.publish(pos)
elif 1 < frame_id < 150:
pos = PoseStamped()
pos.header.stamp = rospy.Time.now()
pos.pose.position.x = 1
pos.pose.position.y = 0
pos.pose.position.z = 0.75
quat = qfe(0, 0, pi / 2)
pos.pose.orientation.w = quat[3]
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pub.publish(pos)
elif 150 <= frame_id < 300:
pos = PoseStamped()
pos.header.stamp = rospy.Time.now()
pos.pose.position.x = 1
pos.pose.position.y = 0
pos.pose.position.z = 0.75
quat = qfe(0, 0, pi / 2)
pos.pose.orientation.w = quat[3]
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pub.publish(pos)
else:
pos = PoseStamped()
pos.header.stamp = rospy.Time.now()
pos.pose.position.x = 0
pos.pose.position.y = 0
pos.pose.position.z = 0
quat = qfe(0, 0, pi / 2)
pos.pose.orientation.w = quat[3]
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pub.publish(pos)
if state_id is 2:
frame_id += 1
rate.sleep()
def visualize_path_as_marker(self, path, path_status):
"""
Publishes visualization Markers to represent the planned path.
Publishes the path as a series of spheres connected by lines.
The color of the spheres is set by the path_status parameter,
which is a list of strings of which the possible values are in
['not_visited', 'visiting', 'visited'].
"""
# Get the time
now = rospy.Time.now()
# If self.path_markers is None, initialize it
if self.path_markers == None:
self.path_markers = MarkerArray()
# # If there are existing markers, delete them
# markers_to_delete = MarkerArray()
# if len(self.path_markers.markers) > 0:
# for marker in self.path_markers.markers:
# marker.action = Marker.DELETE
# markers_to_delete.markers.append(marker)
# self.path_marker_pub.publish(markers_to_delete)
# Clear the path_markers
self.path_markers = MarkerArray()
line_strip_points = []
# Create the waypoint markers
for index, waypoint in enumerate(path):
waypoint_marker = Marker()
waypoint_marker.header.stamp = now
waypoint_marker.header.frame_id = self.field_frame_id
waypoint_marker.ns = "waypoints"
waypoint_marker.id = index
waypoint_marker.type = Marker.ARROW
if index == 0:
waypoint_marker.type = Marker.CUBE
waypoint_marker.action = Marker.MODIFY
waypoint_marker.scale.x = 1
waypoint_marker.scale.y = 1
waypoint_marker.scale.z = 0.25
point = Point(waypoint[0], waypoint[1], 0)
waypoint_marker.pose.position = point
# Store the point for the line_strip marker
line_strip_points.append(point)
# Set the heading of the ARROW
quat = qfe(0, 0, waypoint[2])
waypoint_marker.pose.orientation.x = quat[0]
waypoint_marker.pose.orientation.y = quat[1]
waypoint_marker.pose.orientation.z = quat[2]
waypoint_marker.pose.orientation.w = quat[3]
# Color is based on path_status
status = path_status[index]
if status == 'not_visited':
waypoint_marker.color = ColorRGBA(1,0,0,0.5)
elif status == 'visiting':
waypoint_marker.color = ColorRGBA(0,1,0,0.5)
elif status == 'visited':
waypoint_marker.color = ColorRGBA(0,0,1,0.5)
else:
rospy.err("Invalid path status.")
waypoint_marker.color = ColorRGBA(1,1,1,0.5)
# Put this waypoint Marker in the MarkerArray
self.path_markers.markers.append(waypoint_marker)
# Create the line_strip Marker which connects the waypoints
line_strip = Marker()
line_strip.header.stamp = now
line_strip.header.frame_id = self.field_frame_id
line_strip.ns = "lines"
line_strip.id = 0
line_strip.type = Marker.LINE_STRIP
line_strip.action = Marker.ADD
line_strip.scale.x = 0.1
line_strip.color = ColorRGBA(0,0,1,0.5)
line_strip.points = line_strip_points
self.path_markers.markers.append(line_strip)
# Publish the marker array
self.path_marker_pub.publish(self.path_markers)
wz = msg.angular.z
rospy.init_node("Leang")
rospy.Subscriber("cmd_vel", Twist, kk_cb)
rate = rospy.Rate(10)
x = 0
y = 0
lx = 0
ly = 0
th = 0
ltime = rospy.Time.now()
while not rospy.is_shutdown():
now = rospy.Time.now()
#dt = (now-ltime).to_sec()
#x += vx*math.cos(th)*dt
#y += vx*math.sin(th)*dt
#th += wz*dt
x = math.cos(now.to_sec())
y = math.sin(now.to_sec())
br.sendTransform((x, y, 0), qfe(0, 0, 0), now, "robot",
"map") #x,y,z #timberlock #(0,0,0,1) quaternion
lx = x
ly = y
#x += 0.01
#y += 0.01
rate.sleep()
ltime = now
def wp_pub_sub():
global curr_x,curr_y,curr_orient,next_wp,ready_pub,spin,mode,size,rtl,all_waypoints,first_pub
next_wp.pose.position.z = 0.4
all_waypoints = Waypoints()
rospy.init_node('UAV_setpoint',anonymous=True)
rospy.Subscriber('next_wps',Waypoints,setpoints)
rospy.Subscriber('/mavros/state',State,get_curr_mode)
rospy.Subscriber('slam_out_pose',PoseStamped,set_curr)
rospy.Subscriber('/mavros/local_position/pose',PoseStamped,set_curr_z)
rospy.Subscriber('return_to_launch',Bool,get_rtl)
rate = rospy.Rate(15)
wp_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10)
ready_pub = rospy.Publisher('ready_for_wps', Bool, queue_size=10)
rtl_pub = rospy.Publisher('return_to_launch', Bool, queue_size=10)
rtl_path_pub = rospy.Publisher('next_wps', Waypoints, queue_size=10)
first_pub = rospy.Publisher('first_wps', Bool, queue_size=10)
spin_flag = 0
spin = False
spins = 0
offboard_counter = 0
rtl_flag = 0
size = 128
while not rospy.is_shutdown():
if 'mode' in globals() and mode in "OFFBOARD":
offboard_counter += 1
#rospy.loginfo(str(offboard_counter))
if 0 > offboard_counter <= 5:
rospy.loginfo("Just entered OFFBOARD mode")
pos = PoseStamped()
pos.header.stamp = rospy.Time.now()
pos.pose.position.x = next_wp.pose.position.x #these lines could also be changed instead of above
pos.pose.position.y = next_wp.pose.position.y
pos.pose.position.z = next_wp.pose.position.z #this line can be changed to reflect desired z setpoints
#if 'curr_z' in globals():
# rospy.loginfo('spins:%s,curr_z:%s,offboard_counter:%s',str(spins),str(curr_z),str(offboard_counter))
if spins == 0 and 'curr_z' in globals() and curr_z > 0.15 and offboard_counter >= 50:
rospy.loginfo("executing first spin maneuver")
spin = True
first_pub.publish(True)
if spins%5:
spin = False
if spin:
# Yaw is in /slam_out_pose frame.
if spin_flag == 1:
rospy.loginfo("Yaw Left")
yaw = pi/8
elif spin_flag == 0:
rospy.loginfo("Yaw Right")
yaw = -pi/8
else:
rospy.loginfo("Yaw Back")
yaw = 0
quat = qfe(0,0,yaw+pi/2)#euler angles -- RPY roll pitch yaw --> Q xyzw
#add pi/2 to send to setpoint_position/local
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pos.pose.orientation.w = quat[3]
wp_pub.publish(pos)
while not 'curr_orient' in globals():
time.sleep(0.01)
curr_yaw = efq([curr_orient.x,curr_orient.y,curr_orient.z,curr_orient.w])[-1]
#rospy.loginfo('curr_yaw: %s, des_yaw: %s',str(curr_yaw),str(yaw))
epsilon = 0.1
if abs(curr_yaw - yaw) < epsilon:
spin_flag += 1
if yaw == 0:
spin = False
spin_flag = 0
ready_pub.publish(True) #ready for more waypoints. send nav_map to sentel
spins += 1
rate.sleep()
continue # this tells us to skip everything else in the loop and start from the top, (skip next 7 lines)
if 'rtl' in globals() and rtl and not rtl_flag:
rtl_flag = 1
rtl_pub.publish(True)
rtl_wps = Waypoints()
rtl_wps.x = list(reversed(all_waypoints.x))
rtl_wps.y = list(reversed(all_waypoints.y))
rtl_path_pub.publish(rtl_wps)
quat = qfe(0,0,pi/2)
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pos.pose.orientation.w = quat[3]
wp_pub.publish(pos)
rate.sleep()
def publish_sim_path_as_markers(self, sim_path, frame_id):
"""Publishes the given sim apth as a set of ROS markers"""
msg = MarkerArray()
now = rospy.Time.now()
if not sim_path or len(sim_path) < self.last_marker_length:
diff = self.last_marker_length
offset = 0
if sim_path:
offset = len(sim_path)
diff -= offset
for index in range(diff+1):
marker = Marker()
marker.header.frame_id = frame_id
marker.ns = 'sim_path'
marker.id = offset + index - 1
marker.action = Marker.DELETE
if sim_path:
for index, waypoint in enumerate(sim_path):
marker = Marker()
marker.header.frame_id = frame_id
marker.ns = 'sim_path'
marker.id = index
marker.type = Marker.ARROW
marker.action = Marker.MODIFY
marker.pose.position.x = waypoint[1]
marker.pose.position.y = waypoint[2]
quat = qfe(0, 0, waypoint[3])
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.scale.x = 0.25
marker.scale.y = 0.25
marker.scale.z = 0.25
marker.color.r = 0.25
marker.color.g = 0.25
marker.color.b = 0.25
marker.color.a = 0.25
msg.markers.append(marker)
waypoint = sim_path[-1]
marker = Marker()
marker.header.frame_id = frame_id
marker.ns = 'prediction'
marker.id = 0
marker.type = Marker.MESH_RESOURCE
marker.action = Marker.MODIFY
marker.pose.position.x = waypoint[1]
marker.pose.position.y = waypoint[2]
marker.pose.position.z = 0.13
quat = qfe(0, 0, waypoint[3])
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.color.r = 1
marker.color.g = 0
marker.color.b = 0
marker.color.a = 1
marker.mesh_resource = 'package://gavlab_atrv_description/meshes/atrv_chassis.dae'
marker.mesh_use_embedded_materials = True
msg.markers.append(marker)
self.marker_array_pub.publish(msg)
def wp_pub_sub():
global curr_x,curr_y,curr_orient,next_wp,ready_pub,spin,state_id
next_wp.pose.position.z = 0.5
rospy.init_node('UAV_setpoint',log_level=rospy.DEBUG,anonymous=True)
rospy.Subscriber('next_wps',Waypoints,setpoints)
rospy.Subscriber('slam_out_pose',PoseStamped,set_curr)
rate = rospy.Rate(15)
wp_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10)
ready_pub = rospy.Publisher('ready_for_wps', Bool, queue_size=10)
spin_flag = 0
spin = False
while not rospy.is_shutdown():
if state_id is 1:
pos = PoseStamped()
pos.header.stamp = rospy.Time.now()
pos.pose.position.x=0
pos.pose.position.y=0
pos.pose.position.z=0
quat = qfe(0,0,pi/2)
pos.pose.orientation.w = quat[3]
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pub.publish(pos)
elif state_id is 2:
count = +1
if count < 150:
pos = PoseStamped()
pos.header.stamp = rospy.Time.now()
pos.pose.position.x=0
pos.pose.position.y=0
pos.pose.position.z=0
quat = qfe(0,0,pi/2)
pos.pose.orientation.w = quat[3]
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pub.publish(pos)
pos = PoseStamped()
pos.header.stamp = rospy.Time.now()
pos.pose.position.x = -next_wp.pose.position.y #these lines could also be changed instead of above
pos.pose.position.y = next_wp.pose.position.x
pos.pose.position.z = next_wp.pose.position.z #this line can be changed to reflect desired z setpoints
if curr_z > 0.3:
spin = True
if spin:
# Yaw is in /slam_out_pose frame.
if spin_flag == 1:
rospy.loginfo("Yaw Left")
yaw = 3*pi/4
elif spin_flag == 0:
rospy.loginfo("Yaw Right")
yaw = -3*pi/4
else:
rospy.loginfo("Yaw Back")
yaw = 0
quat = qfe(0,0,yaw+pi/2)#euler angles -- RPY roll pitch yaw --> Q xyzw
#add pi/2 to send to setpoint_position/local
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pos.pose.orientation.w = quat[3]
wp_pub.publish(pos)
while not 'curr_orient' in globals():
time.sleep(0.01)
curr_yaw = efq([curr_orient.x,curr_orient.y,curr_orient.z,curr_orient.w])[-1]
rospy.loginfo('curr: %s, des: %s',str(curr_yaw),str(yaw))
epsilon = 0.1
if abs(curr_yaw - yaw) < epsilon:
spin_flag += 1
if yaw == 0:
spin = False
spin_flag = 0
ready_pub.publish(True) #ready for more waypoints. send nav_map to sentel
rate.sleep()
continue # this tells us to skip everything else in the loop and start from the top, (skip next 7 lines)
quat = qfe(0,0,pi/2)
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pos.pose.orientation.w = quat[3]
wp_pub.publish(pos)
rate.sleep()
