def generate_path(x, y, tol): rospy.wait_for_service('global_planner/planner/make_plan') try: odom_act = rospy.wait_for_message('gazebo_odom', Odometry) start = PoseStamped() x_act = odom_act.pose.pose.position.x y_act = odom_act.pose.pose.position.y start.header.frame_id = 'map' start.pose.position.x = x_act start.pose.position.y = y_act goal = PoseStamped() goal.header.frame_id = 'map' goal.pose.position.x = x goal.pose.position.y = y generate_plan = rospy.ServiceProxy('/global_planner/planner/make_plan', GetPlan) req = GetPlanRequest() req.start = start req.goal = goal req.tolerance = tol generated_plan = generate_plan(req) return generated_plan.plan except rospy.ServiceException as e: print("Service call failed: %s", e)
def request(sx,sy,gx,gy): request = GetPlanRequest(); request.start.header.frame_id="/"+name_space+"/map"; request.start.pose.position.x=sx; request.start.pose.position.y=sy; request.start.pose.orientation.w=1.0; request.goal.header.frame_id="/"+name_space+"/map"; request.goal.pose.position.x=gx; request.goal.pose.position.y=gy; request.goal.pose.orientation.w=1.0; request.tolerance=0.5 try: response = my_server(request) if(len(response.plan.poses)==0): #print(name_space,"no path"); return 1000000; x=(response.plan.poses[0].pose.position.x); y=(response.plan.poses[0].pose.position.y); sum_path=0; for i in response.plan.poses: sum_path+=math.sqrt( (i.pose.position.x-x)**2 + (i.pose.position.y-y)**2); x=(i.pose.position.x) y=(i.pose.position.y) #print(sum_path) return sum_path; except rospy.ServiceException: print ("sending the request failed"); return -2;
def __request_cb(self, userdata, request): request = GetPlanRequest() request.goal.header.stamp = rospy.Time.now() request.goal.header.frame_id = self.frame request.goal.pose = position_tuple_to_pose(userdata.x, userdata.y, userdata.yaw) request.start.header.stamp = rospy.Time.now() request.start.header.frame_id = self.frame request.goal.pose = position_tuple_to_pose(userdata.start_x, userdata.start_y, userdata.start_yaw) #request.start.pose = position_tuple_to_pose(*util.get_current_robot_position(self.frame)) request.tolerance = 0.2 # meters in x/y return request
def __request_cb(self, userdata, request): request = GetPlanRequest() request.goal.header.stamp = rospy.Time.now() request.goal.header.frame_id = self.frame request.goal.pose = position_tuple_to_pose(userdata.x, userdata.y, userdata.yaw) request.start.header.stamp = rospy.Time.now() request.start.header.frame_id = self.frame request.goal.pose = position_tuple_to_pose(userdata.start_x, userdata.start_y, userdata.start_yaw) # request.start.pose = position_tuple_to_pose(*util.get_current_robot_position(self.frame)) request.tolerance = 0.2 # meters in x/y return request
def clicked_point(msg): # rospy.loginfo(msg) goal_pub = rospy.Publisher("/goal", MoveBaseActionGoal, queue_size=1) plan_client = ServiceProxy("/move_base/make_plan", GetPlan) goalPos = PoseStamped() goalPos.header.frame_id = "map" goalPos.header.stamp = rospy.Time.now() goalPos.pose.position.x = msg.point.x goalPos.pose.position.y = msg.point.y goalPos.pose.orientation.x = rot[0] goalPos.pose.orientation.y = rot[1] goalPos.pose.orientation.z = rot[2] goalPos.pose.orientation.w = rot[3] getplan = GetPlanRequest() getplan.start.header.frame_id = "map" getplan.start.header.stamp = rospy.Time.now() getplan.start.pose.position.x = x getplan.start.pose.position.y = y getplan.start.pose.orientation.x = 0 getplan.start.pose.orientation.y = 0 getplan.start.pose.orientation.z = 0 getplan.start.pose.orientation.w = 1 getplan.goal = goalPos getplan.tolerance = 0.5 goal = MoveBaseActionGoal() goal.goal.target_pose = goalPos goal.header.frame_id = "map" goal.header.stamp = rospy.Time.now() goal_pub.publish(goal) res = plan_client.call(getplan) rospy.loginfo(res.plan.header.frame_id) path_pub = rospy.Publisher("trajectory", Path, queue_size=1) if isinstance(res, GetPlanResponse): res.plan.header.frame_id = "map" # res.plan.header.stamp = rospy.Time.now() path_pub.publish(res.plan) while True: plan_pub = rospy.Publisher("/move_base/GlobalPlanner/plan", Path, queue_size=1) plan_pub.publish(res.plan)
def _is_reachable(self, value, start_value): request = GetPlanRequest() request.start.header.stamp = rospy.Time.now() request.start.header.frame_id = '/map' request.start.pose = start_value request.goal.header.stamp = rospy.Time.now() request.goal.header.frame_id = '/map' request.goal.pose = value request.tolerance = 0.3 # meters in x/y rospy.logdebug("%s sending request: %s", self, request) try: response = self._service_proxy(request) rospy.logdebug("%s received response: %s", self, response) response.plan.header.frame_id = '/map' self._planned_paths_pub.publish(response.plan) return len(response.plan.poses) > 0 except ServiceException as e: rospy.logerr(e) return None
def _is_reachable(self, value, start_value): request = GetPlanRequest() request.start.header.stamp = rospy.Time.now() request.start.header.frame_id = '/map' request.start.pose = start_value request.goal.header.stamp = rospy.Time.now() request.goal.header.frame_id = '/map' request.goal.pose = value request.tolerance = 0.3 # meters in x/y rospy.logdebug("%s sending request: %s", self, request) try: response = self._service_proxy(request) rospy.logdebug("%s received response: %s", self, response) response.plan.header.frame_id = '/map' self._planned_paths_pub.publish(response.plan) return len(response.plan.poses) > 0 except ServiceException as e: rospy.logerr(e) return None
def get_plan(self, x, y): planner = GetPlanRequest() mbot_position = self.get_position() # Set planner start pose planner.start.pose.position.x = mbot_position[0][0] planner.start.pose.position.y = mbot_position[0][1] planner.start.pose.position.z = mbot_position[0][2] planner.start.pose.orientation.x = mbot_position[1][0] planner.start.pose.orientation.y = mbot_position[1][1] planner.start.pose.orientation.z = mbot_position[1][2] planner.start.pose.orientation.w = mbot_position[1][3] planner.start.header.frame_id = 'map' planner.tolerance = 0 # Set planner goal pose planner.goal.pose.orientation = planner.start.pose.orientation planner.goal.pose.position = self.grid2pose(x, y, 0).position planner.goal.header.frame_id = 'map' # Get the plan to the goal path = self.make_plan(planner) return len(path.plan.poses)
def begin_explore( self, radius, preempt_func = retfalse ): rospy.logout( 'snaking_explore: setting radius' ) waypoints = self.setup_poses( radius ) # PoseStamped in self.frame local_planner = rospy.get_param('/move_base_node/base_local_planner') # : dwa_local_planner/DWAPlannerROS local_planner = local_planner[string.find(local_planner,'/')+1:] obs_range = rospy.get_param('/move_base_node/global_costmap/obstacle_range') move_tol = rospy.get_param('/move_base_node/'+local_planner+'/xy_goal_tolerance') no_progress = rospy.get_param( rospy.get_name() + '/no_progress_move', 5) unreached_time = rospy.get_param( rospy.get_name() + '/not_reached_move', 30) # I'm not entirely sure which service to use. I do know that # non-NavfnROS make_plan sometimes fails for an unknown # reason... and thus NavfnROS/make_plan is more robust. # srv_name = '/move_base_node/make_plan' srv_name = '/move_base_node/NavfnROS/make_plan' get_plan = rospy.ServiceProxy( srv_name, GetPlan ) # Clear costmap...? Do this here or in smach...? if preempt_func(): # immediately exit if overall action preempted return 'preempted' for i,w in enumerate( waypoints ): if preempt_func(): # immediately exit if overall action preempted return 'preempted' rospy.logout( 'snaking_explore: Seeking waypoint %d of %d' % (i+1,len(waypoints))) # rospy.logout( 'snaking_explore: %2.2f %2.2f in frame %s' % (w.pose.position.x, w.pose.position.y, w.header.frame_id)) rim = self.robot_in_map() w.header.stamp = rospy.Time(0) # transform it _now_ wim = self.ps_in_map( w ) # waypoint in map if not rim or not wim: # if transforms failed rospy.logout( 'snaking_explore: Bad transforms. Aborting explore' ) return 'aborted' if self.range_to_waypoint( rim, wim ) < 0.9 * obs_range: # We're nearby the waypoint, so we'll just trust that the planner # has a good view of its surroundings to determine reachability. req = GetPlanRequest() req.tolerance = 0.1 req.start = rim req.goal = wim resp = get_plan( req ) found_plan = bool( resp.plan.poses != [] ) if not found_plan: rospy.logout( 'snaking_explore: No plan to nearby waypoint. Proceeding to next waypoint' ) # Perhaps its worth pulling the waypoint closer until a path _is_ found? continue # We now know that a path exists. Send the waypoint to the client. rospy.logout( 'snaking_explore: Near by with good plan. Calling move_base action.' ) else: # Any nav plan at beyond obstacle range will require the # nav stack to get a better vantage. Send the waypoint to # the client. rospy.logout( 'snaking_explore: Far away. Calling move_base action.' ) # If we made it this far, it's time to call move_base action. # Cancel any previous goals self.client.cancel_goals_at_and_before_time( rospy.Time.now() ) if os.environ.has_key('ROBOT') and os.environ['ROBOT'] == 'sim': rospy.logout( 'running in sim: give the system a little time to respond' ) rospy.sleep( 1 ) # Send our new goal rospy.logout( 'snaking_explore: sending movebase action goal.' ) move_goal = MoveBaseGoal() move_goal.target_pose = wim self.client.send_goal( move_goal ) if os.environ.has_key('ROBOT') and os.environ['ROBOT'] == 'sim': rospy.logout( 'running in sim: give the system a little time to respond' ) rospy.sleep( 1 ) # We'll monitor the state ourselves, and allow ourselves # the opportunity to cancel the goal for various reasons # (eg. if we haven't moved for a while or if new plans # aren't found after getting within obstacle distance) # When this loop is broken, we'll go to the next goal. We # don't need to cancel the goals here -- they will be # cancelled before a new one is sent. r = rospy.Rate( 10 ) xytt_old = None # Checks for movement stime = rospy.Time.now().to_sec() while True: if self.client.simple_state == SimpleGoalState.DONE: res = self.client.get_result() rospy.logout('snaking_explore: Movebase actionlib completed with result. Proceeding to next waypoint') break rim = self.robot_in_map() w.header.stamp = rospy.Time( 0 ) wim = self.ps_in_map( w ) # Proceed when close enough to goal (within two tolerance factors) if rim and wim: # only do this check if transform succeeds if self.range_to_waypoint( rim, wim ) < move_tol * 2.0: rospy.logout( 'snaking_explore: Close enough to waypoint. Proceeding to next waypoint' ) break # We're near the waypoint, so let's must move on to next # Proceed when there has been no movement (x,y, or theta) for X sec. if rim: # only do this check if transform succeeds xytt = self.xythetatime( rim ) if not xytt_old: xytt_old = xytt if np.abs( xytt[0] - xytt_old[0] ) > 0.05 or \ np.abs( xytt[1] - xytt_old[1] ) > 0.05 or \ np.abs( xytt[2] - xytt_old[2] ) > math.radians(10): xytt_old = xytt if xytt[3] - xytt_old[3] > no_progress: rospy.logout( 'snaking_explore: No movement in %2.1f seconds. Proceeding to next waypoint' % no_progress ) break if rospy.Time.now().to_sec() - stime > unreached_time: rospy.logout( 'snaking_explore: Goal unreached in %2.1f seconds. Proceeding to next waypoint' % unreached_time ) break r.sleep() self.client.cancel_all_goals() rospy.logout( 'snaking_explore: Finished with exploration.' ) return 'succeeded'
def load_targets_task(self): if not os.path.exists(self.nav_points_file): self.target_points = [] self.waypoints = list() with open(self.nav_points_file, "r") as nav_data_file: nav_data_str = nav_data_file.readline() self.target_points = [] while len(nav_data_str) != 0: pos_x = float(nav_data_str.split(" ")[0]) pos_y = float(nav_data_str.split(" ")[1]) pos_z = float(nav_data_str.split(" ")[2]) self.target_points.append([pos_x, pos_y, pos_z]) nav_data_str = nav_data_file.readline() self.waypoints = list() # nav_path_points = [] # with open("/home/xiaoqiang/slamdb/path.csv", "r") as nav_data_file: # nav_data_str = nav_data_file.readline() # while len(nav_data_str) != 0: # pos_x = float(nav_data_str.split(" ")[0]) # pos_y = float(nav_data_str.split(" ")[1]) # pos_z = float(nav_data_str.split(" ")[2]) # nav_path_points.append([pos_x, pos_y, pos_z]) # nav_data_str = nav_data_file.readline() # nav_path_points_2d = [[point[0], point[2]] # for point in nav_path_points] for point in self.target_points: pose_in_world = PoseStamped() pose_in_world.header.frame_id = "ORB_SLAM/World" pose_in_world.header.stamp = rospy.Time(0) pose_in_world.pose.position = Point(point[0], point[1], point[2]) # axis = self.get_target_direction( # [point[0], point[2]], nav_path_points_2d) # q_angle = quaternion_from_euler( # math.pi / 2, -math.atan2(axis[1], axis[0]), 0, axes='sxyz') q_angle = quaternion_from_euler( 0, 0, 0, axes='sxyz') pose_in_world.pose.orientation = Quaternion(*q_angle) # 转至map坐标系 rospy.loginfo("获取ORB_SLAM/World->TF map") tf_flag = False while not tf_flag and not rospy.is_shutdown() and self.running_flag: try: t = rospy.Time(0) self.listener.waitForTransform("ORB_SLAM/World", "map", t, rospy.Duration(1.0)) tf_flag = True except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf.Exception) as e: tf_flag = False rospy.logwarn("获取TF失败 ORB_SLAM/World->map") rospy.logwarn(e) if not self.running_flag: self.load_targets_exited_flag = True return rospy.loginfo("获取TF 成功 ORB_SLAM/World->map") waypoint = self.listener.transformPose( "map", pose_in_world) self.waypoints.append(waypoint) self.load_targets_exited_flag = True # 通过全局规划器,计算目标点的朝向 rospy.loginfo("waiting for move_base/make_plan service") rospy.wait_for_service("/move_base/make_plan") rospy.loginfo("waiting for move_base/make_plan service succeed") make_plan = rospy.ServiceProxy('/move_base/make_plan', GetPlan) for waypoint in self.waypoints: req = GetPlanRequest() req.start = self.waypoints[0] if waypoint == self.waypoints[0]: req.start = self.waypoints[1] req.goal = waypoint req.tolerance = 0.1 res = make_plan(req) if len(res.plan.poses) > 10: res.plan.poses = res.plan.poses[:-4] plan_path_2d = [[point.pose.position.x, point.pose.position.y] for point in res.plan.poses] angle = self.get_target_direction( [waypoint.pose.position.x, waypoint.pose.position.y], plan_path_2d) q_angle = quaternion_from_euler(0, 0, math.atan2( angle[1], angle[0]) + math.pi, axes='sxyz') waypoint.pose.orientation = Quaternion(*q_angle) rosparam.set_param("/galileo/goal_num", str(len(self.target_points)))
def load_targets_task(self): if not os.path.exists(self.nav_points_file): self.target_points = [] self.waypoints = list() with open(self.nav_points_file, "r") as nav_data_file: nav_data_str = nav_data_file.readline() self.target_points = [] while len(nav_data_str) != 0: pos_x = float(nav_data_str.split(" ")[0]) pos_y = float(nav_data_str.split(" ")[1]) pos_z = float(nav_data_str.split(" ")[2]) self.target_points.append([pos_x, pos_y, pos_z]) nav_data_str = nav_data_file.readline() self.waypoints = list() # nav_path_points = [] # with open("/home/xiaoqiang/slamdb/path.csv", "r") as nav_data_file: # nav_data_str = nav_data_file.readline() # while len(nav_data_str) != 0: # pos_x = float(nav_data_str.split(" ")[0]) # pos_y = float(nav_data_str.split(" ")[1]) # pos_z = float(nav_data_str.split(" ")[2]) # nav_path_points.append([pos_x, pos_y, pos_z]) # nav_data_str = nav_data_file.readline() # nav_path_points_2d = [[point[0], point[2]] # for point in nav_path_points] for point in self.target_points: pose_in_world = PoseStamped() pose_in_world.header.frame_id = "ORB_SLAM/World" pose_in_world.header.stamp = rospy.Time(0) pose_in_world.pose.position = Point(point[0], point[1], point[2]) # axis = self.get_target_direction( # [point[0], point[2]], nav_path_points_2d) # q_angle = quaternion_from_euler( # math.pi / 2, -math.atan2(axis[1], axis[0]), 0, axes='sxyz') q_angle = quaternion_from_euler(0, 0, 0, axes='sxyz') pose_in_world.pose.orientation = Quaternion(*q_angle) # 转至map坐标系 rospy.loginfo("获取ORB_SLAM/World->TF map") tf_flag = False while not tf_flag and not rospy.is_shutdown( ) and self.running_flag: try: t = rospy.Time(0) self.listener.waitForTransform("ORB_SLAM/World", "map", t, rospy.Duration(1.0)) tf_flag = True except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf.Exception) as e: tf_flag = False rospy.logwarn("获取TF失败 ORB_SLAM/World->map") rospy.logwarn(e) if not self.running_flag: self.load_targets_exited_flag = True return rospy.loginfo("获取TF 成功 ORB_SLAM/World->map") waypoint = self.listener.transformPose("map", pose_in_world) self.waypoints.append(waypoint) self.load_targets_exited_flag = True # 通过全局规划器,计算目标点的朝向 rospy.loginfo("waiting for move_base/make_plan service") rospy.wait_for_service("/move_base/make_plan") rospy.loginfo("waiting for move_base/make_plan service succeed") make_plan = rospy.ServiceProxy('/move_base/make_plan', GetPlan) for waypoint in self.waypoints: req = GetPlanRequest() req.start = self.waypoints[0] if waypoint == self.waypoints[0]: req.start = self.waypoints[1] req.goal = waypoint req.tolerance = 0.1 res = make_plan(req) if len(res.plan.poses) > 10: res.plan.poses = res.plan.poses[:-4] plan_path_2d = [[point.pose.position.x, point.pose.position.y] for point in res.plan.poses] angle = self.get_target_direction( [waypoint.pose.position.x, waypoint.pose.position.y], plan_path_2d) q_angle = quaternion_from_euler(0, 0, math.atan2(angle[1], angle[0]) + math.pi, axes='sxyz') waypoint.pose.orientation = Quaternion(*q_angle) rosparam.set_param("/galileo/goal_num", str(len(self.target_points)))
def goalCB(self, pose_msg): # if not self.is_working: # rospy.logwarn("can not receive tf between odom and map, navi function is disabled!") rospy.loginfo("receive goal pose, pre-process it") # cancel current goal self.cancelPub.publish(GoalID()) time.sleep(0.5) # clear current costmap self.clearCM.call(EmptyRequest()) time.sleep(0.5) # get robot pose in map frame robot_pose = self.pose_map # get the planned path before actual remap plan_req = GetPlanRequest() plan_req.start.header.frame_id = 'map' plan_req.start.header.stamp = rospy.Time.now() plan_req.start.pose = robot_pose.pose plan_goal = pose_msg # plan_goal = rospy.wait_for_message('/clicked_point', PoseStamped) plan_req.goal = plan_goal plan_req.tolerance = 0.1 try: plan_resp = self.planner.call(plan_req) except: rospy.logwarn("failed to make plan, remap goal") self.goalPub.publish(pose_msg) plan_path = plan_resp.plan plan_path.header.stamp = rospy.Time(0) plan_path.header.frame_id = '/map' self.planPub.publish(plan_path) # plan_path = GetPlanResponse() path_pose = plan_path.poses if len(path_pose) == 0: rospy.logwarn( "failed to find a possible path, remap it only cause the same result!" ) self.goalPub.publish(pose_msg) return dis_array = np.zeros(len(path_pose)) theta_array = np.zeros(len(path_pose)) for i in range(len(path_pose)): current_pose = path_pose[i] dis_array[i] = np.hypot( current_pose.pose.position.x - robot_pose.pose.position.x, current_pose.pose.position.y - robot_pose.pose.position.y) theta_array[i] = math.atan2( current_pose.pose.position.y - robot_pose.pose.position.y, current_pose.pose.position.x - robot_pose.pose.position.x) robot_heading = getYaw(robot_pose.pose.orientation) rospy.loginfo("robot pose-> x:%s, y:%s, theta :%s" % (robot_pose.pose.position.x, robot_pose.pose.position.y, rad2deg(robot_heading))) idx_cond_pre = np.where(dis_array < 0.5)[0] if len(idx_cond_pre) < 5: rospy.logwarn("probably too short to get path, remap it only!") self.goalPub.publish(pose_msg) return idx_cond = idx_cond_pre[-1] theta_sel = theta_array[2:idx_cond + 1] theta_average = np.mean(theta_sel) ang_diff = angdiff(theta_average, robot_heading) if np.abs(ang_diff) > 0.52: rospy.logwarn( "planned path at the back of robot, rotate it with angle diff %s" % rad2deg(ang_diff)) kp, ki, kd = [3.5, 1.0, 0.5] previous_error = 0 previous_integral = 0 theta_init = self.imu rat = rospy.Rate(10) t0 = rospy.get_time() ts = rospy.get_time() vel_msg = Twist() while 1: theta_current = self.imu error = angdiff(theta_init + ang_diff, theta_current) dt = rospy.get_time() - t0 if dt <= 0.0: dt = 0.01 if math.fabs(error) < 0.035: vel_msg.angular.z = 0.0 self.velPub.publish(vel_msg) break elif (rospy.get_time() - ts) > 15: vel_msg.angular.z = 0.0 self.velPub.publish(vel_msg) break wz, error, integral = PIDcontroller(kp, ki, kd, dt, 'PID', previous_error, error, previous_integral) wz = VelLim(wz, 0.05, 0.8) previous_error = error previous_integral = integral vel_msg.angular.z = wz self.velPub.publish(vel_msg) t0 = rospy.get_time() rat.sleep() else: rospy.logwarn( "planned path right in front of robot, no need to rotate") self.goalPub.publish(pose_msg) rospy.loginfo("pre-process finished! republish it to its actual topic")
def get_path(start, goal, tol=1): #Setting the counter for travel dist = 0 so that only the initial length is taken, the always updating path is taken care of later robot.counter = 0 charging_station = load_charging_station(0, 0) rospy.wait_for_service('/move_base/NavfnROS/make_plan') connection_service = rospy.ServiceProxy('/move_base/NavfnROS/make_plan', GetPlan) param = GetPlanRequest() param.start = start param.goal = goal param.tolerance = tol path = connection_service(param) robot.back_to_station = path_distance(path.plan, increment_gain=1) eng_path = calc_power_usage(robot.to_goal_initial, robot.back_to_station) threshold = 25 if (robot.current - ((100 * (eng_path[0][0] + eng_path[2])) / robot.power_max)) > threshold: print("-------------------------------------------------------------") print("New goal received") print("Initial distance to goal: {} m".format( round(robot.to_goal_initial, 3))) print("Distance from goal to charging station: {} m".format( round(robot.back_to_station, 3))) print("Estimated battery consumption for next goal: {} %".format( round(100 * eng_path[0][0] / robot.power_max, 2))) print( "Estimated battery consumption required for total operation: {}%". format(round(100 * eng_path[2] / robot.power_max, 2))) else: print("-------------------------------------------------------------") print("Battery threshold {} %".format(threshold)) print( "Estimated battery usage for next goal and return is {} %, current battery is at {} %" .format(((100 * (eng_path[0][0] + eng_path[2])) / robot.power_max), robot.current)) print("Battery too low for this goal, returning to station") pub.publish(charging_station)
import time #srv_name = '/move_base_node/NavfnROS/make_plan' srv_name = '/move_base_node/make_plan' rospy.init_node('trial_explore') listener = tf.TransformListener() listener.waitForTransform('/base_link', '/map', rospy.Time(0), timeout = rospy.Duration(100) ) get_plan = rospy.ServiceProxy( srv_name, GetPlan ) rospy.get_param('/move_base_node/global_costmap/raytrace_range') rospy.get_param('/move_base_node/TrajectoryPlannerROS/xy_goal_tolerance') req = GetPlanRequest() req.tolerance = 0.5 req.start = ut.robot_in_map( listener ) req.goal.header.stamp = rospy.Time(0) req.goal.header.frame_id = '/map' req.goal.pose.position.x = 0.85 #req.goal.pose.position.y = 0.85 req.goal.pose.orientation.w = 1.0 res = get_plan( req ) found = bool( res.plan.poses != [] ) print res print 'Found Path: ', found
def begin_explore(self, radius, preempt_func=retfalse): rospy.logout('snaking_explore: setting radius') waypoints = self.setup_poses(radius) # PoseStamped in self.frame local_planner = rospy.get_param('/move_base_node/base_local_planner' ) # : dwa_local_planner/DWAPlannerROS local_planner = local_planner[string.find(local_planner, '/') + 1:] obs_range = rospy.get_param( '/move_base_node/global_costmap/obstacle_range') move_tol = rospy.get_param('/move_base_node/' + local_planner + '/xy_goal_tolerance') no_progress = rospy.get_param(rospy.get_name() + '/no_progress_move', 5) unreached_time = rospy.get_param( rospy.get_name() + '/not_reached_move', 30) # I'm not entirely sure which service to use. I do know that # non-NavfnROS make_plan sometimes fails for an unknown # reason... and thus NavfnROS/make_plan is more robust. # srv_name = '/move_base_node/make_plan' srv_name = '/move_base_node/NavfnROS/make_plan' get_plan = rospy.ServiceProxy(srv_name, GetPlan) # Clear costmap...? Do this here or in smach...? if preempt_func(): # immediately exit if overall action preempted return 'preempted' for i, w in enumerate(waypoints): if preempt_func(): # immediately exit if overall action preempted return 'preempted' rospy.logout('snaking_explore: Seeking waypoint %d of %d' % (i + 1, len(waypoints))) # rospy.logout( 'snaking_explore: %2.2f %2.2f in frame %s' % (w.pose.position.x, w.pose.position.y, w.header.frame_id)) rim = self.robot_in_map() w.header.stamp = rospy.Time(0) # transform it _now_ wim = self.ps_in_map(w) # waypoint in map if not rim or not wim: # if transforms failed rospy.logout( 'snaking_explore: Bad transforms. Aborting explore') return 'aborted' if self.range_to_waypoint(rim, wim) < 0.9 * obs_range: # We're nearby the waypoint, so we'll just trust that the planner # has a good view of its surroundings to determine reachability. req = GetPlanRequest() req.tolerance = 0.1 req.start = rim req.goal = wim resp = get_plan(req) found_plan = bool(resp.plan.poses != []) if not found_plan: rospy.logout( 'snaking_explore: No plan to nearby waypoint. Proceeding to next waypoint' ) # Perhaps its worth pulling the waypoint closer until a path _is_ found? continue # We now know that a path exists. Send the waypoint to the client. rospy.logout( 'snaking_explore: Near by with good plan. Calling move_base action.' ) else: # Any nav plan at beyond obstacle range will require the # nav stack to get a better vantage. Send the waypoint to # the client. rospy.logout( 'snaking_explore: Far away. Calling move_base action.') # If we made it this far, it's time to call move_base action. # Cancel any previous goals self.client.cancel_goals_at_and_before_time(rospy.Time.now()) if os.environ.has_key('ROBOT') and os.environ['ROBOT'] == 'sim': rospy.logout( 'running in sim: give the system a little time to respond') rospy.sleep(1) # Send our new goal rospy.logout('snaking_explore: sending movebase action goal.') move_goal = MoveBaseGoal() move_goal.target_pose = wim self.client.send_goal(move_goal) if os.environ.has_key('ROBOT') and os.environ['ROBOT'] == 'sim': rospy.logout( 'running in sim: give the system a little time to respond') rospy.sleep(1) # We'll monitor the state ourselves, and allow ourselves # the opportunity to cancel the goal for various reasons # (eg. if we haven't moved for a while or if new plans # aren't found after getting within obstacle distance) # When this loop is broken, we'll go to the next goal. We # don't need to cancel the goals here -- they will be # cancelled before a new one is sent. r = rospy.Rate(10) xytt_old = None # Checks for movement stime = rospy.Time.now().to_sec() while True: if self.client.simple_state == SimpleGoalState.DONE: res = self.client.get_result() rospy.logout( 'snaking_explore: Movebase actionlib completed with result. Proceeding to next waypoint' ) break rim = self.robot_in_map() w.header.stamp = rospy.Time(0) wim = self.ps_in_map(w) # Proceed when close enough to goal (within two tolerance factors) if rim and wim: # only do this check if transform succeeds if self.range_to_waypoint(rim, wim) < move_tol * 2.0: rospy.logout( 'snaking_explore: Close enough to waypoint. Proceeding to next waypoint' ) break # We're near the waypoint, so let's must move on to next # Proceed when there has been no movement (x,y, or theta) for X sec. if rim: # only do this check if transform succeeds xytt = self.xythetatime(rim) if not xytt_old: xytt_old = xytt if np.abs( xytt[0] - xytt_old[0] ) > 0.05 or \ np.abs( xytt[1] - xytt_old[1] ) > 0.05 or \ np.abs( xytt[2] - xytt_old[2] ) > math.radians(10): xytt_old = xytt if xytt[3] - xytt_old[3] > no_progress: rospy.logout( 'snaking_explore: No movement in %2.1f seconds. Proceeding to next waypoint' % no_progress) break if rospy.Time.now().to_sec() - stime > unreached_time: rospy.logout( 'snaking_explore: Goal unreached in %2.1f seconds. Proceeding to next waypoint' % unreached_time) break r.sleep() self.client.cancel_all_goals() rospy.logout('snaking_explore: Finished with exploration.') return 'succeeded'
#srv_name = '/move_base_node/NavfnROS/make_plan' srv_name = '/move_base_node/make_plan' rospy.init_node('trial_explore') listener = tf.TransformListener() listener.waitForTransform('/base_link', '/map', rospy.Time(0), timeout=rospy.Duration(100)) get_plan = rospy.ServiceProxy(srv_name, GetPlan) rospy.get_param('/move_base_node/global_costmap/raytrace_range') rospy.get_param('/move_base_node/TrajectoryPlannerROS/xy_goal_tolerance') req = GetPlanRequest() req.tolerance = 0.5 req.start = ut.robot_in_map(listener) req.goal.header.stamp = rospy.Time(0) req.goal.header.frame_id = '/map' req.goal.pose.position.x = 0.85 #req.goal.pose.position.y = 0.85 req.goal.pose.orientation.w = 1.0 res = get_plan(req) found = bool(res.plan.poses != []) print res print 'Found Path: ', found for i in xrange(100):