def send_move_base_goal(posx, posy, rotationz):
global last_goal_id
ag = MoveBaseActionGoal()
ag.header = Header()
ag.goal_id = GoalID()
ID = "0"
if(not last_goal_id == None):
print "Last goal id:"
print last_goal_id
IDint = int(float(last_goal_id.id))+1
ID = str(IDint)
print "ID:"
print ID
print "\n"
#ag.header.seq = ID
ag.goal_id.id = ID
print ag.goal_id
ag.goal = MoveBaseGoal()
ag.goal.target_pose.header.frame_id = "map"
ag.goal.target_pose.pose.position.x = posx
ag.goal.target_pose.pose.position.y = posy
dummy = tf.transformations.quaternion_from_euler(0, 0, rotationz)
ag.goal.target_pose.pose.orientation.x = dummy[0]
ag.goal.target_pose.pose.orientation.y = dummy[1]
ag.goal.target_pose.pose.orientation.z = dummy[2]
ag.goal.target_pose.pose.orientation.w = dummy[3]
print ag
pub_goal.publish(ag)
last_goal_id = ag.goal_id
pub_log.publish(rospy.get_name()+": published ActionGoal")
def publishNextPoint(self):
x_next, y_next = self.convert_node_to_point(self.next_node)
w_next = self.computeNextOrientation(x_next, y_next)
rospy.loginfo(
"[%s] Converted next node to point: (%s,%s), orientation: %s" %
(self.node_name, x_next, y_next, w_next))
goal_id = GoalID()
goal_id.id = str(self.goal_idx)
msg = MoveBaseActionGoal()
msg.goal_id = goal_id
msg.goal.target_pose.header.frame_id = self.frame_id
msg.goal.target_pose.pose.position.x = x_next
msg.goal.target_pose.pose.position.y = y_next
msg.goal.target_pose.pose.position.z = 0
msg.goal.target_pose.pose.orientation.x = 0
msg.goal.target_pose.pose.orientation.y = 0
msg.goal.target_pose.pose.orientation.z = math.sin(w_next / 2)
msg.goal.target_pose.pose.orientation.w = math.cos(w_next / 2)
self.pub_goal.publish(msg)
rospy.loginfo("[%s] Dispatched goal point: (%s,%s)" %
(self.node_name, x_next, y_next))
if self.next_node == self.goal:
self.dispatched_end = True
def get_goal_to_publish():
# Get a random goal and go there
# We could try to improve this random search, and have some memory
global latest_goal_time
global latest_goal_id
global goal_points_list
if latest_goal_id == None:
latest_goal_id = 1
latest_goal_time = time.time()
else:
if time.time() - latest_goal_time > 20.:
latest_goal_id += 1
latest_goal_time = time.time()
else:
# Print should not happen anymore
print "Too little time passed since last goal sent"
return None
goal_to_send = MoveBaseActionGoal()
current_goal_id = GoalID()
current_goal_id.id = str(latest_goal_id)
goal_to_send.goal_id = current_goal_id
pose_stamped = PoseStamped()
pose = Pose()
idx = random.randrange(len(goal_points_list))
pose.position.x = goal_points_list[idx][0]
pose.position.y = goal_points_list[idx][1]
roll = 0.
pitch = 0.
yaw = 0.
quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
pose.orientation.x = quaternion[0]
pose.orientation.y = quaternion[1]
pose.orientation.z = quaternion[2]
pose.orientation.w = quaternion[3]
pose_stamped.pose = pose
goal = MoveBaseGoal()
goal.target_pose = pose_stamped
goal.target_pose.header.frame_id = 'map'
goal_to_send.goal = goal
return goal_to_send
def go_to_location(self, location, frame_id, timeout=50.0):
'''
description: move the robot to the desired location
input: string location, the name of the location that you want the robot to go
double timeout, max time to allow to reach the location before returning
string frame_id, frame where the pose is located
NOTE: the location needs to be loaded in the parameter server in advance, to get a list
of availbale locations use mbot.get_available_locations() function
output: bool success, whether if the robot was succesful or not at reaching the goal
'''
timeout_ros = rospy.Duration(timeout)
time_start = rospy.Time.now()
try:
location_pose = self.get_available_poses()[location]
except KeyError:
rospy.logerr(
'{} is not a valid waypoint from this list: {}'.format(
location, self.get_available_poses()))
return None
goal = PoseStamped()
if not isinstance(frame_id, str):
frame_id = 'odom'
goal.header.frame_id = frame_id
goal.header.stamp = rospy.Time.now()
quaternion = tf.transformations.quaternion_from_euler(
0, 0, location_pose[2])
goal.pose.orientation.x = quaternion[0]
goal.pose.orientation.y = quaternion[1]
goal.pose.orientation.z = quaternion[2]
goal.pose.orientation.w = quaternion[3]
goal.pose.position.x = location_pose[0]
goal.pose.position.y = location_pose[1]
goal.pose.position.z = 0
final_goal = MoveBaseActionGoal()
final_goal.header = goal.header
final_goal.goal_id.stamp = rospy.Time.now()
final_goal.goal_id.id = str(random())
goal_id = final_goal.goal_id
final_goal.goal.target_pose = goal
self.__move_base_safe_pub.publish(final_goal)
rospy.loginfo("Sending goal to move_base_simple, destination : " +
location)
while not rospy.is_shutdown(
) and rospy.Time.now() - time_start < timeout_ros:
if self.__goals_status:
for i in self.__goals_status:
if i.goal_id == goal_id and i.status == 3:
return True
return False
def set_destination(self, destination):
print destination['latitude']
print destination['longitude']
msg = MoveBaseActionGoal()
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = frame_id
msg.sentence = sentence
self.pub.publish(msg)
return "a"
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
def go_to_pose(self, location_pose, frame_id, back_up=[1, 1], timeout=50):
'''
description: move the robot to the desired pose
needs: mir_move_base_safe and mbot_actions move_base_server
input: PoseStamped location_pose, x, y, yaw
double timeout, max time to allow to reach the location before returning
string frame_id, frame where the pose is located
output: bool success, whether if the robot was succesful or not at reaching the goal
'''
timeout_ros = rospy.Duration(timeout)
time_start = rospy.Time.now()
goal = PoseStamped()
if not isinstance(frame_id, str):
frame_id = 'odom'
goal.header.frame_id = frame_id
goal.header.stamp = rospy.Time.now()
quaternion = tf.transformations.quaternion_from_euler(
0, 0, location_pose[2])
goal.pose.orientation.x = quaternion[0]
goal.pose.orientation.y = quaternion[1]
goal.pose.orientation.z = quaternion[2]
goal.pose.orientation.w = quaternion[3]
goal.pose.position.x = location_pose[0]
goal.pose.position.y = location_pose[1]
goal.pose.position.z = 0
final_goal = MoveBaseActionGoal()
final_goal.header = goal.header
final_goal.goal_id.stamp = rospy.Time.now()
final_goal.goal_id.id = str(random())
goal_id = final_goal.goal_id
final_goal.goal.target_pose = goal
self.__move_base_safe_pub.publish(final_goal)
rospy.loginfo("Sending goal to move_base_simple, destination : (" +
str(location_pose[0]) + "," + str(location_pose[1]) +
") with yaw: " + str(location_pose[2]))
while not rospy.is_shutdown(
) and rospy.Time.now() - time_start < timeout_ros:
if self.__goals_status:
for i in self.__goals_status:
if i.goal_id == goal_id and i.status == 3:
return True
if i.goal_id == goal_id and i.status == 4:
return self.go_to_pose([
location_pose[0] + back_up[0],
location_pose[1] + back_up[1], location_pose[2]
],
frame_id=frame_id,
back_up=back_up,
timeout=timeout)
return False
def generate_goal():
global goal
pose_stamped = PoseStamped()
pose_stamped.pose = None # no goal at start
move_base_goal = MoveBaseGoal()
move_base_goal.target_pose = pose_stamped
goal = MoveBaseActionGoal()
goal.goal = move_base_goal
def goalcallback(self, data):
#we'll send a goal to the robot to move 3 meters forward
goal = MoveBaseGoal()
# goal.target_pose.header.frame_id = 'base_link'
# goal.target_pose.header.stamp = rospy.Time.now()
# goal.target_pose.pose.position.x = 0.2
# goal.target_pose.pose.orientation.w = 1.0
goal.target_pose = data
#start moving
rospy.logerr(goal)
# self.move_base.send_goal(goal)
#TODO check status of route, if on another route, cancel it first
# self.cancelMovement()
t = rospy.Time.now()
#draws the path
req = GetPlanRequest()
req.start = self.robot_pose_stamped
req.start.header.stamp = t
req.goal = data
req.goal.header.stamp = t
req.tolerance = .02
# return_path = self.get_path_to_goal_srv(req)
# self.next_path_pub.publish(return_path)
self.move_to_goal_count += 1
g = MoveBaseActionGoal();
t = rospy.Time.now()
g.header = Header(self.move_to_goal_count, t, "map")
g.goal_id.stamp = t
g.goal_id.id = "movement_num:"+str(self.move_to_goal_count)
g.goal.target_pose = data
# newnppose = vector3_to_numpy(data.pose.position)
# if np.linalg.norm(newnppose - self.lastnppose) >.05 :
# self.goal_pose_stamped_pub.publish(data)
self.move_to_goal_pub.publish(g)
rospy.logerr("published move_base goal")
def __init__(self):
self.cob_marker_goal = DetectObjectsGoal()
self.MovePTP_goal = MoveLinGoal()
self.MoveStart_goal = MoveLinGoal()
self.MoveBaseHome_goal = MoveBaseActionGoal()
self.MoveBaseShelf_goal = MoveBaseActionGoal()
# protected region initCode on begin #
genpy.message.fill_message_args(self.MovePTP_goal, [rospy.get_param('/simple_pnp/MovePTPGoal')])
genpy.message.fill_message_args(self.MoveStart_goal, [rospy.get_param('/simple_pnp/MoveStartGoal')])
genpy.message.fill_message_args(self.MoveBaseHome_goal, [rospy.get_param('/simple_pnp/MoveBaseHomeGoal')])
genpy.message.fill_message_args(self.MoveBaseShelf_goal, [rospy.get_param('/simple_pnp/MoveBaseShelfGoal')])
genpy.message.fill_message_args(self.cob_marker_goal, [rospy.get_param('/simple_pnp/CobMarkerGoal')])
# protected region initCode end #
pass
def __init__(self):
self.goal_id = 0
print("reached?")
self.marker = Marker()
self.marker.type = Marker.SPHERE
self.marker.action = Marker.ADD
self.marker.scale.x = 10
self.marker.scale.y = 10
self.marker.scale.z = 10
self.marker.color.a = 1
self.marker.pose.orientation.w = 1.0
self.marker.pose.position.x = 2
self.marker.pose.position.y = 2
self.marker.pose.position.z = 2
self.marker.header.frame_id = '/map'
self.goal_message = MoveBaseActionGoal()
rospy.init_node("goal_generator", anonymous=False)
rospy.loginfo("Goal Generator initiated")
# rospy.on_shutdown(self.shutdown)
rospy.Subscriber("/move_base/result", MoveBaseActionResult,
self.goalCallback)
self.rbt_goal = rospy.Publisher('/move_base/goal',
MoveBaseActionGoal,
queue_size=2)
self.rbt_goal.publish(self.goal_message)
self.vis_goal = rospy.Publisher('/visualization_marker',
Marker,
queue_size=2)
self.vis_goal.publish(self.marker)
rospy.loginfo(self.marker)
rospy.spin()
def set_goal(x,y): ac_goal = MoveBaseActionGoal() ac_goal.goal.target_pose.header.frame_id = "map" ac_goal.goal.target_pose.pose.orientation.w = 1.0 ac_goal.goal.target_pose.pose.position.x = x ac_goal.goal.target_pose.pose.position.y = y pub2.publish(ac_goal)
def __init__(self):
rospy.init_node('MultiGoalListen', anonymous=False)
rospy.on_shutdown(self.shutdown)
self.r = redis.Redis(host="127.0.0.1", port=6379, db=0)
if os.path.exists("/home/eaibot/dashgo_ws/src/dashgo/dashgo_nav/maps/current_pose.json")==False:
open("/home/eaibot/dashgo_ws/src/dashgo/dashgo_nav/maps/current_pose.json","w")
rospy.Subscriber('/robot_pose', Pose, self.robot_pose_callback)
self.r.set("RosServiceState", 1)
self.goal_pub = rospy.Publisher('/move_base/goal', MoveBaseActionGoal, queue_size=5)
self.goal_cancel = rospy.Publisher('/move_base/cancel', GoalID, queue_size=5)
rospy.Subscriber('/move_base/result', MoveBaseActionResult, self.status_callback)
rospy.Subscriber('/multi_cancel_goal', Int16, self.cancel_callback)
rospy.Subscriber('/multi_publish_goal', Int16, self.publish_callback)
attr_dict = {
"mode": "loop",
"loopWay": "auto",
"isNavNext": 0,
"currentState": "stopped",
"goalQueue": "GoalQueueA",
"currentGoal": "",
"successNum": "0",
"failedNum": "0",
"intervalTime": "3",
}
self.r.hmset("GoalState", attr_dict)
self.goal = MoveBaseActionGoal()
self.isContinue=False
self.isNavStop=False
self.lastGoalName=""
self.failRun=True
rospy.spin()
def husky_mover():
pub = rospy.Publisher('/move_base/goal', MoveBaseActionGoal, queue_size=0)
rospy.init_node('husky_mover')
myGoal = MoveBaseActionGoal()
targetPos = PoseStamped()
rospy.sleep(1)
rospy.loginfo("Setting the robot goal.")
myGoal.goal.target_pose.header.frame_id = "odom"
myGoal.goal.target_pose.header.stamp = rospy.Time.now()
myGoal.goal.target_pose.pose.position.x = 9
myGoal.goal.target_pose.pose.position.y = 3
q = tf.transformations.quaternion_from_euler(0, 0, math.pi)
myGoal.goal.target_pose.pose.orientation.x = q[0]
myGoal.goal.target_pose.pose.orientation.y = q[1]
myGoal.goal.target_pose.pose.orientation.z = q[2]
myGoal.goal.target_pose.pose.orientation.w = q[3]
pub.publish(myGoal)
rospy.loginfo("Node exiting.")
rospy.sleep(30);
rospy.loginfo("Setting the robot goal.")
myGoal.goal.target_pose.header.frame_id = "odom";
myGoal.goal.target_pose.header.stamp = rospy.Time.now();
myGoal.goal.target_pose.pose.position.x = 1;
pub.publish(myGoal)
rospy.loginfo("Node exiting.")
def __init__(self, area, sc, task_id):
self.area = area
self.sc = sc
self.task_id = task_id
self.CM0 = Marker2D("AreaScanMarker/obstacle_marker",
1.0,
0.0,
0.0,
task_id=self.task_id)
self.CM1 = Marker2D("AreaScanMarker/area_marker",
0.0,
0.0,
1.0,
marker_type=4,
task_id=self.task_id)
self.CM2 = Marker2D("AreaScanMarker/vec_marker",
0.0,
1.0,
0.0,
marker_type=0,
task_id=self.task_id)
self.x_list = np.zeros(0)
self.y_list = np.zeros(0)
# pos = [position.x, position.y, angle]
self.pos = np.array([0, 0, 0], dtype='float')
self.angle_list = np.zeros(0)
self.listener = tf.TransformListener()
self.trans = 0
self.rot = 0
self.status = GoalStatusArray()
self.goal = MoveBaseActionGoal()
self.map = OccupancyGrid()
self.reshaped_map = np.zeros(0, dtype='int')
def add_waypoint(self, goal_with_priority, add_after_current = False, need_tf = False): # GoalWithPriority
goal_id = self.new_goal_id()
pose = goal_with_priority.pose
goal_with_priority.goal_id = goal_id
move_base_msg = MoveBaseActionGoal()
move_base_msg.header.frame_id = 'odom'
move_base_msg.goal.target_pose.header.frame_id = 'odom'
move_base_msg.goal.target_pose.pose = pose
move_base_msg.goal_id = goal_id
goal = GoalManager.GoalWrapper(move_base_msg, goal_with_priority, need_tf = need_tf)
goal.goal_with_priority = goal_with_priority
if add_after_current:
self.goals.insert(self.current_idx + 1, goal)
else:
self.goals.append(goal)
rospy.loginfo("Received goal:\n\tx:%f\n\ty:%f" % (pose.position.x, pose.position.y))
def move():
# Starts a new node
rospy.init_node('plat_move', anonymous=True)
velocity_publisher = rospy.Publisher('/move_base/goal',
MoveBaseActionGoal,
queue_size=10)
cmd = MoveBaseActionGoal()
#rate = rospy.Rate(0.0636) # 0.1hz
#Receiveing the user's input
print("Let's move your marslite lol")
while not rospy.is_shutdown():
rospy.sleep(2.)
raw_input("Press Enter to continue...")
print("outside button")
cmd.goal.target_pose.header.frame_id = 'map'
cmd.goal.target_pose.pose.position.x = 1.054
cmd.goal.target_pose.pose.position.y = -0.045
cmd.goal.target_pose.pose.orientation.z = 0.917
cmd.goal.target_pose.pose.orientation.w = -0.398
velocity_publisher.publish(cmd)
rospy.sleep(2.)
raw_input("Press Enter to continue...")
print("outside door")
cmd.goal.target_pose.header.frame_id = 'map'
cmd.goal.target_pose.pose.position.x = 1.075
cmd.goal.target_pose.pose.position.y = 0.199
cmd.goal.target_pose.pose.orientation.z = -0.368
cmd.goal.target_pose.pose.orientation.w = 0.930
velocity_publisher.publish(cmd)
def set_new_pos():
global srv_client_user_interface_assignment_, previous_state_
#ask to user to insert the input
print(
"Insert a new position between (-4,-3) (-4,2) (-4,7) (5,-7) (5,-3) (5,1)"
)
x = float(raw_input('x :'))
y = float(raw_input('y :'))
#check the input
if ((x == -4 and (y == 2 or y == -3 or y == 7))
or (x == 5 and (y == 1 or y == -3 or y == -7))):
#if input is valid
#set the input values in parameters for desired position
rospy.set_param("des_pos_x", x)
rospy.set_param("des_pos_y", y)
#public a message MovebaseActionGoal() in topic /move_base/goal
msg_goal = MoveBaseActionGoal()
msg_goal.goal.target_pose.header.frame_id = "map"
msg_goal.goal.target_pose.pose.orientation.w = 1
#set in the position the desired ones
msg_goal.goal.target_pose.pose.position.x = rospy.get_param(
'des_pos_x')
msg_goal.goal.target_pose.pose.position.y = rospy.get_param(
'des_pos_y')
pub2.publish(msg_goal)
print("goal: x=" + str(rospy.get_param('des_pos_x')) + " y=" +
str(rospy.get_param('des_pos_y')))
resp = srv_client_user_interface_assignment_()
else:
#if input is not valid tell it to user
print("NOT VALID INPUT")
print('please insert another position')
check_status()
return []
def compute_initial_pose(self):
self.initial_pose = rospy.wait_for_message('/pose', PoseStamped)
rospy.loginfo("The default position is: {}".format(self.initial_pose))
# Trasfrom from PoseStamped to MoveBaseActionGoal.
move_base = MoveBaseGoal(self.initial_pose)
move_base_action = MoveBaseActionGoal()
move_base_action.goal = move_base
move_base_action.goal.target_pose.header.frame_id = "map"
move_base_action.header.stamp.secs = self.initial_pose.header.stamp.secs
move_base_action.header.stamp.nsecs = self.initial_pose.header.stamp.nsecs
move_base_action.header.frame_id = "map"
self.goal = move_base_action
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
# The following code is just for illustrating how the behavior logger works.
# Text logged by the behavior logger is sent to the operator and displayed in the GUI.
##time_to_wait = rospy.Time.now() - self._start_time - self._target_time
##if time_to_wait > 0:
##Logger.loginfo('Need to wait for %.1f seconds.' % time_to_wait)
goal = MoveBaseActionGoal()
goal.header.stamp = Time.now()
goal.header.frame_id = userdata.frameId
goal.goal.target_pose.pose.position = userdata.pose.position
goal.goal.distance = userdata.params['distance']
# "straighten up" given orientation
yaw = euler_from_quaternion([userdata.pose.orientation.x, userdata.pose.orientation.y, userdata.pose.orientation.z, self.pose.orientation.w])[2]
goal.goal.target_pose.pose.orientation.x = 0
goal.goal.target_pose.pose.orientation.y = 0
goal.goal.target_pose.pose.orientation.z = math.sin(yaw/2)
goal.goal.target_pose.pose.orientation.w = math.cos(yaw/2)
goal.goal.target_pose.header.frame_id = userdata.frameId
goal.goal_id.id = 'driveTo' + str(goal.header.stamp.secs) + '.' + str(goal.header.stamp.nsecs)
goal.goal_id.stamp = goal.header.stamp
userdata.goalId = goal.goal_id.id
self._pub.publish(self._topic_move_base_goal, goal)
return 'succeeded'
def callback_simple_goal(self, goal):
rospy.logdebug('[Move Base Fake] Simple Goal: {}'.format(
str(goal.pose.position)))
action_goal = MoveBaseActionGoal()
action_goal.header.stamp = rospy.Time.now()
action_goal.goal.target_pose = goal
self.goal_pub.publish(action_goal)
def goTo(name):
# TODO: Implement this.
global navPending
global needsCancel
global callback
navPending = True
if name == 'lower torso':
torso = fetch_api.Torso()
torso.set_height(0.0)
navPending = False
return 0
elif name == 'raise torso':
torso = fetch_api.Torso()
torso.set_height(0.4)
navPending = False
return 0
else:
if not os.path.exists('pickle/' + name):
print 'No such pose \'' + name + '\''
navPending = False
return 1
else:
loadfile = open('pickle/' + name, 'rb')
stampedCoPose = pickle.load(loadfile)
loadfile.close()
# TODO: Figure out why this goal doesn't cause any motion
mbagoal = MoveBaseActionGoal()
mbgoal = MoveBaseGoal()
mbgoal.target_pose.header.frame_id = 'map'
mbgoal.target_pose.header.stamp = rospy.Time().now()
mbgoal.target_pose.pose = stampedCoPose.pose #potential issue here
mbagoal.goal = mbgoal
#mbagoal.header =
#mbagoal.goal_id.id = 'ios'
pub.publish(mbagoal)
#pub.send_goal(mbgoal)
# END TODO
rospy.sleep(5)
while callback.motion:
print 'waiting'
rospy.sleep(1)
navPending = False
callback.motion = True
return 0
navPending = False
return 1
def move_to_goal(self, goal):
mbag = MoveBaseActionGoal()
# First Header
mbag.header.frame_id = '' # Use the map frame to define goal poses
mbag.header.seq = self.goal_id
mbag.header.stamp = rospy.Time.now()
# Now the Goal ID
mbag.goal_id.stamp = rospy.Time.now()
mbag.goal_id.id = "g_" + str(self.goal_id)
# Now the goal, which we already have as an argument
mbag.goal = goal
#PoseStamped(Pose(Point(2.0, -3.0, 0.0), quaternion_from_euler(0, 0, -1.5706, axes='sxyz')))
self.goal_setter.publish(mbag)
def driveTo(x,y, theta):
rospy.loginfo("Drive to (%f, %f, %f)", x, y, theta)
newPose = generate_pose(x, y, 0)
#print newPose
actionGoal = MoveBaseActionGoal()
actionGoal.header = genHeader()
actionGoal.goal_id.id = str(driveTo.goalID)
actionGoal.goal_id.stamp = rospy.Time.now()
goal = MoveBaseGoal()
goal.target_pose = newPose
actionGoal.goal = goal
# Publish the goal to the robot
global actionGoalPublisher
actionGoalPublisher.publish(actionGoal)
global soundClient
# Wait for the robot's status to to have reached the goal
timeOutCounter = 0
while not rospy.is_shutdown(): # This is done so that status can be checked and used
rospy.sleep(4.)
timeOutCounter += 1
currentStatus = getStatus(driveTo.goalID)
global cant_reach_list
print "Status: %d, GoalID: %d, Driving to: (%f, %f, %f), # unreachable: %d" % (currentStatus, driveTo.goalID, x, y, theta, len(cant_reach_list ))
if currentStatus == GoalStatus.ABORTED or timeOutCounter > 20:
soundClient.say("Abort driving to goal.")
print "The goal was aborted"
cant_reach_list.append((x, y))
break
elif currentStatus == GoalStatus.REJECTED:
soundClient.say("Goal rejected")
print "The goal was rejected"
break
elif currentStatus == GoalStatus.LOST:
soundClient.say("Robot is lost")
print "The robot is lost, exiting driving"
#TODO Should we send a cancel message?
exit(1)
break
elif currentStatus == GoalStatus.SUCCEEDED:
soundClient.say("Goal reached. Moving on.")
print "Drive to complete!"
break
driveTo.goalID += 1
def move(self, x, y, a):
goal = MoveBaseActionGoal()
goal.goal.target_pose.header.frame_id = 'odom'
goal.goal.target_pose.pose.position.x = x
goal.goal.target_pose.pose.position.y = y
orient = tf.transformations.quaternion_from_euler(0, 0, a)
goal.goal.target_pose.pose.orientation.z = orient[2]
goal.goal.target_pose.pose.orientation.w = orient[3]
self.pub_goal.publish(goal)
def colour_move(self, x, z):
colour_found = MoveBaseActionGoal()
colour_found.goal.target_pose.pose.position.x = x
colour_found.goal.target_pose.pose.orientation.z = z
colour_found.goal.target_pose.pose.orientation.w = 1
colour_found.goal.target_pose.header.stamp = rospy.Time.now()
colour_found.goal.target_pose.header.frame_id = 'base_link'
colour_found.goal.target_pose.header.seq = 101
self.movePub.publish(colour_found)
def move(self, x, y):
co_move = MoveBaseActionGoal()
co_move.goal.target_pose.pose.position.x = x
co_move.goal.target_pose.pose.position.y = y
co_move.goal.target_pose.pose.orientation.w = 1
co_move.goal.target_pose.header.stamp = rospy.Time.now()
co_move.goal.target_pose.header.frame_id = 'map'
co_move.goal.target_pose.header.seq = 102
self.movePub.publish(co_move)
def goalcallback(self, data):
#we'll send a goal to the robot to move 3 meters forward
goal = MoveBaseGoal()
# goal.target_pose.header.frame_id = 'base_link'
# goal.target_pose.header.stamp = rospy.Time.now()
# goal.target_pose.pose.position.x = 0.2
# goal.target_pose.pose.orientation.w = 1.0
goal.target_pose = data
#start moving
rospy.logerr(goal)
# self.move_base.send_goal(goal)
#TODO check status of route, if on another route, cancel it first
# self.cancelMovement()
t = rospy.Time.now()
#draws the path
req = GetPlanRequest()
req.start = self.robot_pose_stamped
req.start.header.stamp = t
req.goal = data
req.goal.header.stamp = t
req.tolerance = .02
# return_path = self.get_path_to_goal_srv(req)
# self.next_path_pub.publish(return_path)
self.move_to_goal_count += 1
g = MoveBaseActionGoal()
t = rospy.Time.now()
g.header = Header(self.move_to_goal_count, t, "map")
g.goal_id.stamp = t
g.goal_id.id = "movement_num:" + str(self.move_to_goal_count)
g.goal.target_pose = data
# newnppose = vector3_to_numpy(data.pose.position)
# if np.linalg.norm(newnppose - self.lastnppose) >.05 :
# self.goal_pose_stamped_pub.publish(data)
self.move_to_goal_pub.publish(g)
rospy.logerr("published move_base goal")
def talker(self,data):
numPath = len(data.poses)
for i in range(numPath):
pub_msg = MoveBaseActionGoal()
pub_msg.goal.target_pose = data.poses[i]
pub_msg.header = data.header
pub_msg.goal.target_pose.header.frame_id = 'map'
pub_msg.header.frame_id = 'map'
# pub_msg.goal.target_pose.header.frame_id = data.poses[i].header.frame_id
pub_msg.goal.target_pose.pose.position.x = data.poses[i].pose.position.x-2
pub_msg.goal.target_pose.pose.position.y = data.poses[i].pose.position.y-2
pub_msg.goal.target_pose.pose.position.z = data.poses[i].pose.position.z
# pub_msg.goal.target_pose.pose.orientation = data.poses[i].pose.orientation
if(self.RecFlag):
self.pub.publish(pub_msg)
rospy.sleep(2)
print(pub_msg)
# flag=True
self.RecFlag=False
def __init__(self, parameters):
self.parameters = parameters
self.ssne_param = self.parameters.ssne_param
self.num_input = self.ssne_param.num_input
self.num_hidden = self.ssne_param.num_hnodes
self.num_output = self.ssne_param.num_output
if self.parameters.arch_type == 'QUASI_GRUMB':
self.ssne = mod.Quasi_GRUMB_SSNE(parameters)
else:
self.ssne = mod.Fast_SSNE(parameters) #nitialize SSNE engine
#####Initiate the agent
self.agent = Agent_Pop(parameters, 0)
#ROS stuff
self.oracle_x = [[-1, -1, -1], [-1, -1, 1], [-1, 1, -1], [-1, 1, 1],
[1, -1, -1], [1, -1, 1], [1, 1, -1], [1, 1, 1]]
self.oracle_y = [[3.348, 0.955], [0.550, 1.051], [0.546, 8.904],
[3.299, 8.929], [6.583, 9.0120], [9.087, 8.966],
[9.316, 0.897], [6.569, 0.926]]
self.oracle_goals = self.format_goal(self.oracle_y)
self.home_coordinates = [[5.0, 4.0]]
self.home_goal = self.format_goal(self.home_coordinates)
rospy.init_node('Evolve_GRUMB', anonymous=True) # Node
self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.update_scan)
# self.cmd_sub = rospy.Subscriber('/navigation_velocity_smoother/raw_cmd_vel', Twist, self.update_command)
self.goal_reached_sub = rospy.Subscriber('/move_base/status',
GoalStatusArray,
self.update_goal_status)
self.odom_sub = rospy.Subscriber('/base_pose_ground_truth', Odometry,
self.update_odom)
self.goal_pub = rospy.Publisher('/move_base/goal',
MoveBaseActionGoal,
queue_size=1)
self.goal_pub.publish(MoveBaseActionGoal())
time.sleep(0.3) # Dummy hack
self.pub = rospy.Publisher(
'/navigation_velocity_smoother/raw_cmd_vel', Twist,
queue_size=10) # Publisher to command velocity
# Control Action defined and initialized
self.action = Twist()
self.action.linear.x = 0.0
self.action.angular.z = 0.0
self.action.linear.y = 0.0
# Define max applicable to robot
self.max_speed = 1.0
self.is_pursuing = False
self.pursue_status = 1
self.scan_readings = None
self.base_odom = Pose()
def slam_move(pose):
goal = MoveBaseActionGoal()
goal.goal.target_pose.header.frame_id = 'map'
goal.goal.target_pose.pose.position.x = pose[0]
goal.goal.target_pose.pose.position.y = pose[1]
goal.goal.target_pose.pose.position.z = 0.0
goal.goal.target_pose.pose.orientation.x = 0
goal.goal.target_pose.pose.orientation.y = 0
goal.goal.target_pose.pose.orientation.z = math.sin(pose[2] / 2)
goal.goal.target_pose.pose.orientation.w = math.cos(pose[2] / 2)
slam_move_pub.publish(goal)
def pubGoal(x, y):
pub_msg = MoveBaseActionGoal()
# pub_msg.header = data.header
pub_msg.goal.target_pose.header.frame_id = 'map'
pub_msg.header.frame_id = 'map'
# pub_msg.goal.target_pose.header.frame_id = data.poses[i].header.frame_id
pub_msg.goal.target_pose.pose.position.x = x
pub_msg.goal.target_pose.pose.position.y = y
pub_msg.goal.target_pose.pose.position.z = 0
print(pub_msg)
pub.publish(pub_msg)
def set_target():
global pub_target,desired_position_
Goal = MoveBaseActionGoal()
Goal.goal.target_pose.header.frame_id = "map"
Goal.goal.target_pose.pose.orientation.w =1
Goal.goal.target_pose.pose.position.x = rospy.get_param("des_pos_x")
Goal.goal.target_pose.pose.position.y = rospy.get_param("des_pos_y")
pub_target.publish(Goal)
print("Published new target!")
desired_position_.x = rospy.get_param("des_pos_x")
desired_position_.y = rospy.get_param("des_pos_y")
def get_goals(self, file):
# Read in the next goal pose. Set it up, return to send goal method.
#FORMAT:
#x, y;
run_identfier = time.time()
goal_list = []
clockwise = True if run_identfier % 2 == 0 else False
run_identfier = str(
run_identfier) + "_clock" if clockwise else "_counterclock"
with open(file, 'r') as goals_file:
goals_reader = csv.reader(goals_file, delimiter=',')
# for i in range(5):
for row_num, row in enumerate(goals_reader):
MBAG = MoveBaseActionGoal()
MBAG.goal_id = GoalID()
MBAG.goal_id.id = str(run_identfier) + "_path_" + str(row_num)
MBAG.header = Header()
MBAG.header.frame_id = 'map'
MBG = MoveBaseGoal()
MBG.target_pose.header.frame_id = "map"
MBG.target_pose.pose.position.x = float(row[0])
MBG.target_pose.pose.position.y = float(row[1])
MBG.target_pose.pose.orientation.w = 1.0
MBAG.goal = MBG
goal_list.append(MBAG)
#Switch directions half the time
if clockwise:
temp = goal_list[0]
goal_list[0] = goal_list[2]
goal_list[2] = temp
rospy.loginfo("Pathing clockwise: " + str(clockwise))
return goal_list
def publishNextPoint(self):
x_next, y_next = self.convert_node_to_point(self.next_node)
w_next = self.computeNextOrientation(x_next,y_next)
rospy.loginfo("[%s] Converted next node to point: (%s,%s), orientation: %s" %(self.node_name, x_next, y_next, w_next))
goal_id = GoalID()
goal_id.id = str(self.goal_idx)
msg = MoveBaseActionGoal()
msg.goal_id = goal_id
msg.goal.target_pose.header.frame_id = self.frame_id
msg.goal.target_pose.pose.position.x = x_next
msg.goal.target_pose.pose.position.y = y_next
msg.goal.target_pose.pose.position.z = 0
msg.goal.target_pose.pose.orientation.x = 0
msg.goal.target_pose.pose.orientation.y = 0
msg.goal.target_pose.pose.orientation.z = math.sin(w_next/2)
msg.goal.target_pose.pose.orientation.w = math.cos(w_next/2)
self.pub_goal.publish(msg)
rospy.loginfo("[%s] Dispatched goal point: (%s,%s)" %(self.node_name,x_next,y_next))
if self.next_node == self.goal:
self.dispatched_end = True
def sendToOnce():
global PUB_GOAL, PUBD
# if PUBD:
# return
# PUBD = True
DISTANCE_THRESHOLD = .1
print'********'
# x = data.pose.pose.position.x
# y = data.pose.pose.position.y
# if math.sqrt(x**2+y**2) < DISTANCE_THRESHOLD:
# return
ps = PoseStamped()
ps.header.frame_id = "/map"
ps.header.stamp = rospy.Time.now()
ps.pose.position.x = 1
ps.pose.position.y = 0
ps.pose.position.z = 0
q = quaternion_from_euler(0, 0, 0, 'sxyz')
ps.pose.orientation.x = q[0]
ps.pose.orientation.y = q[1]
ps.pose.orientation.z = q[2]
ps.pose.orientation.w = q[3]
mbag = MoveBaseActionGoal()
mbag.header = ps.header
mbag.goal.target_pose = ps
print mbag
PUB_GOAL.publish(mbag)
def publish_goal(): global centroids global finished global goal_counter global failed global full_fail global last_goal global last_none_flag global goal global init init= PoseStamped() init.pose.position.x=0 init.pose.position.y=0 init.pose.position.z=0 init.pose.orientation.x=0 init.pose.orientation.y=0 init.pose.orientation.z=1 goal_counter += 1 # print "dajkhfjajlfjdlka" if len(centroids) > 0: print "Publishing goal %s" % goal_counter # get the first centroid first = centroids.pop(0) # check if it is too close dist = math.sqrt((first.pose.position.x - newPose.pose.position.x)**2 + (first.pose.position.y - newPose.pose.position.y)**2) # if last_goal != None: # last_none_flag = 0 # last_dist = 999 # if not last_none_flag: # last_dist = math.sqrt((last_goal.pose.position.x - newPose.pose.position.x)**2 + (last_goal.pose.position.y - newPose.pose.position.y)**2) # create a goal goal = MoveBaseActionGoal() goal.header = first.header goal.goal_id.id = "Goal%s" % goal_counter goal.goal.target_pose = first goal_pub.publish(goal) last_goal = first # else: # print "Goal too close" # # goal_counter -= 1 # if len(centroids) > 0: # publish_goal() # elif full_fail > 3: # finished = 1 # else: # full_fail += 1 # run_navigation(False) else: # there are no reachable centroids if failed: finished = 1 goal = MoveBaseActionGoal() goal.goal_id.id = "Goal%s" % goal_counter goal.goal.target_pose = init goal_pub.publish(goal) else: failed = 1 goal = MoveBaseActionGoal() goal.goal_id.id = "Goal%s" % goal_counter goal.goal.target_pose = init goal_pub.publish(goal) run_navigation(False)
