def __init__(self): # type: () -> None
self._robot_state = RobotState()
self._robot_control = RobotControl()
self._goal_pool = GoalPool()
self._grid = Grid()
self._goal_selector = GoalSelector()
self._path_finder = PathFinder()
self._marker_drawer = MarkerDrawer()
self._current_goal = None
def initStates(self):
self.state_lock = threading.RLock()
self._states = {0: RobotState(timestamp=0)}
self._lastState = self._states[0]
self._lastState.gt = State([0, 0, 0])
self._lastState.noised = State([0, 0, 0])
self._lastState.filtered = State([0, 0, 0])
def __init__(self, gui):
super().__init__()
# In order to get to the slider values
self.gui = gui
self.title = "Robot and Walls"
# Window size
self.top = 15
self.left = 15
self.width = 700
self.height = 700
# State/action text
self.sensor_text = "No sensor"
self.action_text = "No action"
# The world state
self.world_state = WorldState()
# For querying door
self.door_sensor = DoorSensor()
# For moving robot
self.robot_state = RobotState()
# For robot state estimation
self.state_estimation = RobotStateEstimation()
# Height of prob
self.draw_height = 0.6
# Set geometry
self.initUI()
def __init__(self, robot_radius, robot_fov_radius, static_map_topic, local_goal_pose_topic,
initial_pose_topic, point_clicked_topic, simulated_occ_grid_topic, simulated_pose_topic,
simulated_fov_pointcloud_topic, simulated_robot_polygonal_footprint_topic,
simulated_robot_polygonal_fov_topic, gazebo_model_states_topic):
# Parameters
self.static_map_topic = static_map_topic
self.local_goal_pose_topic = local_goal_pose_topic
self.initial_pose_topic = initial_pose_topic
self.point_clicked_topic = point_clicked_topic
self.simulated_occ_grid_topic = simulated_occ_grid_topic
self.simulated_pose_topic = simulated_pose_topic
self.simulated_fov_pointcloud_topic = simulated_fov_pointcloud_topic
self.simulated_robot_polygonal_footprint_topic = simulated_robot_polygonal_footprint_topic
self.simulated_robot_polygonal_fov_topic = simulated_robot_polygonal_fov_topic
## Gazebo
self.gazebo_model_states_topic = gazebo_model_states_topic
# World state
self.simulated_map = None
self.static_map = None
# Subscribers
rospy.Subscriber(self.static_map_topic, OccupancyGrid, self._static_map_callback)
rospy.Subscriber(self.initial_pose_topic, PoseWithCovarianceStamped, self._initial_pose_callback)
rospy.Subscriber(self.point_clicked_topic, PointStamped, self._point_for_obstacle)
# Services
rospy.Service(self.local_goal_pose_topic, LocalGoalPose, self._local_goal_pose_callback)
# Publishers
self.simulated_pose_pub = rospy.Publisher(self.simulated_pose_topic, PoseStamped, queue_size=1)
self.simulated_robot_polygonal_footprint_pub = rospy.Publisher(self.simulated_robot_polygonal_footprint_topic, PolygonStamped, queue_size=1)
self.simulated_robot_polygonal_fov_pub = rospy.Publisher(self.simulated_robot_polygonal_fov_topic, PolygonStamped, queue_size=1)
self.simulated_fov_pointcloud_pub = rospy.Publisher(self.simulated_fov_pointcloud_topic, PointCloud, queue_size=1)
self.simulated_occ_grid_pub = rospy.Publisher(self.simulated_occ_grid_topic, OccupancyGrid, queue_size=1)
# Initialize map and robot
while self.static_map is None:
rospy.sleep(0.2)
self.simulated_robot_state = RobotState(robot_radius, robot_fov_radius, self.static_map.info.resolution)
self.simulated_map = MultilayeredMap(self.static_map, self.simulated_robot_state.metadata)
self.simulated_fov_pointcloud = PointCloud()
## Gazebo subscriber goes here because map must be created first
rospy.Subscriber(self.gazebo_model_states_topic, ModelStates, self._gazebo_model_states_callback)
def plan_between_joint_states(self, js1, js2):
"""Plan path between two joint states.
Arguments:
js1,js2 : {JointState message}
Returns:
trajectory : {RobotTrajectory message}
"""
tmp_robot_state = self._get_start_state()
rs = RobotState(copy.deepcopy(tmp_robot_state))
rs.update_from_joint_state(js1)
self.set_start_state(rs.msg)
self.set_joint_value_target(js2)
traj = self.plan()
self.set_start_state(tmp_robot_state)
if len(traj.joint_trajectory.joint_names) <= 0:
return None
else:
return traj
def __init__(self, gui_world):
super().__init__()
# In order to get to the slider values
self.gui = gui_world
self.title = "Robot and Walls"
# Window size
self.top = 15
self.left = 15
self.width = 700
self.height = 700
# State/action text
self.sensor_text = "No sensor"
self.action_text = "No action"
self.move_text = "No move"
self.loc_text = "No location"
# The world state
self.world_state = WorldState()
# For querying door
self.door_sensor = DoorSensor()
# For moving robot
self.robot_state = RobotState()
# For robot state estimation
self.state_estimation = RobotStateEstimation()
# For keeping sampled error
self.last_wall_sensor_noise = 0
self.last_move_noise = 0
# Height of prob
self.draw_height = 0.6
# Set geometry
self.text = "None"
self.init_ui()
def getState(self, timestamp=None, insert=True):
self.state_lock.acquire()
# case of external call, for non modification
if timestamp is None:
c = copy.copy(self._lastState)
self.state_lock.release()
return c
timestamp = stamptofloat(timestamp)
if timestamp not in self._states:
if not insert:
self.state_lock.release()
return None
self._states[timestamp] = RobotState(timestamp=timestamp)
self._states[timestamp].setPreviousRobotState(self._lastState)
# if timestamp > self._lastState.time:
# self._lastState = self._states[timestamp]
self.state_lock.release()
return self._states[timestamp]
return True
# set the probabilities based on the gui
def set_probabilities(self, in_prob_see_door_if_door,
in_prob_see_door_if_not_door):
self.prob_see_door_if_door = in_prob_see_door_if_door
self.prob_see_door_if_no_door = in_prob_see_door_if_not_door
if __name__ == '__main__':
ws_global = WorldState()
ds_global = DoorSensor()
rs_global = RobotState()
# Robot should be at 0.5, no door at 0.5, so this should be false
print("Testing probabilities for robot NOT in front of door")
rs_global.robot_loc = ws_global.place_robot_not_in_front_of_door()
if ds_global.is_in_front_of_door(ws_global, rs_global):
raise ValueError("The robot should NOT be in front of a door")
# Check that we get our probabilites back (mostly)
count_returned_true = 0
for i in range(0, 1000):
if ds_global.sensor_reading(ws_global, rs_global):
count_returned_true += 1
prob_count = count_returned_true / 1000
if abs(prob_count - ds_global.prob_see_door_if_no_door) > 0.1:
__maintainer__ = "Chris Suarez"
__email__ = "*****@*****.**"
__status__ = "Production"
__doc__ = """This subcribes to some point cloud topic and then transforms it into a laserscan.
It is intended for use on a mobie robot with cloud scan of the environment to use for 3D navigation.
Any point in the cloud above the robot_height will be thrown out as well as any point that has a
z value of 0.0 +-floor_range (assumed to be the floor that the robot is driving on)."""
import rospy
from cloud_to_laserscan import CloudToLaserscan
from robot_state import RobotState
from live_nav import LiveNav
from live_mapper import LiveMapper
if __name__ == '__main__':
rospy.init_node('nav_3d')
try:
_RobotState = RobotState()
# _CloudToLaserscan = CloudToLaserscan()
# _LiveNav = LiveNav()
_LiveMap = LiveMapper()
pub_rate = max(_RobotState.pub_rate, _LiveMap.pub_rate)
p_rate = rospy.Rate(pub_rate)
while not rospy.is_shutdown():
_RobotState.analyze_links()
p_rate.sleep()
except rospy.ROSInterruptException:
pass
class BasicSimulator:
obstacle_id_counter = 1
def __init__(self, robot_radius, robot_fov_radius, static_map_topic, local_goal_pose_topic,
initial_pose_topic, point_clicked_topic, simulated_occ_grid_topic, simulated_pose_topic,
simulated_fov_pointcloud_topic, simulated_robot_polygonal_footprint_topic,
simulated_robot_polygonal_fov_topic, gazebo_model_states_topic):
# Parameters
self.static_map_topic = static_map_topic
self.local_goal_pose_topic = local_goal_pose_topic
self.initial_pose_topic = initial_pose_topic
self.point_clicked_topic = point_clicked_topic
self.simulated_occ_grid_topic = simulated_occ_grid_topic
self.simulated_pose_topic = simulated_pose_topic
self.simulated_fov_pointcloud_topic = simulated_fov_pointcloud_topic
self.simulated_robot_polygonal_footprint_topic = simulated_robot_polygonal_footprint_topic
self.simulated_robot_polygonal_fov_topic = simulated_robot_polygonal_fov_topic
## Gazebo
self.gazebo_model_states_topic = gazebo_model_states_topic
# World state
self.simulated_map = None
self.static_map = None
# Subscribers
rospy.Subscriber(self.static_map_topic, OccupancyGrid, self._static_map_callback)
rospy.Subscriber(self.initial_pose_topic, PoseWithCovarianceStamped, self._initial_pose_callback)
rospy.Subscriber(self.point_clicked_topic, PointStamped, self._point_for_obstacle)
# Services
rospy.Service(self.local_goal_pose_topic, LocalGoalPose, self._local_goal_pose_callback)
# Publishers
self.simulated_pose_pub = rospy.Publisher(self.simulated_pose_topic, PoseStamped, queue_size=1)
self.simulated_robot_polygonal_footprint_pub = rospy.Publisher(self.simulated_robot_polygonal_footprint_topic, PolygonStamped, queue_size=1)
self.simulated_robot_polygonal_fov_pub = rospy.Publisher(self.simulated_robot_polygonal_fov_topic, PolygonStamped, queue_size=1)
self.simulated_fov_pointcloud_pub = rospy.Publisher(self.simulated_fov_pointcloud_topic, PointCloud, queue_size=1)
self.simulated_occ_grid_pub = rospy.Publisher(self.simulated_occ_grid_topic, OccupancyGrid, queue_size=1)
# Initialize map and robot
while self.static_map is None:
rospy.sleep(0.2)
self.simulated_robot_state = RobotState(robot_radius, robot_fov_radius, self.static_map.info.resolution)
self.simulated_map = MultilayeredMap(self.static_map, self.simulated_robot_state.metadata)
self.simulated_fov_pointcloud = PointCloud()
## Gazebo subscriber goes here because map must be created first
rospy.Subscriber(self.gazebo_model_states_topic, ModelStates, self._gazebo_model_states_callback)
def _static_map_callback(self, static_map):
# For the moment, we don't want to manage new static maps for the
# node's life duration
if self.static_map is None:
self.static_map = static_map
def _local_goal_pose_callback(self, request):
local_pose_goal = request.local_pose_goal
is_manipulation = request.is_manipulation
response = LocalGoalPoseResponse()
goal_map_coords = Utils.map_coord_from_ros_pose(local_pose_goal, self.simulated_map.info.resolution)
expected_polygon_footprint = Point((local_pose_goal.pose.position.x, local_pose_goal.pose.position.y)).buffer(self.simulated_robot_state.metadata.radius)
# Check if expected robot polygon is going to intersect any obstacle's polygons
obstacles_colliding_with_robot = set()
for obstacle in self.simulated_map.obstacles.values():
if expected_polygon_footprint.intersects(obstacle.polygon):
# If the robot entered in collision with at least one unmovable obstacle, the pose is not updated
if obstacle.movability == Movability.unmovable:
response.real_pose = self.simulated_robot_state.pose
return response
obstacles_colliding_with_robot.add(obstacle)
# Check if robot is going to enter in contact with static obstacles
is_colliding_with_static_obstacles = (
Utils._is_in_matrix(goal_map_coords[0], goal_map_coords[1], self.simulated_map.info.width, self.simulated_map.info.height) and
self.simulated_map.inflated_static_occ_grid[goal_map_coords[0]][goal_map_coords[1]] >= Utils.ROS_COST_POSSIBLY_CIRCUMSCRIBED)
# If no obstacles collided with the robot, simply update the pose
if not (bool(obstacles_colliding_with_robot) or is_colliding_with_static_obstacles):
self.simulated_robot_state.set_pose(local_pose_goal, self.simulated_map.info.resolution)
self.simulated_fov_pointcloud = self.simulated_map.get_point_cloud_in_fov(self.simulated_robot_state.pose)
else:
# Else we must check if these obstacles collide with other when translation is applied
translation = (local_pose_goal.pose.position.x - self.simulated_robot_state.pose.pose.position.x,
local_pose_goal.pose.position.y - self.simulated_robot_state.pose.pose.position.y)
for colliding_obstacle in obstacles_colliding_with_robot:
colliding_obstacle_with_push = copy.deepcopy(colliding_obstacle)
colliding_obstacle_with_push.move(translation)
if colliding_obstacle_with_push.movability == Movability.movable:
# Check collision between obstacles
for other_obstacle_id, other_obstacle in self.simulated_map.obstacles.items():
# If the movable obstacle we are moving enters in collision with another, the pose is not updated
if (other_obstacle_id != colliding_obstacle_with_push.obstacle_id and
colliding_obstacle_with_push.polygon.intersects(other_obstacle.polygon)):
response.real_pose = self.simulated_robot_state.pose
return response
# Check collision between pushed obstacle and static obstacles
for map_point in colliding_obstacle_with_push.discretized_polygon:
if self.simulated_map.static_occ_grid[map_point[0]][map_point[1]] == Utils.ROS_COST_LETHAL:
response.real_pose = self.simulated_robot_state.pose
return response
# If no obstacle bothered us while pushing a movable obstacle, apply translation to it and update robot pose
self.simulated_robot_state.set_pose(local_pose_goal, self.simulated_map.info.resolution)
if is_manipulation:
for colliding_obstacle in obstacles_colliding_with_robot:
self.simulated_map.manually_move_obstacle(colliding_obstacle.obstacle_id, translation)
self.simulated_fov_pointcloud = self.simulated_map.get_point_cloud_in_fov(self.simulated_robot_state.pose)
response.real_pose = self.simulated_robot_state.pose
return response
def _initial_pose_callback(self, pose_with_covariance):
pose = PoseStamped()
pose.header = pose_with_covariance.header
pose.pose = pose_with_covariance.pose.pose
map_coords = Utils.map_coord_from_ros_pose(pose, self.simulated_map.info.resolution)
try:
self.set_initial_robot_pose(pose, map_coords[0], map_coords[1])
except RobotPlacementException:
rospy.logerr(
"The given robot pose is not valid: : it is either outside of the map or intersects obstacles.")
def _point_for_obstacle(self, point):
pass
# TODO BONUS allow simple obstacle creation through rviz
# Check in polygonal representation of all obstacles if point is inside by other obstacle
static_models = set()
non_static_models = {}
def _gazebo_model_states_callback(self, model_states):
# Add or update models
for i in range(len(model_states.name)):
model_name = model_states.name[i]
pose = model_states.pose[i]
# If model is in non_static_models, update its pose
if model_name in self.non_static_models.keys():
# TODO update our obstacle from gazebo model
# self.simulated_map.obstacles.values()
# self.simulated_map.manually_move_obstacle(colliding_obstacle.obstacle_id, translation)
pass
# If Gazebo model is not already in our own representation, add it
else:
if model_name in self.static_models:
continue
else:
# Call GetModel Service to get Volume and Metadata for obstacle
rospy.wait_for_service('/gazebo/GetModel')
try:
get_model = rospy.ServiceProxy('/gazebo/GetModel', GetModel)
model = get_model(model_name).model
# If static, don't add obstacle to map, and add name to static_models set
if model.is_static or "turtlebot" in model_name:
self.static_models.add(model_name)
# If non-static, add obstacle to map and add obstacle id and its name to non_static_models dict
else:
for link in model.links:
for collision in link.collisions:
shape = collision.shape
pose = collision.pose
if shape.shape_type == shape.BOX_SHAPE:
size = shape.size
half_x, half_y, half_z = 0.5*shape.size.x, 0.5*shape.size.y, 0.5*shape.size.z
relative_points_set = {
(half_x, half_y),
(half_x, -half_y),
(-half_x, half_y),
(-half_x, -half_y),
}
yaw = Utils.yaw_from_geom_quat(pose.orientation)
absolute_points_set = set()
for point in relative_points_set:
absolute_point = (2.973 + pose.position.x + point[0] * math.cos(yaw) - point[1] * math.sin(yaw),
2.659 - (pose.position.y + point[0] * math.sin(yaw) + point[1] * math.cos(yaw)))
absolute_points_set.add(absolute_point)
obstacle_id = self.manually_add_obstacle_from_real_coordinates(absolute_points_set, True)
self.non_static_models.update({model_name:obstacle_id})
pass
except rospy.ServiceException, e:
print "Service call failed: %s" % e
# Remove from our dict or set the models that non longer exist in Gazebo
for static_model_name in self.static_models:
if (static_model_name not in model_states.name):
self.static_models.remove(static_model_name)
for non_static_model_name in self.non_static_models:
if (non_static_model_name not in model_states.name):
self.non_static_models.pop(non_static_model_name)
def main():
robot = RobotState("debile")
robot.start()
# end homework 2 : problem 1
return True
# set the probabilities based on the gui
def set_probabilities(self, in_prob_see_door_if_door,
in_prob_see_door_if_not_door):
self.prob_see_door_if_door = in_prob_see_door_if_door
self.prob_see_door_if_no_door = in_prob_see_door_if_not_door
if __name__ == '__main__':
ws = WorldState()
ds = DoorSensor()
rs = RobotState()
# Robot should be at 0.5, no door at 0.5, so this should be false
print("Testing probabilities for robot NOT in front of door")
rs.robot_loc = ws.place_robot_NOT_in_front_of_door()
if ds.is_in_front_of_door(ws, rs):
raise ValueError("The robot should NOT be in front of a door")
# Check that we get our probabilites back (mostly)
count_returned_true = 0
for i in range(0, 1000):
if ds.sensor_reading(ws, rs) == True:
count_returned_true += 1
prob_count = count_returned_true / 1000
if abs(prob_count - ds.prob_see_door_if_no_door) > 0.1:
def main():
robot = RobotState("mark")
robot.start()
else:
raise
return xmltodict.parse(xml)['robot']
def start_host_robot(config, sm):
mod_name = str(config['mod_name'])
robot_module = importlib.import_module("behaviors." + mod_name)
behavior_thread = Thread(target=robot_module.behavior, args=(sm, ))
behavior_thread.daemon = True
behavior_thread.start()
pipe = PayloadPipe()
pipe.set_visibility(True)
config = load_xml_to_dict()
sm = StateManager(
pipe,
RobotState(config['name'], config['model'], config['state'],
config['progression']))
start_host_robot(config, sm)
pipe.start_spinning()
while True:
try:
time.sleep(0.1)
except KeyboardInterrupt:
pipe.stop_spinning()
pipe.disable()
sys.exit()
class RobotNavigation:
def __init__(self): # type: () -> None
self._robot_state = RobotState()
self._robot_control = RobotControl()
self._goal_pool = GoalPool()
self._grid = Grid()
self._goal_selector = GoalSelector()
self._path_finder = PathFinder()
self._marker_drawer = MarkerDrawer()
self._current_goal = None
def update(self): # type: () -> None
""" Updates the navigation of the robot. """
if not self._robot_state.received_all_data():
return
self._grid.update(self._robot_state)
self._marker_drawer.draw_obstacles(self._grid.obstacles, self._robot_state)
self._goal_pool.check_goals(self._robot_state)
new_goal = self._goal_selector.select_goal(self._goal_pool.get_uncollected_goals(), self._grid,
self._robot_state)
if new_goal is not self._current_goal and new_goal is not None:
rospy.loginfo("Target: (%s %s), reward=%s" % (new_goal.x, new_goal.y, new_goal.reward))
self._current_goal = new_goal
self._marker_drawer.draw_goals(self._current_goal, self._goal_pool.goals, self._robot_state)
# stop robot if no goal is selected
if self._current_goal is None:
self._robot_control.stop()
return
goal_grid_position = self._grid.nearby_free_grid_position((self._current_goal.x, self._current_goal.y),
GOAL_RADIUS_SQRT)
# set goal as unreachable if no grid position found
if goal_grid_position is None:
self._current_goal.unreachable = True
self._robot_control.stop()
return
# find the path to the goal
path = self._path_finder.find_path(self._grid.obstacles, self._robot_state.proximal_position,
goal_grid_position)
# check if path was found
if len(path) <= 1:
self._current_goal.unreachable = True
self._robot_control.stop()
return
self._marker_drawer.draw_path(path, self._robot_state)
# get furthest away target position which is still in sight
target_position = self._grid.first_in_sight(path[:0:-1], self._robot_state.proximal_position)
if target_position is None:
target_position = path[1]
# check if target position is in obstacle
if self._grid.obstacles.__contains__(target_position):
self._robot_control.stop()
return
self._robot_control.navigate(target_position, self._robot_state)
self._marker_drawer.draw_direction(target_position, self._robot_state)