Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

WIP: Planner integration #146

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

WIP: Planner integration #146

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 0 additions & 2 deletions param/motion_command_coordinator.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,6 @@ update_rate : 25

# Startup timeout
startup_timeout: 15.0
# If there is no task for this many seconds a zero velocity command will be sent
task_timeout: 1.0

# Resolution to generate linear motion profiles with
linear_motion_profile_timestep: 0.020
Expand Down
10 changes: 7 additions & 3 deletions param/tasks.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,13 @@
max_translation_speed: 1.0
# maximum z velocity
max_z_velocity: 2.0

max_velocity_time_duration: 30.0
# planner lag, in seconds
planning_lag: .100
# timeout for time to get a plan
planning_timeout: .5
# distance from goal to stop replanning
done_replanning_radius: 1.0

# TAKEOFF TASK
# Delay between arming and taking off
Expand Down Expand Up @@ -36,11 +41,10 @@ translation_xyz_tolerance : 0.3
control_lag : 0.3

# GO TO ROOMBA
go_to_roomba_tolerance: 0.3
go_to_roomba_height: 1.5

# TRACK ROOMBA
# height at which roomba tracking takes place
# also used for GO TO ROOMBA
track_roomba_height: 1.5
# maximum radial distance from roomba
max_roomba_dist: 2.0
Expand Down
92 changes: 48 additions & 44 deletions src/iarc7_motion/iarc_tasks/block_roomba_task.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,13 +5,10 @@
import tf2_ros
import tf2_geometry_msgs
import threading
import numpy as np

from geometry_msgs.msg import TwistStamped
from geometry_msgs.msg import Point
from geometry_msgs.msg import PointStamped
from geometry_msgs.msg import Vector3Stamped
from geometry_msgs.msg import Vector3
from iarc7_msgs.msg import MotionPointStamped, MotionPointStampedArray

from task_utilities.acceleration_limiter import AccelerationLimiter

Expand All @@ -25,12 +22,13 @@
NopCommand)

class BlockRoombaTaskState(object):
init = 0
waiting = 1
descent = 2
INIT = 0
WAITING = 1
PLANNING = 2
DESCENDING = 3
LANDED = 4

class BlockRoombaTask(AbstractTask):

def __init__(self, task_request):
super(BlockRoombaTask, self).__init__()

Expand All @@ -39,67 +37,66 @@ def __init__(self, task_request):
if self._roomba_id == '':
raise ValueError('An invalid roomba id was provided to BlockRoombaTask')

self._distance_to_roomba = None
self._roomba_odometry = None
self._roomba_point = None
self._limiter = AccelerationLimiter()

self._canceled = False
self._last_update_time = None
self._current_velocity = None
self._transition = None

self._target = (0,0)

self._lock = threading.RLock()

try:
self._TRANSFORM_TIMEOUT = rospy.get_param('~transform_timeout')
self._MIN_MANEUVER_HEIGHT = rospy.get_param('~min_maneuver_height')
self._MAX_TRANSLATION_SPEED = rospy.get_param('~max_translation_speed')
self._MAX_START_TASK_DIST = rospy.get_param('~block_roomba_max_start_dist')
self._MAX_Z_VELOCITY = rospy.get_param('~max_z_velocity')
self._K_X = rospy.get_param('~k_term_tracking_x')
self._K_Y = rospy.get_param('~k_term_tracking_y')
self._descent_velocity = rospy.get_param('~block_descent_velocity')
self._DESCENT_VELOCITY = rospy.get_param('~block_descent_velocity')

_roomba_diameter = rospy.get_param('~roomba_diameter')
_drone_width = rospy.get_param('~drone_landing_gear_width')
_drone_width = rospy.get_param('~drone_landing_gear_width')
except KeyError as e:
rospy.logerr('Could not lookup a parameter for block roomba task')
raise
self._overshoot = (_roomba_diameter + _drone_width)/2
self._state = BlockRoombaTaskState.init

# the roomba diameter/2 (radius) is acting as a buffer
self._overshoot = _roomba_diameter + _drone_width/2
self._state = BlockRoombaTaskState.INIT

def get_desired_command(self):
with self._lock:
if self._canceled:
return (TaskCanceled(),)

if (self._state == BlockRoombaTaskState.init):
if self._state == BlockRoombaTaskState.INIT:
if (not self.topic_buffer.has_roomba_message()
or not self.topic_buffer.has_odometry_message()
or not self.topic_buffer.has_landing_message()):
self._state = BlockRoombaTaskState.waiting
self._state = BlockRoombaTaskState.WAITING
else:
self._state = BlockRoombaTaskState.descent
self._state = BlockRoombaTaskState.PLANNING

