def execute(self, ud):
start_time = rospy.get_time()
rate = rospy.Rate(2.5)
while not self.preempt_requested() and not rospy.is_shutdown():
segmented_objs = self.segment_srv(
only_surface=True).segmented_objects.objects
if segmented_objs:
table = segmented_objs[0]
pose = geo_msgs.PoseStamped(
table.point_cloud.header,
geo_msgs.Pose(table.center, table.orientation))
pose = TF2().transform_pose(pose, pose.header.frame_id, 'map')
width, length = table.width, table.depth
table_tf = to_transform(pose, 'table_frame')
TF2().publish_transform(table_tf)
if 'table' in ud:
table.name = ud['table'].name
ud['table'] = table
ud['table_pose'] = pose
rospy.loginfo("Detected table of size %.1f x %.1f at %s",
width, length, pose2d2str(pose))
return 'succeeded'
elif self.timeout and rospy.get_time() - start_time > self.timeout:
rospy.logwarn("No table detected after %g seconds",
rospy.get_time() - start_time)
return 'aborted'
try:
rate.sleep()
except rospy.ROSInterruptException:
break
rospy.logwarn(
"Detect table has been preempted after %g seconds (timeout %g)",
rospy.get_time() - start_time, self.timeout)
return 'preempted'
def nav_goal_cb(msg):
rospy.loginfo("Calling MBF's move_base action with target pose %s",
pose2d2str(msg))
goal = MoveBaseGoal(target_pose=msg)
move_base_ac.send_goal(goal, done_cb=move_done_cb)
# rospy.sleep(0.000005)#random.randrange(300, 800)/100.0) hostias!!! tarda bastante, aunque pongas epsilon!!! (en sim, supongo)
time.sleep(2.5)
move_base_ac.cancel_goal()
rospy.loginfo("CANCEL!!!")
def nav_goal_cb(msg):
global target_pose
target_pose = msg
goal = GetPathGoal(use_start_pose=False,
start_pose=msg,
target_pose=msg,
planner="planner2")
get_path_ac.send_goal(goal, done_cb=plan_done_cb)
rospy.loginfo(
"Relaying move_base_simple/goal %s from RViz to MBF's planner",
pose2d2str(msg))
def nav_goal_cb(msg):
global target_pose
target_pose = msg
goal = GetPathGoal(use_start_pose=False, start_pose=msg, target_pose=msg)
get_path_ac.send_goal(goal, done_cb=plan_done_cb)
rospy.loginfo(
"Relaying move_base_simple/goal %s from RViz to MBF's planner",
pose2d2str(msg))
rospy.sleep(1)
goal = MoveBaseGoal(target_pose=msg, controller='TEBPlanner')
move_base_ac.send_goal(goal, done_cb=move_done_cb)
def execute(self, ud):
# Cut path up to current waypoint, so we don't redo it from the beginning after recovering
# ExePath must provide this specific message at the end of the result message in case of failure
cwp_index = ud['message'].find('current waypoint: ')
if cwp_index >= 0:
# TODO I must adapt this to integrate the progress tracker with the smoothed path logic (or remove entirely path follower support) and remove 'path' key
next_waypoint = int(ud['message'][cwp_index +
len('current waypoint: '):])
ud['path'].poses = ud['path'].poses[next_waypoint:]
elif ud['reached_wp'] is not None:
# TEB logic using progress tracker: very brittle, but seems to work! hope I can do better w/ BTs
next_waypoint = ud['reached_wp'] + 1
ud['waypoints'] = ud['waypoints'][next_waypoint:]
else:
# None on 'reached_wp' normally means a failure after a recovery attempt
rospy.logwarn(
"No current waypoint provided; assuming we didn't reach even the first waypoint"
)
try:
rb = self.recovery_behaviors[self.consecutive_failures]
ud['recovery_behavior'] = rb
self.consecutive_failures += 1
rospy.loginfo(
"Attempt recovery behavior '%s' after %d %s", rb,
self.consecutive_failures, "consecutive failures"
if self.consecutive_failures > 1 else "failure")
return 'recover'
except IndexError:
rospy.loginfo(
"Recovery behaviors exhausted after %d consecutive failures",
self.consecutive_failures)
self.consecutive_failures = 0
if ud['waypoints']:
next_wp_pose = ud['waypoints'][0]
rospy.loginfo("Navigate to the next waypoint at %s",
pose2d2str(next_wp_pose))
ud['next_wp'] = next_wp_pose
return 'next_wp'
return 'aborted'
def spawn_cats(use_preferred_locs=False):
added_poses = [] # to check that cats are at least CATS_MIN_DIST apart from each other
for cat in cats:
cat_index = 0
while cat_index < cat['count'] and not rospy.is_shutdown():
if use_preferred_locs:
x, y = random.choice(PREFERRED_LOCATIONS)
else:
x = random.uniform(min_x, max_x)
y = random.uniform(min_y, max_y)
theta = random.uniform(-pi, +pi)
pose = create_2d_pose(x, y, theta)
# we check that the distance to all previously added surfaces is below a threshold to space the surfaces
if close_to_prev_pose(pose, added_poses, CATS_MIN_DIST):
continue
# check also that the surface is not too close to the robot
if close_to_robot(pose, robot_pose, CATS_MIN_DIST):
continue
# and not within an obstacle
if close_to_obstacle(x, y, theta, 0.0):
continue
added_poses.append(pose)
model = cat['name']
model_name = model + '_' + str(cat_index)
success = spawn_model(
name=model_name,
model=models[model].format(name=model_name),
pose=pose,
frame='ground_plane::link'
)
if success:
rospy.loginfo("Spawn %s at %s", model_name, pose2d2str(pose))
cat_index += 1
def nav_goal_cb(msg):
rospy.loginfo("Calling MBF's move_base action with target pose %s", pose2d2str(msg))
goal = MoveBaseGoal(target_pose=msg, controller='TEBPlanner')
move_base_ac.send_goal(goal, done_cb=nav_done_cb)
def execute(self, ud):
rospy.loginfo("Object '%s' moved to tray at %s", ud['object'].id,
pose2d2str(ud['pose_on_tray']))
PlanningScene().move_obj_to_tray(ud['object'].id, ud['pose_on_tray'])
return 'succeeded'
def execute(self, ud):
rospy.loginfo("Object '%s' pose readjusted to %s", ud['object'].id,
pose2d2str(ud['new_pose']))
PlanningScene().displace_obj(ud['object'].id, ud['new_pose'])
return 'succeeded'