def test_semantic_map(node):
p00 = create_2d_pose(0, 0, 0, 'map')
p11 = create_2d_pose(1, 1, 0, 'map')
p22 = create_2d_pose(2, 2, 0, 'map')
p33 = create_2d_pose(3, 3, 0, 'map')
p16 = create_2d_pose(1.6, 1.6, 0, 'map')
sm = SemanticMap()
# add_object
sm.add_object('obj1', 'dummy', p11, (1, 1))
Visualization().add_box_marker(p00, (1, 1, 0.0001), Visualization.rand_color(0.2))
Visualization().publish_markers()
assert len(sm.objects_at(p00, (1, 1))) == 1
assert len(sm.objects_at(p00, (0.9, 0.9))) == 0
Visualization().add_box_marker(p22, (1, 1, 0.0001), Visualization.rand_color(0.2))
Visualization().publish_markers()
assert len(sm.objects_at(p22, (1, 1))) == 1
Visualization().add_box_marker(p33, (1, 1, 0.0001), Visualization.rand_color(0.2))
Visualization().publish_markers()
assert len(sm.objects_at(p33, (1, 1))) == 0
sm.add_object('obj2', 'dummy', p22, (0.5, 0.5))
Visualization().add_box_marker(p16, (1, 1, 0.0001), Visualization.rand_color(0.2))
Visualization().publish_markers()
assert len(sm.objects_at(p16, (1, 1))) == 2
# remove_object
sm.remove_object('obj1')
assert len(sm.objects_at(p16, (1, 1))) == 1
sm.remove_object('obj2')
assert len(sm.objects_at(p16, (1, 1))) == 0
# objects count
sm.add_object('obj1', 'dummy', p11, (1, 1))
sm.add_object('obj2', 'dummy', p22, (0.5, 0.5))
sm.add_object('obj3', 'dummy', p33, (0.5, 0.5))
assert sm.objects_count() == 3
assert sm.objects() == {'obj1', 'obj2', 'obj3'}
sm.remove_object('obj1')
sm.remove_object('obj2')
sm.remove_object('obj3')
assert sm.objects_count() == 0
assert sm.objects() == set()
def __init__(self):
smach.State.__init__(self,
outcomes=['succeeded', 'tray_full'],
input_keys=['object'],
output_keys=['pose_on_tray'])
self.tray_link = rospy.get_param('tray_link', 'tray_link')
self.tray_full = False
self.slots_x = int(cfg.TRAY_SIDE_X / cfg.TRAY_SLOT +
0.1) # avoid float division pitfall
self.slots_y = int(cfg.TRAY_SIDE_Y / cfg.TRAY_SLOT +
0.1) # until I switch to Python3
self.offset_x = 0.0 if self.slots_x % 2 else cfg.TRAY_SLOT / 2.0
self.offset_y = 0.0 if self.slots_y % 2 else cfg.TRAY_SLOT / 2.0
self.next_x = 0
self.next_y = 0
# add a collision object for the tray surface, right above the mesh
PlanningScene().add_tray(
create_3d_pose(0, 0, 0.0015, 0, 0, 0, self.tray_link),
(cfg.TRAY_SIDE_X + 0.01, cfg.TRAY_SIDE_Y + 0.01, 0.002))
# visualize place poses (for debugging)
points = []
for _ in range(self.slots_x * self.slots_y):
points.append(self._next_pose(0.01).pose.position)
Visualization().add_markers(Visualization().create_point_list(
create_2d_pose(0, 0, 0, self.tray_link), points,
[1, 0, 0, 1])) # solid red points
Visualization().publish_markers()
self.tray_full = False
def close_to_obstacle(x, y, theta, clearance):
# check if the location is away from any non-zero cost in the global costmap by at least the given clearance
# (we need to subtract robot radius because check pose service assumes we want to check the footprint cost)
resp = check_pose_srv(pose=create_2d_pose(x, y, theta, 'map'), safety_dist=clearance - robot_radius,
costmap=CheckPoseRequest.GLOBAL_COSTMAP)
if resp.state > 0 or resp.cost > 0:
return True
return False
def model_states_cb(self, msg):
if self.killed_cats and not self.alive_cats:
# all cats toppled; we can stop listening for model states
rospy.loginfo("All cats toppled; stop listening for model states")
self.model_states_sub.unregister()
return
# TODO maybe pause/resume myself (so once)? looks like not needed with current throttling
for index, model_name in enumerate(msg.name):
if model_name in self.alive_cats:
if abs(roll(msg.pose[index])) > self.hit_roll_threshold:
# we consider a cat toppled after it rolls by more than hit_roll_threshold
# TODO: maybe better, use body/head contacts with ground_plane
rospy.loginfo("Cat %s toppled at %s! (|roll| > %g)",
model_name, pose3d2str(msg.pose[index]),
self.hit_roll_threshold)
# send toppled cats out of the house and stop tracking them
pose = create_2d_pose(4.0 + len(self.killed_cats), 1.0,
math.pi / 2.0)
self.set_model_state_srv(
ModelState(model_name, pose, Twist(), 'map'))
self.killed_cats.append(model_name)
del self.alive_cats[model_name]
elif rospy.get_time() - self.alive_cats[model_name][
'last_hit'] < self.hit_sock_duration:
# hit, so stop moving; admittedly, not the most realistic for a cat
pass
elif self.prowling_step > 0.0:
# hanging around: perform small steps but changing directions whenever we touch anything
new_pose = copy.deepcopy(msg.pose[index])
contact = self.alive_cats[model_name]['contact']
if contact:
# use inv contact direction to limit the possible orientations to those away from the contact
contact_inv_dir = math.atan2(
contact.contact_normals[0].y,
contact.contact_normals[0].x)
new_yaw_bound1 = norm_angle(contact_inv_dir -
math.pi / 3.0)
new_yaw_bound2 = norm_angle(contact_inv_dir +
math.pi / 3.0)
new_yaw = random.uniform(new_yaw_bound1,
new_yaw_bound2)
new_pose.orientation = quaternion_msg_from_yaw(new_yaw)
rospy.loginfo(
"%s: contact from %f; new direction to %f",
model_name, contact_inv_dir, new_yaw)
self.alive_cats[model_name]['contact'] = None
translate_pose(new_pose, self.prowling_step, 'x')
self.set_model_state_srv(
ModelState(model_name, new_pose, Twist(), 'map'))
elif model_name not in self.killed_cats:
# first time to see this model; keep track of it if listed under target models
# (individual instances are numbered _0, _1 and so by the model spawner)
if model_name in self.target_models or '_'.join(
model_name.split('_')[:-1]) in self.target_models:
self.alive_cats[model_name] = {
'last_hit': -1.0,
'contact': None
}
def make_goal(self, ud, goal):
""" Create a goal for the action
"""
room_number = ud['room_number']
segmented_map = ud['segmented_map']
# We need an image containing only the room to explore, so we create an empty image with the same
# properties as the original map, set to white (\xFF) all pixels that correspond to the given room
# number in the segmented map, and set as black (\x00) te rest
goal.input_map = ud['map_image']
goal.input_map.data = b''
# segmented_map encoding is 32SC1, so 4 bytes, though just the first byte is enough up to 255 rooms
for i in range(segmented_map.height):
for j in range(segmented_map.width):
idx = i * segmented_map.step + j * 4
val = segmented_map.data[idx:idx + 4]
pixel = struct.unpack('<BBBB', val)[0]
if pixel == room_number:
goal.input_map.data += b'\xFF'
else:
goal.input_map.data += b'\x00'
fov_points = rospy.get_param(
'/exploration/room_exploration_server/field_of_view_points')
goal.field_of_view_origin = TF2().transform_pose(
None, 'kinect_rgb_frame', 'base_footprint').pose.position
goal.field_of_view = [geometry_msgs.Point32(*pt) for pt in fov_points]
goal.planning_mode = 2 # plan a path for coverage with the robot's field of view
goal.starting_position = to_pose2d(
ud['robot_pose']) # we need to convert to Pose2D msg
# use room center as starting point; theta is ignored by room exploration
room_info = ud['room_information_in_meter'][room_number - 1]
start_point = geometry_msgs.PointStamped()
start_point.header.frame_id = 'map'
start_point.point = room_info.room_center
goal.starting_position.x = room_info.room_center.x
goal.starting_position.y = room_info.room_center.y
goal.starting_position.theta = 0.0 # it's ignored
self.start_pub.publish(start_point)
# provide the starting pose so we can move there before starting exploring
ud['start_pose'] = create_2d_pose(room_info.room_center.x,
room_info.room_center.y, 0.0, 'map')
# IDEA: can use room_info.room_min_max to avoid points colliding with the walls
# visualize explore map and fov on RViz
fov = geometry_msgs.PolygonStamped()
fov.header.frame_id = 'kinect_rgb_frame'
fov.polygon.points = goal.field_of_view
rospy.Publisher('/exploration/img',
sensor_msgs.Image,
queue_size=1,
latch=True).publish(goal.input_map)
if not self.fov_pub_timer:
self.fov_pub_timer = rospy.Timer(
rospy.Duration(0.1), lambda e: self.fov_pub.publish(fov))
def spawn_rockets():
# spawn 10 x 10 rockets
rocket_index = 1
for i, j in product(range(-10, 0), range(10)):
spawn_model(
name='rocket' + str(rocket_index),
model=models['rocket'],
pose=create_2d_pose(i, j, 0),
frame='ground_plane::link'
)
rocket_index += 1
def test_semantic_layer(node):
p00 = create_2d_pose(0, 0, 1, 'map')
p11 = create_2d_pose(1, 2, 1, 'map')
p22 = create_2d_pose(2, 2, 1, 'map')
p33 = create_2d_pose(3, 3, 1, 'map')
p16 = create_2d_pose(1.6, 1.6, 0, 'map')
sz1 = (2.0, 2.0)
sz2 = (1.2, 0.8)
sz3 = (0.5, 0.5)
sz4 = (0.75, 0.75)
sl = SemanticLayer()
# add some obstacles
assert sl.add_object('obs0', 'blocked_area', p11, sz1, costmap='global')
assert sl.remove_object('obs0', 'blocked_area', costmap='global')
assert sl.add_object('obs1', 'obstacle', p11, sz1, costmap='local')
assert sl.add_object('obs2', 'obstacle', p11, sz2, costmap='local')
assert sl.add_object('obs3', 'obstacle', p11, sz3, costmap='local')
assert sl.add_object('obs4', 'obstacle', p11, sz4, costmap='local')
# add some free space
assert sl.add_object('fs1', 'free_space', p11, (2.5, 2.9), costmap='local')
assert sl.add_object('fs2', 'free_space', p11, sz2, costmap='local')
# move the first one
assert sl.add_object('obs1', 'obstacle', p33, sz1, costmap='local')
# resize the second one
assert sl.add_object('obs2', 'obstacle', p11, sz1, costmap='local')
# remove un-existing object
assert not sl.remove_object('xxx', 'obstacle', costmap='local')
# remove all objects
assert sl.remove_object('fs2', 'free_space', costmap='local')
assert sl.remove_object('fs1', 'free_space', costmap='local')
assert sl.remove_object('obs4', 'obstacle', costmap='local')
assert sl.remove_object('obs3', 'obstacle', costmap='local')
assert sl.remove_object('obs2', 'obstacle', costmap='local')
assert sl.remove_object('obs1', 'obstacle', costmap='local')
def execute(self, ud):
table = ud['table']
table_pose = ud['table_pose'] # expected on map frame, as robot_pose
if not table or not table_pose:
rospy.logerr("Detected table contains None!")
return 'no_table'
p_x = self.distance + table.depth / 2.0
n_x = -p_x
p_y = self.distance + table.width / 2.0
n_y = -p_y
table_tf = to_transform(table_pose, 'table_frame')
TF2().publish_transform(table_tf)
table_tf.transform.translation.z = 0.0
closest_pose = None
closest_dist = float('inf')
from math import pi
poses = [('p_x', create_2d_pose(p_x, 0.0, -pi, 'table_frame')),
('n_x', create_2d_pose(n_x, 0.0, 0.0, 'table_frame')),
('p_y', create_2d_pose(0.0, p_y, -pi / 2.0, 'table_frame')),
('n_y', create_2d_pose(0.0, n_y, +pi / 2.0, 'table_frame'))]
pose_array = geometry_msgs.PoseArray() # for visualization
sides_poses = {}
for name, pose in poses:
pose = transform_pose(pose, table_tf)
pose_array.poses.append(deepcopy(pose.pose))
pose_array.poses[
-1].position.z += 0.025 # raise over costmap to make it visible
sides_poses[name] = pose
dist = distance_2d(pose, ud['robot_pose'])
if dist < closest_dist:
closest_pose = pose
closest_dist = dist
ud['poses'] = sides_poses
ud['closest_pose'] = closest_pose
pose_array.header = deepcopy(closest_pose.header)
pose_array.poses.append(deepcopy(closest_pose.pose))
pose_array.poses[
-1].position.z += 0.05 # remark the closest pose with a double arrow
self.poses_viz.publish(pose_array)
return 'succeeded'
def test_progress_tracker(node):
wp = [
create_2d_pose(-1, -1, 0, 'map'),
create_2d_pose(+1, -1, 0, 'map'),
create_2d_pose(+1, +1, 0, 'map'),
create_2d_pose(-1, +1, 0, 'map')
]
origin = create_2d_pose(0, 0, 0, 'map')
Visualization().add_point_markers(wp, 0.1)
pt = ProgressTracker(wp, np.sqrt(2) - 0.99)
for theta in np.arange(-np.pi, np.pi, np.pi / 100.0):
rpose = create_2d_pose(np.cos(theta), np.sin(theta), 0, 'map')
Visualization().add_line_marker([origin, rpose])
Visualization().publish_markers()
pt.update_pose(rpose)
if theta > (3 * np.pi) / 4.0:
assert pt.reached_waypoint() == 3
assert pt.next_waypoint() is None
elif theta > np.pi / 4.0:
assert pt.reached_waypoint() == 2
assert pt.next_waypoint() == 3
elif theta > -np.pi / 4.0:
assert pt.reached_waypoint() == 1
assert pt.next_waypoint() == 2
elif theta > -(3.0 * np.pi) / 4.0:
assert pt.reached_waypoint() == 0
assert pt.next_waypoint() == 1
else:
assert pt.reached_waypoint() is None
assert pt.next_waypoint() == 0
def spawn_surfaces(use_preferred_locs=False):
added_poses = [] # to check that tables are at least SURFS_MIN_DIST apart from each other
for surf in surfaces:
surf_index = 0
# clearance = surface diagonal / 2 + some extra margin
clearance = sqrt((surf['size'][0] / 2.0) ** 2 + (surf['size'][1] / 2.0) ** 2) + 0.1
while surf_index < surf['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, SURFS_MIN_DIST):
continue
# check also that the surface is not too close to the robot
if close_to_robot(pose, robot_pose, SURFS_MIN_DIST):
continue
# and in open space
if close_to_obstacle(x, y, theta, clearance):
continue
added_poses.append(pose)
model = surf['name']
model_name = model + '_' + str(surf_index + 10) # allow for some objects added by hand
success = spawn_model(
name=model_name,
model=models[model],
pose=pose,
frame='ground_plane::link'
)
if success:
# populate this surface with some objects
spawn_objects(surf, surf_index + 10)
surf_index += 1
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
#!/usr/bin/env python
import rospy
from smach.user_data import UserData
from thorp_toolkit.geometry import TF2, create_2d_pose
from thorp_smach.states.navigation import LookToPose
from thorp_smach.utils import wait_for_mbf
if __name__ == '__main__':
rospy.init_node('run_look_to_pose_state')
# MBF is the last component to start, so wait for it before running the sm
wait_for_mbf()
ud = UserData()
ud['robot_pose'] = TF2().transform_pose(None, 'base_footprint', 'map')
ud['target_pose'] = create_2d_pose(0.0, 0.0, 0.0, 'map')
state = LookToPose()
t0 = rospy.get_time()
outcome = state.execute(ud)
rospy.loginfo("LookToPose completed in %.2fs with outcome '%s'",
rospy.get_time() - t0, outcome)
# Wait for ctrl-c to stop the application
rospy.spin()
rospy.signal_shutdown('All done.')
load_models()
robot_pose = TF2().transform_pose(None, 'base_footprint', 'map')
robot_radius = rospy.get_param('move_base_flex/local_costmap/robot_radius')
random.seed()
use_preferred_locs = len(sys.argv) > 2 and '-l' in sys.argv
rospy.loginfo("Spawning %s in %s locations", sys.argv[1], 'preferred' if use_preferred_locs else 'random')
if sys.argv[1] == 'objects':
spawn_surfaces(use_preferred_locs)
rospy.loginfo("Spawned objects:\n " + '\n '.join('{}: {}'.format(k, v) for k, v in spawned.items()))
elif sys.argv[1] == 'cats':
spawn_cats(use_preferred_locs)
spawn_rockets()
elif sys.argv[1] == 'playground_random': # random objects over a random table
surface = random.choice(surfaces)
spawn_model(surface['name'] + '_0', models[surface['name']], create_2d_pose(0.44, 0, pi / 2.0),
'ground_plane::link')
spawn_objects(surface, 0, sys.argv[2] if len(sys.argv) > 2 and not use_preferred_locs else None)
elif sys.argv[1] == 'playground_objs': # a sample of objects mostly at reachable locations
spawn_model('lack_table', models['lack_table'], create_2d_pose(0.45, 0, 0.0), 'ground_plane::link')
for obj in PLAYGROUND_OBJS:
spawn_model(obj[0], models[obj[1]], create_3d_pose(*obj[2]), 'ground_plane::link')
elif sys.argv[1] == 'playground_cubes': # cubes at reachable locations, ready to stack
spawn_model('doll_table', models['doll_table'], create_2d_pose(0.38, 0, 0.0), 'ground_plane::link')
for obj in PLAYGROUND_CUBES:
spawn_model(obj[0], models[obj[1]], create_3d_pose(*obj[2]), 'doll_table::link')
sys.exit(0)
state_pub = rospy.Publisher("gazebo/set_model_state", ModelState, queue_size=1)
pose_pub = rospy.Publisher("target_object_pose", PoseStamped, queue_size=1)
target_models = sys.argv[1:]
if not target_models:
rospy.logwarn("No target models provided; nothing to do")
print("Usage: model_markers.py <list of interactive models>")
sys.exit(0)
try:
model_states = rospy.wait_for_message("gazebo/model_states", ModelStates, 10.0)
except rospy.ROSInterruptException:
sys.exit(0)
except rospy.ROSException as err:
rospy.logwarn(err)
rospy.logwarn("Creating markers for models %s on random poses", str(target_models))
model_states = ModelStates()
for model_name in target_models:
model_states.name.append(model_name)
model_states.pose.append(create_2d_pose(randint(1, 10), randint(1, 10), uniform(-pi, pi)))
for index, model_name in enumerate(model_states.name):
if model_name in target_models:
make_interactive_marker(model_states.pose[index], model_name)
rospy.loginfo("Interactive marker added at %s for model %s",
pose3d2str(model_states.pose[index]), model_name)
server.applyChanges()
rospy.spin()