if (self._state == BlockRoombaTaskState.waiting):
if self._state == BlockRoombaTaskState.WAITING:
if (not self.topic_buffer.has_roomba_message()
or not self.topic_buffer.has_odometry_message()
or not self.topic_buffer.has_landing_message()):
return (TaskRunning(), NopCommand())
else:
self._state = BlockRoombaTaskState.descent
self._state = BlockRoombaTaskState.PLANNING

if (self._state == BlockRoombaTaskState.descent):
if self._state == BlockRoombaTaskState.PLANNING:
try:
roomba_transform = self.topic_buffer.get_tf_buffer().lookup_transform(
'level_quad',
self._roomba_id,
rospy.Time(0),
rospy.Duration(self._TRANSFORM_TIMEOUT))
drone_transform = self.topic_buffer.get_tf_buffer().lookup_transform(
'level_quad',
'map',
rospy.Time(0),
rospy.Duration(self._TRANSFORM_TIMEOUT))
except (tf2_ros.LookupException,
tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as ex:
Expand All @@ -126,33 +123,40 @@ def get_desired_command(self):
roomba_y_velocity = self._roomba_odometry.twist.twist.linear.y
roomba_velocity = math.sqrt(roomba_x_velocity**2 + roomba_y_velocity**2)

roomba_vector = Vector3Stamped()
drone_height = self.topic_buffer.get_odometry_message().pose.pose.position.z
time_to_descend = drone_height/self._DESCENT_VELOCITY

roomba_vector_x = roomba_x_velocity/roomba_velocity
roomba_vector_y = roomba_y_velocity/roomba_velocity

roomba_vector.vector.x = roomba_x_velocity/roomba_velocity
roomba_vector.vector.y = roomba_y_velocity/roomba_velocity
roomba_vector.vector.z = 0.0
goal_x = (self._roomba_point.point.x + self._overshoot * roomba_vector_x)/time_to_descend
goal_y = (self._roomba_point.point.y + self._overshoot * roomba_vector_y)/time_to_descend

# p-controller
x_vel_target = ((self._roomba_point.point.x + self._overshoot * roomba_vector.vector.x)
* self._K_X + roomba_x_velocity)
y_vel_target = ((self._roomba_point.point.y + self._overshoot * roomba_vector.vector.y)
* self._K_Y + roomba_y_velocity)

z_vel_target = self._descent_velocity
# we need a better way to plan
self._target = (goal_x, goal_y)

#caps velocity
self._state = BlockRoombaTaskState.DESCENDING

if self._state == BlockRoombaTaskState.DESCENDING:

x_vel_target = self._target[0]
y_vel_target = self._target[1]

z_vel_target = self._DESCENT_VELOCITY

# cap velocity
vel_target = math.sqrt(x_vel_target**2 + y_vel_target**2)

if vel_target > self._MAX_TRANSLATION_SPEED:
x_vel_target = x_vel_target * (self._MAX_TRANSLATION_SPEED/vel_target)
y_vel_target = y_vel_target * (self._MAX_TRANSLATION_SPEED/vel_target)

if (abs(z_vel_target) > self._MAX_Z_VELOCITY):
z_vel_target = z_vel_target/abs(z_vel_target) * self._MAX_Z_VELOCITY
rospy.logwarn("Max Z velocity reached in block roomba")

desired_vel = [x_vel_target, y_vel_target, z_vel_target]

odometry = self.topic_buffer.get_odometry_message()
drone_vel_x = odometry.twist.twist.linear.x
drone_vel_y = odometry.twist.twist.linear.y
Expand All @@ -171,7 +175,7 @@ def get_desired_command(self):
velocity.twist.linear.z = desired_vel[2]

self._current_velocity = desired_vel

return (TaskRunning(), VelocityCommand(velocity))

return (TaskAborted(msg='Illegal state reached in Block Roomba Task' ),)
Expand All @@ -183,14 +187,14 @@ def _check_max_roomba_range(self):
for odometry in self.topic_buffer.get_roomba_message().data:
if odometry.child_frame_id == self._roomba_id:
self._roomba_odometry = odometry
self._roomba_found = True
_distance_to_roomba = math.sqrt(self._roomba_point.point.x**2 +
self._roomba_found = True
self._distance_to_roomba = math.sqrt(self._roomba_point.point.x**2 +
self._roomba_point.point.y**2)
return (_distance_to_roomba <= (self._MAX_START_TASK_DIST + self._overshoot))
return (self._distance_to_roomba <= (self._MAX_START_TASK_DIST + self._overshoot))
return False

def cancel(self):
rospy.loginfo('HitRoomba Task canceled')
rospy.loginfo('BlockRoombaTask canceled')
self._canceled = True
return True

Expand Down
Loading