def setGoal(self, goal):
if self._current_goal is not None:
self._new_goal = utils.pose_to_pose2D(goal.pose)
#cancle possible previous goal
self.cancel()
else:
self._current_goal = utils.pose_to_pose2D(goal.pose)
def plan(self, goal, p_time, until_first_solution):
self._stepQueue.clear()
self._start_left = StepTarget()
self._start_left.leg = strings.left
start_pose_l = self.getFootPoseFromTf(strings.l_sole_frame,
strings.map_frame)
self._start_left.pose = utils.pose_to_pose2D(start_pose_l)
self._start_right = StepTarget()
self._start_right.leg = strings.right
start_pose_r = self.getFootPoseFromTf(strings.r_sole_frame,
strings.map_frame)
self._start_right.pose = utils.pose_to_pose2D(start_pose_r)
self._goal_left = StepTarget()
self._goal_left.leg = strings.left
self._goal_left.pose = self.getFootFromRobotPose(goal, strings.left)
self._goal_right = StepTarget()
self._goal_right.leg = strings.right
self._goal_right.pose = self.getFootFromRobotPose(goal, strings.right)
#msg = PlanFootstepsBetweenFeet()
#msg.start_left = self._start_left
#msg.start_right = self._start_right
#msg.goal_left = self._goal_left
#msg.goal_right = self._goal_right
#msg.planning_time = p_time
#msg.until_first_solution = True
try:
resp = self._pathSrvFeet(self._start_left, self._start_right,
self._goal_left, self._goal_right, p_time,
until_first_solution, self._eps)
#resp = self._pathSrvFeet(msg)p_time
except rospy.ServiceException as exc:
print("Service did not process request: " + str(exc))
return Error.SERVICE_ERR
if resp.result == True:
rospy.loginfo(
"Planning succeeded with %d steps, path costs: %f, eps: %f" %
(len(resp.footsteps), resp.costs, resp.final_eps))
self._eps = resp.final_eps
self._stepQueue.addSteps(resp.footsteps)
return Error.OK
else:
rospy.logerr("Service call failed")
return Error.PLANER_ERR
def transformSteps(self, step, frame_id=strings.map_frame):
ret = copy.deepcopy(step)
step_pose = PoseStamped()
step_pose.header.frame_id = frame_id
step_pose.pose = utils.pose2D_to_pose(step.pose)
pose_transformed = self.delay_transform(step_pose, strings.odom_frame)
if pose_transformed is None:
return None
ret.pose = utils.pose_to_pose2D(pose_transformed.pose)
return ret
def execute(self, goal):
self._current_goal = utils.pose_to_pose2D(goal.target_pose.pose)
self._canceled = False
status = self.handleGoal()
results = False
if status == Error.OK:
results = True
rospy.loginfo('Robot reached destination')
elif status == Error.CANCELED:
self._server.set_preempted()
rospy.logerr("Goal cancelled.")
else:
rospy.logerr("Robot did not reach goal")
self.clear()
self._server.set_succeeded(results)
def hasFinished(self):
if self._current_goal is None:
return True
if not self._footstepHandler.hasSteps():
return True
pose = utils.pose_to_pose2D(self._robotPose)
x = abs(pose.x - self._current_goal.x)
print("x distance: %f" % x)
if x > self._acc_x:
return False
y = abs(pose.y - self._current_goal.y)
if y > self._acc_y:
return False
theta = abs(self.anglediff2(pose.theta, self._current_goal.theta))
if theta > self._acc_theta:
return False
return True
rospy.Subscriber(strings.pose_topic, PoseWithCovarianceStamped, pose_callback)
rospy.Subscriber(strings.goal_topic, PoseStamped, goal_callback)
pathPubl = rospy.Publisher(strings.pub_path_topic, Path, queue_size=10)
goal_pose = Pose()
goal_pose.position.x= 1
goal_pose.position.y= 0
goal_pose.position.z= 0.0
goal_pose.orientation.x=0
goal_pose.orientation.y=0
goal_pose.orientation.z=-0.0154884964884
goal_pose.orientation.w=0.999880046044
goal = utils.pose_to_pose2D(goal_pose)
time.sleep(3)
footstepHandler.setGoal(goal)
#hasFInished = False
rate = rospy.Rate(2)
while not rospy.is_shutdown():
if footstepHandler.askPath():
steps = footstepHandler.getSteps(maxSteps)
publish_steps(steps)
if steps is not None:
pass
#footstepHandler.sendSteps(steps)