def get_action(self):
mba = MoveBaseAction()
ag_ = MoveBaseActionGoal()
ag_.header = Header()
ag_.header.frame_id = "map"
ag_.header.stamp = rospy.Time.now()
ag_.goal = MoveBaseGoal()
ag_.goal.target_pose = self.target_waypoint
mba.action_goal = ag_
return mba
class Charge(object):
ac = MoveBaseAction()
anchor_x = 0.4
anchor_y = 0.0
anchor_theta = 0.0
def adjust(self, mode=127):
rospy.wait_for_service('/pose_adjust/adjust_service')
rospy.wait_for_service('Track')
try:
PoseAdjustClient = rospy.ServiceProxy(
"/pose_adjust/adjust_service", SetArPose)
trackClient = rospy.ServiceProxy("Track", SetCharge)
tresp = trackClient(0)
if tresp.success == False:
return False
resp = PoseAdjustClient(True)
tresp = trackClient(127)
if tresp.success == 0:
return False
return resp.success
except rospy.ServiceException, e:
print "adjust() err"
trackClient = rospy.ServiceProxy("Track", SetCharge)
tresp = trackClient(127)
return False
def __goto_waypoint__(self, coordinates):
"""
Send the goal given by coordinates to the move_base node
"""
rospy.loginfo('Goto waypoint: {}'.format(coordinates))
# Generate a action message
goal = MoveBaseAction()
goal.action_goal.goal.target_pose.header.stamp = rospy.Time.now()
goal.action_goal.goal.target_pose.header.frame_id = 'map'
goal.action_goal.goal.target_pose.pose.position.x = coordinates[0]
goal.action_goal.goal.target_pose.pose.position.y = coordinates[1]
# Get the quaternion from the orientation in degree
yaw = coordinates[2] * math.pi / 360.0
goal.action_goal.goal.target_pose.pose.orientation.z = math.sin(yaw)
goal.action_goal.goal.target_pose.pose.orientation.w = math.cos(yaw)
self.current_target = coordinates
self.command_start = rospy.Time.now().to_sec()
# Send the waypoint
self.navigation_client.send_goal(goal.action_goal.goal, self.__done_callback__, \
self.__active_callback__, self.__feedback_callback__)
def go_directly(self, start, target=None):
if self.move_base is None:
self.move_base = actionlib.SimpleActionClient(
"move_base", MoveBaseAction)
self.move_base.wait_for_server()
if target is None:
target = start
start = None
else:
start_coords = self.map_server.waypoint_to_coords(start)
self.gazebo.set_turtlebot_position(start_coords["x"],
start_coords["y"], 0)
target_coords = self.map_server.waypoint_to_coords(target)
goal = MoveBaseAction()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
quarternion = t.quarternion_from_euler(0, 0, 0)
goal.target_pose.pose = Pose(
Point(target_coords.x, target_coords.y, 0),
Quaternion(quarternion[0], quarternion[1], quarternion[2],
quarternion[4]))
self.gazebo.set_charging_srv(False)
self.move_base.send_goal(goal)
result = self.move_base.wait_for_result()
state = self.move_base.get_state()
if result and state == GoalStatus.SUCCEEDED:
return True
else:
return False
def __init__(self):
rospy.init_node('send_goal')
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.mygoal = []
self.points_num = rospy.get_param('~points_num', 0)
rospy.loginfo('points num: %d' % self.points_num)
for i in range(self.points_num):
self.mygoal.append(
rospy.get_param('~goal_points/point_%d' % i, None))
self.client.wait_for_server()
self.goal = MoveBaseAction()
self.goal.action_goal.goal.target_pose.pose.position.x = self.mygoal[
0][0]
self.goal.action_goal.goal.target_pose.pose.position.y = self.mygoal[
0][1]
self.goal.action_goal.goal.target_pose.pose.position.z = self.mygoal[
0][2]
self.goal.action_goal.goal.target_pose.pose.orientation.x = self.mygoal[
0][3]
self.goal.action_goal.goal.target_pose.pose.orientation.y = self.mygoal[
0][4]
self.goal.action_goal.goal.target_pose.pose.orientation.z = self.mygoal[
0][5]
self.goal.action_goal.goal.target_pose.pose.orientation.w = self.mygoal[
0][6]
for next_goal in filter(lambda x: x != None, self.mygoal):
self.goal.action_goal.goal.target_pose.header.frame_id = '/map'
self.goal.action_goal.goal.target_pose.header.stamp = rospy.Time.now(
)
self.goal.action_goal.goal.target_pose.pose.position.x = next_goal[
0]
self.goal.action_goal.goal.target_pose.pose.position.y = next_goal[
1]
self.goal.action_goal.goal.target_pose.pose.position.z = next_goal[
2]
self.goal.action_goal.goal.target_pose.pose.orientation.x = next_goal[
3]
self.goal.action_goal.goal.target_pose.pose.orientation.y = next_goal[
4]
self.goal.action_goal.goal.target_pose.pose.orientation.z = next_goal[
5]
self.goal.action_goal.goal.target_pose.pose.orientation.w = next_goal[
6]
self.client.send_goal_and_wait(self.goal.action_goal.goal)
state = self.client.get_state()
while state != actionlib.GoalStatus.SUCCEEDED and not rospy.is_shutdown(
):
self.client.send_goal_and_wait(self.goal.action_goal.goal)
state = self.client.get_state()
rospy.loginfo('the state is: %s' % state)
def posePublisher(
): #Convert absolute waypoint to vector relative to robot, then publish navigation goal to ROS
global currPosX, currPosY, currPosZ, debug0
# desiredPose = PoseStamped()
# desiredPose.header.frame_id = "/gps"
# desiredPose.header.stamp = rospy.Time.now()
desiredPose = MoveBaseAction()
goal.target_pose.header.frame_id = "base_link"
goal.target_pose.header.stamp = rospy.get_rostime()
if debug:
rospy.loginfo("LatWP: %f, LonWP: %f, LatCur: %f, LonCur: %f", latWP,
lonWP, latCur, lonCur)
distToWP = haversineDistance(latCur, lonCur, latWP, lonWP)
bearingToWP = bearing(latCur, lonCur, latWP, lonWP)
# TODO: Use the correct margin of error
if (distToWP < 3): # If there is less than 3 meters left
rospy.wait_for_service('next_waypoint_srv')
next_waypoint = rospy.ServiceProxy('next_waypoint_srv', Trigger)
try:
resp = next_waypoint()
except rospy.ServiceException as exc:
rospy.loginfo("Next waypoint service failed")
goal.target_pose.position.x = currPosX + (
distToWP * cos(bearingToWP)
) #Convert distance and angle to waypoint from Polar to Cartesian co-ordinates then add current position of robot odometry
goal.target_pose.position.y = currPosY + (distToWP * sin(bearingToWP))
goal.target_pose.position.z = altWP - currPosZ #Assuming CurrPosZ is abslolute (eg barometer or GPS)
goal.target_pose.orientation.x = 0
goal.target_pose.orientation.y = 0
goal.target_pose.orientation.z = 0
goal.target_pose.orientation.w = 1
action_client.send_goal(goal)
action_client.wait_for_result()
# navGoalPub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10) #Publish Nav Goal to ROS topic
# navGoalPub.publish(desiredPose)
if (action_client.get_result()):
rospy.loginfo(
"GPS Fix is Valid! Setting Navigation Goal to: %f, %f, %f",
goal.target_pose.position.x, goal.target_pose.position.y,
goal.target_pose.position.z)
rospy.loginfo("Robot is heading %f metres at a bearing of %f degrees",
distToWP, (bearingToWP * 180 / pi + 360) % 360)
def robotGo(self,target_goal):
self.client.wait_for_server()
goal=MoveBaseAction()
goal.action_goal.goal.target_pose.header.frame_id='/map'
goal.action_goal.goal.target_pose.header.stamp=rospy.Time.now()
goal.action_goal.goal.target_pose.pose.position.x=target_goal[0]
goal.action_goal.goal.target_pose.pose.position.y=target_goal[1]
goal.action_goal.goal.target_pose.pose.position.z=target_goal[2]
goal.action_goal.goal.target_pose.pose.orientation.x=target_goal[3]
goal.action_goal.goal.target_pose.pose.orientation.y=target_goal[4]
goal.action_goal.goal.target_pose.pose.orientation.z=target_goal[5]
goal.action_goal.goal.target_pose.pose.orientation.w=target_goal[6]
self.client.send_goal_and_wait(goal.action_goal.goal)
state=self.client.get_state()
while state != actionlib.GoalStatus.SUCCEEDED and not rospy.is_shutdown():
self.client.send_goal_and_wait(goal.action_goal.goal)
state=self.client.get_state()
rospy.loginfo('the state is: %s'%state)