def move(self, position_constraint_string, frame):
p = PositionConstraint()
p.constraint = position_constraint_string
p.frame = frame
plan = self.global_planner.getPlan(p)
if plan:
o = OrientationConstraint()
o.frame = frame
self.local_planner.setPlan(plan, p, o)
return plan
def turn_towards(self, x, y, frame, offset=0.0):
current_pose = self.get_location()
p = PositionConstraint()
p.constraint = "(x-%f)^2+(y-%f)^2 < %f^2" % (
current_pose.frame.p.x(), current_pose.frame.p.y(), 0.1)
p.frame = current_pose.frame_id
plan = self.global_planner.getPlan(p)
o = OrientationConstraint(look_at=geometry_msgs.msg.Point(x, y, 0.0),
angle_offset=offset)
o.frame = frame
self.local_planner.setPlan(plan, p, o)
return plan
def waypoint_constraint(waypoint_designator, radius, look=True):
"""
Navigates to a radius from a waypoint
:param waypoint_designator: designator resolving to the waypoint
:param radius: allowed distance to the waypoint
:param look: whether or not to take the orientation of the waypoint (default True)
:return: navigation constraints, if the entity does not resolve, None is returned.
:rtype: tuple(PositionConstraint, OrientationConstraint)
"""
e = waypoint_designator.resolve()
if not e:
rospy.logerr(
"waypoint_constraint function: No entity could be resolved from designator '%s'" % waypoint_designator)
return None
rospy.logdebug("Navigating to waypoint '{}'".format(e.id))
try:
x = e.pose.frame.p.x()
y = e.pose.frame.p.y()
rz, _, _ = e.pose.frame.M.GetEulerZYX()
except Exception as e:
rospy.logerr(e)
return None
pc = PositionConstraint(constraint="(x-%f)^2+(y-%f)^2 < %f^2" % (x, y, radius), frame="/map")
oc = None
if look:
oc = OrientationConstraint(look_at=Point(x + 10, y, 0.0), angle_offset=rz, frame="/map")
return pc, oc
def generateConstraint(self):
arm = self.arm.resolve()
if arm == self.robot.arms['left']:
angle_offset = math.atan2(-self.robot.grasp_offset.y,
self.robot.grasp_offset.x)
elif arm == self.robot.arms['right']:
angle_offset = math.atan2(self.robot.grasp_offset.y,
self.robot.grasp_offset.x)
else:
raise IndexError('Arm not found')
radius = math.hypot(self.robot.grasp_offset.x,
self.robot.grasp_offset.y)
x = self.point.point.x
y = self.point.point.y
frame_id = self.point.header.frame_id
# Outer radius
radius -= 0.1
ro = "(x-%f)^2+(y-%f)^2 < %f^2" % (x, y, radius + 0.075)
ri = "(x-%f)^2+(y-%f)^2 > %f^2" % (x, y, radius - 0.075)
pc = PositionConstraint(constraint=ri + " and " + ro, frame=frame_id)
oc = OrientationConstraint(look_at=Point(x, y, 0.0),
frame=frame_id,
angle_offset=angle_offset)
return pc, oc
def execute(self, userdata=None):
door_1_position = self._robot.ed.get_entity(
id=challenge_knowledge.reentry_door_1).pose.position
door_2_position = self._robot.ed.get_entity(
id=challenge_knowledge.reentry_door_2).pose.position
door_1_constraint = PositionConstraint()
door_1_constraint.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2" % (
door_1_position.x, door_1_position.y, 0.7)
door_2_constraint = PositionConstraint()
door_2_constraint.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2" % (
door_2_position.x, door_2_position.y, 0.7)
# TODO: make sure that waypoints exist!!!
# TODO: make sure door id designator is writeable
while True:
plan1 = self._robot.base.global_planner.getPlan(door_1_constraint)
plan2 = self._robot.base.global_planner.getPlan(door_2_constraint)
print "Plan 1 length: %i" % len(plan1)
print "Plan 2 length: %i" % len(plan2)
if len(plan1) < 3:
self._door_id_designator.writeable.write(
challenge_knowledge.target_door_1)
return "door_found"
elif len(plan2) < 3:
self._door_id_designator.writeable.write(
challenge_knowledge.target_door_2)
return "door_found"
if self.preempt_requested():
return 'preempted'
time.sleep(1)
def get_position_constraint(self, entity_id_area_name_map):
try:
res = self._get_constraint_srv(entity_ids=[k for k in entity_id_area_name_map],
area_names=[v for k, v in entity_id_area_name_map.iteritems()])
except Exception as e:
rospy.logerr("Can't get position constraint: {}".format(e))
return None
if res.error_msg != '':
rospy.logerr(res.error_msg)
return None
return PositionConstraint(constraint=res.position_constraint_map_frame, frame="/map")
def __init__(self, robot, entity_id, obstacle_radius):
smach.State.__init__(self, outcomes=["done", "timeout"])
self._robot = robot
try:
pose = robot.ed.get_entity(id=entity_id).pose
except Exception as e:
rospy.logerr(e)
sys.exit(1)
self.pc = PositionConstraint(frame="/map",
constraint="(x-%f)^2+(y-%f)^2 < 0.05" %
(pose.position.x, pose.position.y))
self.obstacle_radius = obstacle_radius
def _turn_towards_operator(self):
f = self._robot.base.get_location().frame
robot_position = f.p
operator_position = self._last_operator._pose.p
p = PositionConstraint()
p.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2" % (operator_position.x(),
operator_position.y(),
self._operator_radius)
o = OrientationConstraint()
if self._operator_id:
o.frame = self._operator_id
else:
o.frame = 'map'
o.look_at = kdl_conversions.kdl_vector_to_point_msg(
self._last_operator.pose.frame.p)
dx = operator_position.x() - robot_position.x()
dy = operator_position.y() - robot_position.y()
yaw = math.atan2(dy, dx)
# ToDo: make nice!
pose = geometry_msgs.msg.PoseStamped()
pose.header.frame_id = "/map"
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = robot_position.x()
pose.pose.position.y = robot_position.y()
xx, yy, zz, ww = kdl.Rotation.RPY(0, 0, yaw).GetQuaternion()
pose.pose.orientation.x = xx
pose.pose.orientation.y = yy
pose.pose.orientation.z = zz
pose.pose.orientation.w = ww
plan = [pose]
rospy.loginfo("Operator within self._lookat_radius")
self._robot.base.local_planner.setPlan(plan, p, o)
def pose_constraints(x, y, rz=None, radius=0.15, frame_id="/map"):
"""
Generate navigation constraints for a radius from a pose
:param x, y: x and y coordinates
:param rz: orientation to assume. None=no orientation constraint (default None)
:param radius: allowed distance to the pose
:param frame_id: frame in which the pose is expressed
:return: navigation constraints
:rtype: tuple(PositionConstraint, OrientationConstraint)
"""
pc = PositionConstraint(constraint="(x-%f)^2+(y-%f)^2 < %f^2" % (x, y, radius),
frame=frame_id)
oc = None
if rz:
oc = OrientationConstraint(look_at=Point(x + 1, y, 0.0), angle_offset=rz, frame=frame_id)
return pc, oc
def arms_reach_constraint(pose_designator, arm_designator, look=True):
"""
Position so that the arm can reach the position/entity
:param pose_designator: designator that resolves to a FrameStamped of the point to be reached
:param arm_designator: PublicArmDesignator, arm to use for manipulation
:param look: bool, whether or not the orientation must be constrained as well
:return: navigation constraints, if a designator does not resolve None is returned
:rtype: tuple(PositionConstraint, OrientationConstraint)
"""
arm = arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm, Designator {} did not resolve".format(arm_designator))
return None
radius = math.hypot(arm.base_offset.x(), arm.base_offset.y())
pose = pose_designator.resolve()
if not pose:
rospy.logerr("No such place_pose, Designator {} did not resolve".format(pose_designator))
return None
try:
x = pose.frame.p.x()
y = pose.frame.p.y()
except KeyError as ke:
rospy.logerr("Could not determine pose: ".format(ke))
return None
# Outer radius
ro = "(x-%f)^2+(y-%f)^2 < %f^2" % (x, y, radius + 0.075)
ri = "(x-%f)^2+(y-%f)^2 > %f^2" % (x, y, radius - 0.075)
pc = PositionConstraint(constraint=ri + " and " + ro, frame="/map")
oc = None
if look:
angle_offset = -math.atan2(arm.base_offset.y(), arm.base_offset.x())
oc = OrientationConstraint(look_at=Point(x, y, 0.0), frame="/map", angle_offset=angle_offset)
return pc, oc
def _distance_to_poi_area(self, frame_stamped, arm):
"""
:return: length of the path the robot would need to drive to place at the given point
:rtype: int [plan steps]
"""
base_offset = arm.base_offset
radius = math.hypot(base_offset.x(), base_offset.y())
x = frame_stamped.frame.p.x()
y = frame_stamped.frame.p.y()
radius -= 0.1
ro = "(x-%f)^2+(y-%f)^2 < %f^2" % (x, y, radius + 0.075)
ri = "(x-%f)^2+(y-%f)^2 > %f^2" % (x, y, radius - 0.075)
pos_constraint = PositionConstraint(constraint=ri + " and " + ro, frame=frame_stamped.frame_id)
plan_to_poi = self.robot.base.global_planner.getPlan(pos_constraint)
if plan_to_poi:
distance = len(plan_to_poi)
# print "Distance to {fs}: {dist}".format(dist=distance, fs=frame_stamped.frame.p)
else:
distance = None
return distance
def execute(self, userdata=None):
e = self.destination_designator.resolve()
if not e:
rospy.logerr(
"CheckDoorPassable::execute: No entity could be resolved from designator '%s'"
% self.destination_designator)
return None
try:
x = e.pose.position.x
y = e.pose.position.y
except:
return None
pc = PositionConstraint(constraint="(x-%f)^2+(y-%f)^2 < %f^2" %
(x, y, 0.5),
frame="/map")
plan = self.robot.base.global_planner.getPlan(pc)
if plan and len(plan) < 3 and computePathLength(plan) < 3.0:
return "passable"
else:
return "blocked"
def _replan(self):
self._replan_attempts += 1
rospy.loginfo("Trying to get a global plan")
operator_position = self._last_operator._pose.p
# Define end goal constraint, solely based on the (old) operator position
self._replan_pc = PositionConstraint()
self._replan_pc.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2" % (
operator_position.x(), operator_position.y(),
self._operator_radius)
ros_plan = self._robot.base.global_planner.getPlan(self._replan_pc)
if not ros_plan or not self._robot.base.global_planner.checkPlan(
ros_plan):
rospy.loginfo("No global plan possible")
else:
self._robot.speech.speak("Just a sec, let me try this way.")
rospy.loginfo("Found a global plan, sending it to local planner")
self._replan_time = rospy.Time.now()
self._replan_active = True
oc = self._robot.base.local_planner.getCurrentOrientationConstraint(
)
self._visualize_plan(ros_plan)
self._robot.base.local_planner.setPlan(ros_plan, self._replan_pc,
oc)
self._breadcrumbs = []
def dummy_constraint(name="test"):
pc = PositionConstraint(constraint="constraint_"+name, frame="/map")
oc = OrientationConstraint(look_at=Point(0, 0, 0), frame="/map")
return pc, oc
def radius_constraint(entity, radius, margin):
"""
Navigates to a radius from an ed entity. If a convex hull is present, the distance
to the convex hull is used. Otherwise, the distance to the pose of the entity is used
:param entity: EdEntityDesignator for the object to observe
:param radius: (float) desired distance to the pose of the entity [m]
:param margin: (float) allowed margin w.r.t. specified radius on both sides [m]
:return: navigation constraints. If the entity does not resolve, None is returned.
:rtype: tuple(PositionConstraint, OrientationConstraint)
"""
e = entity.resolve()
if not e:
rospy.logerr("No entity from {}".format(entity))
return None
try:
ch = e.shape.convex_hull
except NotImplementedError:
# In case of an entity without convex hull, we might not want to use this
ch = []
x = e.pose.frame.p.x()
y = e.pose.frame.p.y()
assert e.pose.frame_id.strip(
"/"
) == "map", "radius constraint function assumes entities are defined w.r.t. map frame"
if len(
ch
) > 0: # If a convex hull is present, use this to create the position constraint
pci = "" # Create an empty position constraint
for i in xrange(len(ch)): # Iterate over the convex hull
j = (i + 1) % len(ch)
dx = ch[j].x() - ch[i].x()
dy = ch[j].y() - ch[i].y()
length = math.hypot(dx, dy)
xs = x + ch[i].x() + (dy / length) * radius
ys = y + ch[i].y() - (dx / length) * radius
if i != 0:
pci = pci + ' and '
pci = pci + "-(x-%f)*%f+(y-%f)*%f > 0.0 " % (xs, dy, ys, dx)
else: # If not, simply use the x and y position
outer_radius = max(0., radius + margin)
inner_radius = max(0., radius - margin)
ro = "(x-%f)^2+(y-%f)^2 < %f^2" % (x, y, outer_radius)
ri = "(x-%f)^2+(y-%f)^2 > %f^2" % (x, y, inner_radius)
pci = ri + " and " + ro
pc = PositionConstraint(
constraint=pci,
frame="/map") # Create the position constraint from the string
oc = None
return pc, oc
def execute(self, userdata=None):
""" A concurrent state machine which follows and afterwards deletes breadcrumbs from the buffer variable
:return: follow_bread if the list of breadcrumbs is not empty
no_follow_bread_ask_finalize if the single remaining breadcrumb is the operator
no_follow_bread_recovery if buffer is completely empty
"""
while self._buffer:
if self.preempt_requested():
return 'aborted'
crumb = self._buffer.popleft()
if not self._breadcrumb or self._breadcrumb[-1].crumb.distance_to_2d(
crumb._pose.p) > self._breadcrumb_distance:
self._breadcrumb.append(CrumbWaypoint(crumb))
rospy.loginfo("Self._breadcrumb length: {}".format(len(self._breadcrumb)))
# We don't want to base this on distance since that relies on the operator behavior
# if self._robot.base.local_planner.getDistanceToGoal() > 0.65: # 2.0: # len(buffer) > 5:
# self._have_followed = True
robot_position = self._robot.base.get_location().frame
if self._breadcrumb:
self._operator = self._breadcrumb[-1].crumb
else:
return 'no_follow_bread_recovery'
#
# if self._operator:
# operator = self._robot.ed.get_entity(id=self._operator.id)
# while True: # Should be timer, I think
try:
_, semantics = self._robot.hmi.query(description="", grammar="T[true] -> %s stop" % self._robot.robot_name, target="T")
if semantics:
return 'no_follow_bread_ask_finalize'
except hmi.TimeoutException:
rospy.logwarn("[HMC] Listening timeout")
# return 'no_follow_bread_ask_finalize'
# if operator:
# rospy.loginfo("Distance to operator: {}".format(operator.distance_to_2d(robot_position.p)))
# if self._have_followed and operator.distance_to_2d(robot_position.p) < 1.0:
# self._have_followed = False
# return 'no_follow_bread_ask_finalize'
# self._breadcrumb = [crwp for crwp in self._breadcrumb
# if crwp.crumb.distance_to_2d(robot_position.p) > self._lookat_radius]
current_index = -1
for i, crumb in enumerate(self._breadcrumb):
if crumb.crumb.distance_to_2d(robot_position.p) < self._lookat_radius:
current_index = i
# throw away all breadcrumbs before where we are
self._breadcrumb = self._breadcrumb[current_index+1:]
f = self._robot.base.get_location().frame
previous_point = f.p
operator_position = self._operator._pose.p
''' Define end goal constraint, solely based on the (old) operator position '''
p = PositionConstraint()
p.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2" % (operator_position.x(), operator_position.y(),
self._operator_radius)
o = OrientationConstraint()
o.frame = self._operator.id
''' Calculate global plan from robot position, through breadcrumbs, to the operator '''
for crumb_waypoint in self._breadcrumb:
crumb = crumb_waypoint.crumb
diff = crumb._pose.p - previous_point
dx, dy = diff.x(), diff.y()
length = crumb.distance_to_2d(previous_point)
if length != 0:
dx_norm = dx / length
dy_norm = dy / length
yaw = math.atan2(dy, dx)
start = 0
end = int(length / self._resolution)
for i in range(start, end):
x = previous_point.x() + i * dx_norm * self._resolution
y = previous_point.y() + i * dy_norm * self._resolution
kdl_pose = kdl_conversions.kdl_frame_stamped_from_XYZRPY(x=x, y=y, z=0, yaw=yaw)
crumb_waypoint.waypoint = kdl_conversions.kdl_frame_stamped_to_pose_stamped_msg(kdl_pose)
previous_point = copy.deepcopy(crumb._pose.p)
if len(self._breadcrumb) > 0:
ros_plan = [crwp.waypoint for crwp in self._breadcrumb]
if not self._robot.base.global_planner.checkPlan(ros_plan):
rospy.loginfo("Breadcrumb plan is blocked, removing blocked points")
rospy.loginfo("Breadcrumbs before removing blocked points: {}".format(len(ros_plan)))
ros_plan = [crwp.waypoint for crwp in self._breadcrumb
if self._robot.base.global_planner.checkPlan([crwp.waypoint])]
rospy.loginfo("Breadcrumbs length after removing blocked points: {}".format(len(ros_plan)))
# if the new ros plan is empty, we do have an operator but there are no breadcrumbs that we can reach
# if not ros_plan:
# return 'no_follow_bread_recovery'
else:
return 'no_follow_bread_recovery'
buffer_msg = Marker()
buffer_msg.type = Marker.POINTS
buffer_msg.header.stamp = rospy.get_rostime()
buffer_msg.header.frame_id = "/map"
buffer_msg.id = 0
buffer_msg.action = Marker.DELETEALL
self._breadcrumb_pub.publish(buffer_msg)
buffer_msg = Marker()
buffer_msg.type = Marker.POINTS
buffer_msg.scale.x = 0.05
buffer_msg.scale.y = 0.05
buffer_msg.header.stamp = rospy.get_rostime()
buffer_msg.header.frame_id = "/map"
buffer_msg.color.a = 1
buffer_msg.color.r = 1
buffer_msg.color.g = 0
buffer_msg.color.b = 0
# buffer_msg.lifetime = rospy.Time(1)
buffer_msg.id = 0
buffer_msg.action = Marker.ADD
for crumb in self._breadcrumb:
buffer_msg.points.append(crumb.waypoint.pose.position)
self._breadcrumb_pub.publish(buffer_msg)
# line_strip = Marker()
# line_strip.type = Marker.LINE_STRIP
# line_strip.scale.x = 0.05
# line_strip.header.frame_id = "/map"
# line_strip.header.stamp = rospy.Time.now()
# line_strip.color.a = 1
# line_strip.color.r = 0
# line_strip.color.g = 1
# line_strip.color.b = 1
# line_strip.id = 0
# line_strip.action = Marker.ADD
#
# for crwp in ros_plan:
# line_strip.points.append(crwp)
#self._plan_marker_pub.publish(line_strip)
self._robot.base.local_planner.setPlan(ros_plan, p, o)
rospy.sleep(rospy.Duration(0.5))
return 'follow_bread'
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
self.robot = robot
self.position_constraint = PositionConstraint()
self.orientation_constraint = OrientationConstraint()
self.position_constraint.constraint = "x^2+y^2 < 1"
self.requested_location = None
rospy.Subscriber("/location_request", std_msgs.msg.String,
self.requestedLocationcallback)
self.random_nav_designator = RandomNavDesignator(self.robot)
with self:
smach.StateMachine.add("WAIT_A_SEC",
states.WaitTime(robot, waittime=1.0),
transitions={
'waited': "SELECT_ACTION",
'preempted': "Aborted"
})
smach.StateMachine.add("SELECT_ACTION",
SelectAction(),
transitions={
'continue': "DETERMINE_TARGET",
'pause': "SELECT_ACTION",
'stop': "SAY_DONE"
})
@smach.cb_interface(
outcomes=['target_determined', 'no_targets_available'],
input_keys=[],
output_keys=[])
def determine_target(userdata, designator):
entity_id = designator.getRandomGoal()
sentences = [
"Lets go look at the %s", "Lets have a look at the %s",
"Lets go to the %s", "Lets move to the %s",
"I will go to the %s", "I will now move to the %s",
"I will now drive to the %s", "I will look the %s",
"The %s will be my next location", "The %s it is",
"New goal, the %s", "Going to look at the %s",
"Moving to the %s", "Driving to the %s", "On to the %s",
"On the move to the %s", "Going to the %s"
]
robot.speech.speak(random.choice(sentences) % entity_id,
block=False)
return 'target_determined'
smach.StateMachine.add('DETERMINE_TARGET',
smach.CBState(determine_target,
cb_kwargs={
'designator':
self.random_nav_designator
}),
transitions={
'target_determined': 'DRIVE',
'no_targets_available': 'SELECT_ACTION'
})
smach.StateMachine.add('DRIVE',
states.NavigateToObserve(
robot, self.random_nav_designator),
transitions={
"arrived": "SAY_SUCCEEDED",
"unreachable": 'SAY_UNREACHABLE',
"goal_not_defined": 'SELECT_ACTION'
})
smach.StateMachine.add("SAY_SUCCEEDED",
states.Say(robot, [
"I am here",
"Goal succeeded",
"Another goal succeeded",
"Goal reached",
"Another goal reached",
"Target reached",
"Another target reached",
"Destination reached",
"Another destination reached",
"I have arrived",
"I have arrived at my goal",
"I have arrived at my target",
"I have arrived at my destination",
"I am at my goal",
"I am at my target",
"I am at my destination",
"Here I am",
]),
transitions={'spoken': 'SELECT_ACTION'})
smach.StateMachine.add(
"SAY_UNREACHABLE",
states.Say(robot, [
"I can't find a way to my goal, better try something else",
"This goal is unreachable, I better find somewhere else to go",
"I am having a hard time getting there so I will look for a new target"
]),
transitions={'spoken': 'SELECT_ACTION'})
smach.StateMachine.add(
"SAY_DONE",
states.Say(robot, [
"That's all folks", "I'll stay here for a while", "Goodbye"
]),
transitions={'spoken': 'Done'})
def _update_navigation(self):
"""Set the navigation plan to match the breadcrumbs collected into self._breadcrumbs.
This list has all the Entity's of where the operator has been"""
self._robot.head.cancel_goal()
f = self._robot.base.get_location().frame
robot_position = f.p
operator_position = self._last_operator._pose.p
''' Define end goal constraint, solely based on the (old) operator position '''
p = PositionConstraint()
p.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2" % (operator_position.x(),
operator_position.y(),
self._operator_radius)
o = OrientationConstraint()
if self._operator_id:
o.frame = self._operator_id
else:
o.frame = 'map'
o.look_at = kdl_conversions.kdl_vector_to_point_msg(
self._last_operator.pose.frame.p)
''' Calculate global plan from robot position, through breadcrumbs, to the operator '''
res = 0.05
kdl_plan = []
previous_point = robot_position
if self._operator:
breadcrumbs = self._breadcrumbs + [self._operator]
else:
breadcrumbs = self._breadcrumbs + [self._last_operator]
for crumb in breadcrumbs:
assert isinstance(crumb, Entity)
diff = crumb._pose.p - previous_point
dx, dy = diff.x(), diff.y()
length = crumb.distance_to_2d(previous_point)
if length != 0:
dx_norm = dx / length
dy_norm = dy / length
yaw = math.atan2(dy, dx)
start = 0
end = int(length / res)
for i in range(start, end):
x = previous_point.x() + i * dx_norm * res
y = previous_point.y() + i * dy_norm * res
kdl_plan.append(
kdl_conversions.kdl_frame_stamped_from_XYZRPY(x=x,
y=y,
z=0,
yaw=yaw))
previous_point = copy.deepcopy(crumb._pose.p)
# Delete the elements from the plan within the operator radius from the robot
cutoff = int(self._operator_radius / (2.0 * res))
if len(kdl_plan) > cutoff:
del kdl_plan[-cutoff:]
ros_plan = frame_stampeds_to_pose_stampeds(kdl_plan)
# Check if plan is valid. If not, remove invalid points from the path
if not self._robot.base.global_planner.checkPlan(ros_plan):
rospy.loginfo(
"Breadcrumb plan is blocked, removing blocked points")
# Go through plan from operator to robot and pick the first unoccupied point as goal point
ros_plan = [
point for point in ros_plan
if self._robot.base.global_planner.checkPlan([point])
]
self._visualize_plan(ros_plan)
self._robot.base.local_planner.setPlan(ros_plan, p, o)