def legacy_get_waypoints(data):
"""Extract the waypoints from move_group/display_planned_path.
Legacy function of get_waypoints, this is a way neater implimentation.
Args:
aruco_coords (bool): true if trajectory needed in aruco coords
visualise_trajectory (bool): true if visualisaiton needed
"""
global waypoints, done_waypoints
points_list = data.trajectory[0].multi_dof_joint_trajectory.points
p = Pose()
for transform in points_list:
# create a list of waypoints in pose.Pose after convertion from
# planned trajectory
p.convert_geometry_transform_to_pose(transform.transforms[0])
waypoints.append(list(p.as_waypoints()))
done_waypoints = True
def get_waypoints(data):
"""Extract the waypoints from move_group/display_planned_path.
Very dirty implimentation, if a neater version is needed use
the legacy_get_waypoints function.
Note:
If visualisation is needed aruco_coords should be true.
If visualise_trajectory is true no momement will take place
it will be stuck in a while loop which will exit only by exiting
the module.
Args:
aruco_coords (bool): true if trajectory needed in aruco coords
visualise_trajectory (bool): true if visualisaiton needed
"""
global waypoints, done_waypoints
# filter and extract the information needed.
points_list = data.trajectory[0].multi_dof_joint_trajectory.points
# create pose object
p = Pose()
# if aruco_coords are to be matched
if aruco_coords:
# creating the tf listerner objects
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
# stay in loop until tf found
while True:
try:
# check to see if working with real drone or simulation
if real_drone:
trans = tfBuffer.lookup_transform('world', 'odom',
rospy.Time())
else:
trans = tfBuffer.lookup_transform('world', 'nav',
rospy.Time())
print(trans)
# once tf found get out of here
break
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
print('not yet found')
# stay in loop until tf found
continue
# initialise variables for visualising trajectory
i = 0
br = []
test_trans = []
br1 = []
test_trans1 = []
# iterate over each transform
for transform in points_list:
# check if you want to visualise the trajectory in rviz
if visualise_trajectory:
# append the transforms in list
test_trans.append(
th.multiply_transforms(trans.transform,
transform.transforms[0]))
# create a transform stamped variable to store planned trajectory.
trans_stamped = TransformStamped()
# store values in the variable
trans_stamped.transform.translation.x = transform.transforms[
0].translation.x
trans_stamped.transform.translation.y = transform.transforms[
0].translation.y
trans_stamped.transform.translation.z = transform.transforms[
0].translation.z
trans_stamped.transform.rotation.x = transform.transforms[
0].rotation.x
trans_stamped.transform.rotation.y = transform.transforms[
0].rotation.y
trans_stamped.transform.rotation.z = transform.transforms[
0].rotation.z
trans_stamped.transform.rotation.w = transform.transforms[
0].rotation.w
trans_stamped.header.stamp = rospy.Time.now()
trans_stamped.header.frame_id = 'odom'
trans_stamped.child_frame_id = 'original_' + str(i)
# append transform in list
test_trans1.append(trans_stamped)
# append broadcaster in list
br1.append(tf2_ros.StaticTransformBroadcaster())
# add the frames to the trajectory in aruco's world frame
test_trans[i].header.stamp = rospy.Time.now()
test_trans[i].header.frame_id = 'world'
test_trans[i].child_frame_id = 'test_transform_' + str(i)
test_trans[i].transform.translation.y *= -1
# append broadcaster in list
br.append(tf2_ros.StaticTransformBroadcaster())
# store transform in pose object
p.convert_geometry_transform_to_pose(test_trans[i].transform)
# check if aruco coords are to be added
if aruco_coords:
# make the transform from odom/nav to world.
temp_trans = th.multiply_transforms(trans.transform,
transform.transforms[0])
temp_trans.header.stamp = rospy.Time.now()
temp_trans.header.frame_id = 'world'
temp_trans.child_frame_id = 'test_transform_' + str(i)
# changing the direction of the y coord
temp_trans.transform.translation.y *= -1
p.convert_geometry_transform_to_pose(temp_trans)
i += 1
else:
p.convert_geometry_transform_to_pose(transform.transforms[0])
# store final waypoints in a list
waypoints.append(np.around(p.as_waypoints(), decimals=2))
# set varible to inform that waypoints are ready to be sent for execution
done_waypoints = True
if visualise_trajectory:
# stay in this loop if visualisation in needed.
while True:
for i in range(len(test_trans)):
br[i].sendTransform(test_trans[i])
br1[i].sendTransform(test_trans1[i])
class moveAction(object):
"""Action server class to move drone on waypoints.
This class in the action server used to move the drone on
waypoints generated. It handles the actual movement of the
drone and publishes the feedback.
Args:
name (string): name of the server
real_drone (bool): true if working with real drone not simulation
aruco_mapping (bool): true, using aruco_mapping and not odometry
"""
# create messages that are used to publish feedback/result
_feedback = drone_application.msg.moveFeedback()
_result = drone_application.msg.moveResult()
def __init__(self, name, real_drone, aruco_mapping):
self._action_name = name
# creating a tf listener object
self.tf_listener = tf.TransformListener()
# creating publisher for command velocity to drone
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
# creating an action server object
self._as = actionlib.SimpleActionServer(
self._action_name,
drone_application.msg.moveAction,
execute_cb=self.execute_cb,
auto_start=False)
# starting the action server
self._as.start()
# twist object to stop motion
self.empty_twist = Twist()
# Pose object to hold current global pose
self.camera_pose = Pose()
self.real_drone = real_drone
self.aruco_mapping = aruco_mapping
print('server ready')
print(
'Be careful, once a list of waypoints is given there is no way to stop execution yet.'
)
def moniter_transform(self):
"""Moniter the current pose of the drone based on odometry.
Returns:
numpy.array: containing the x, y, z, yaw values
"""
trans = None
# stay in the loop until transform found
while trans is None:
try:
# checking if working with real drone or simulation
if self.real_drone:
trans, rot = self.tf_listener.lookupTransform(
'/ardrone_base_link', '/odom', rospy.Time(0))
else:
trans, rot = self.tf_listener.lookupTransform(
'nav', 'base_link', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
# not exiting the loop until tf found
continue
euler = tf.transformations.euler_from_quaternion(rot)
return np.array([trans[0], trans[1], trans[2], euler[2]])
def get_camera_pose(self, temp_pose):
"""Callback for global camera pose monitered by aruco_mapping.
Args:
temp_pose (ArucoMaker): topic published by aruco_mapping.
Stores:
camera_pose (pose.Pose): stores global camera pose in Pose.
"""
self.camera_pose.convert_geometry_transform_to_pose(
temp_pose.global_camera_pose, ['z', 'y', 'x', 1])
self.camera_pose.x *= -1
def move_to_waypoint(self, waypoint):
"""Get the drone to actually move to a waypoint.
Takes the waypoint and the current pose of controls pid
It calls pid until the current pose is equal to the waypoint
with a cetatin error tolerence.
Args:
waypoint (numpy.array): contains the waypoint to travel to
"""
# checking if using aruco mapping or odometry for localisation
if self.aruco_mapping:
current_pose = self.camera_pose.as_waypoints()
else:
current_pose = self.moniter_transform()
# dict for PID
state = dict()
# initialisation of various variables for PID
state['lastError'] = np.array([0., 0., 0., 0.])
state['integral'] = np.array([0., 0., 0., 0.])
state['derivative'] = np.array([0., 0., 0., 0.])
# set pid gains
xy_pid = [2, 0.0, 0.]
# xy_pid = [0.2, 0.00, 0.1]
state['p'] = np.array([xy_pid[0], xy_pid[0], 0.3, 1.0], dtype=float)
state['i'] = np.array([xy_pid[1], xy_pid[1], 0.0025, 0.0], dtype=float)
state['d'] = np.array([xy_pid[2], xy_pid[2], 0.15, 0.0], dtype=float)
# to avoid huge jump for the first iteration
state['last_time'] = time.time()
last_twist = np.zeros(4)
marker_not_detected_count = 0
# not controlling yaw since drone was broken
# stay in the loop until staisfactory error range attained
while not np.allclose(current_pose[0:-1], waypoint[0:-1], atol=0.1):
# check if using aruco_mapping or odometry
if self.aruco_mapping:
current_pose = self.camera_pose.as_waypoints()
pid_twist, state = pid(current_pose, waypoint, state)
# if no aruco is being detected
if (current_pose == np.array([0., 0., 0., 0.])).all():
marker_not_detected_count += 1
# if the feed is stuck
if (last_twist == np.array([
pid_twist.linear.x, pid_twist.linear.y,
pid_twist.linear.z, pid_twist.angular.z
])).all():
marker_not_detected_count += 1
# if we are sure it is stuck or no aruco found
if marker_not_detected_count > 2:
self.pub.publish(self.empty_twist)
marker_not_detected_count = 0
print('feed stuck!!!')
else:
self.pub.publish(pid_twist)
# store last twist values to check if feed is stuck or not
last_twist[0] = pid_twist.linear.x
last_twist[1] = pid_twist.linear.y
last_twist[2] = pid_twist.linear.z
last_twist[3] = pid_twist.angular.z
else:
current_pose = self.moniter_transform()
pid_twist, state = pid(current_pose, waypoint, state)
self.pub.publish(pid_twist)
print(current_pose)
# publish the feedback
self._feedback.difference = waypoint - current_pose
self._as.publish_feedback(self._feedback)
def execute_cb(self, goal):
"""Start Excecution of movement.
receives the goal and calls move_to_waypoint.
Args:
goal (MoveAction.goal): the goal to which the drone has to move
"""
# helper variables
success = True
# start executing the action
print('starting exec')
self.move_to_waypoint(np.array(goal.waypoint))
self.pub.publish(self.empty_twist)
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
if success:
self._result.error = [2]
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
