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)