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 _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 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)))
#! /usr/bin/env python
import rospy
from nav_msgs.srv import GetPlan, GetPlanRequest
import sys
rospy.init_node('service_client')
rospy.wait_for_service('/move_base/make_plan')
make_plan_service = rospy.ServiceProxy('/move_base/make_plan', GetPlan)
msg = GetPlanRequest()
msg.start.header.frame_id = 'map'
msg.start.pose.position.x = 0
msg.start.pose.position.y = 0
msg.start.pose.position.z = 0
msg.start.pose.orientation.x = 0
msg.start.pose.orientation.y = 0
msg.start.pose.orientation.z = 0
msg.start.pose.orientation.w = 0
msg.goal.header.frame_id = 'map'
msg.goal.pose.position.x = 1
msg.goal.pose.position.y = 2
msg.goal.pose.position.z = 0
msg.goal.pose.orientation.x = 0
msg.goal.pose.orientation.y = 0
msg.goal.pose.orientation.z = 0
msg.goal.pose.orientation.w = 0
result = make_plan_service(msg)
print result
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 util as ut
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'
def tracking(self, event):
if not self.normal:
return
st = rospy.Time.now()
#rospy.loginfo("tracking loop")
if self.target_global is None:
rospy.logerr("%s : no goal" % rospy.get_name())
return
else:
#rospy.loginfo("%s :have seen goal" % rospy.get_name())
pass
#print("fuckkckckc why???")
#print(self.target_global)
end_p = self.transform_pose(self.target_global.pose, self.bot_frame,
self.map_frame)
#end_p = self.target_global
#end_p.header.frame_id = self.map_frame
if end_p is None:
return
end_p.header.frame_id = self.bot_frame
start_p = PoseStamped()
start_p.pose.position.x = 0
start_p.pose.position.y = 0
start_p.pose.position.z = 0
start_p.header.frame_id = self.bot_frame
#start_p = self.transform_pose(start_p.pose, self.map_frame, self.bot_frame)
if start_p is None:
return
req_path = GetPlanRequest()
req_path.start = start_p
req_path.goal = end_p
oe = end_p
for i in range(self.search_max):
try:
res_path = self.req_path_srv(req_path)
self.last_plan = res_path
#rospy.loginfo("%s : plan success" % rospy.get_name())
break
except:
rospy.logerr("%s : path request fail" % rospy.get_name())
if self.last_plan is None:
return
res_path = self.last_plan
r = np.linalg.norm([oe.pose.position.x, oe.pose.position.y])
theta = math.atan2(oe.pose.position.y, oe.pose.position.x)
theta += (-1)**(i) * (i + 1) * self.search_angle
end_p.pose.position.x = r * math.cos(theta)
end_p.pose.position.y = r * math.sin(theta)
self.pub_target_marker.publish(self.to_marker(end_p, [0, 0, 1]))
self.pursuit.set_path(res_path.plan)
goal = self.pursuit.get_goal(start_p)
if goal is None:
rospy.logwarn("goal reached, to next goal")
self.target_global = None
self.set_lane(True)
return
# print("trace", goal)
goal = self.transform_pose(goal.pose, self.map_frame, self.bot_frame)
goal.header.frame_id = self.map_frame
self.pub_goal_marker.publish(self.to_marker(goal))
self.pub_pid_goal.publish(goal)
et = rospy.Time.now()
#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
float64 w
"""
import rospy
from nav_msgs.srv import GetPlan, GetPlanRequest
rospy.init_node("make_plan")
print("Waiting for /move_base/make_plan service")
rospy.wait_for_service('/move_base/make_plan')
try:
makePlanClient = rospy.ServiceProxy('/move_base/make_plan', GetPlan)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
req = GetPlanRequest()
# Start
req.start.header.frame_id = 'map'
req.start.pose.orientation.z = 0.7071
req.start.pose.orientation.w = 0.7071
# goal
req.goal.header.frame_id = 'map'
req.goal.pose.position.x = 1.0
req.goal.pose.position.y = 1.0
req.goal.pose.orientation.z = 0.7071
req.goal.pose.orientation.w = 0.7071
print(req)