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):
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 __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 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 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 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 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 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 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 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 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 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 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 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 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 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 __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 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 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 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 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 main():
global pub,yaw_,yaw_error_allowed_ ,position_,desired_position_ ,target_pos_lists
rospy.init_node('target_controller')
pub = rospy.Publisher('/move_base/goal', MoveBaseActionGoal, queue_size=5)
sub_odom = rospy.Subscriber('/gmapping_odom', Twist, clbk_odom)
# sub_odom = rospy.Subscriber('/gmapping_odom', Twist, target_maker)
# check_set_new_pos()
Goal = MoveBaseActionGoal()
Goal.goal.target_pose.header.frame_id = "map"
Goal.goal.target_pose.pose.orientation.w =1
rate = rospy.Rate(0.1)
count = 0
while not rospy.is_shutdown():
Goal.header.stamp = rospy.get_time()
if count == 0:
count += 1
random_choice = random.randrange(6)
Goal.goal.target_pose.pose.position.x = target_pos_lists[random_choice][0]
Goal.goal.target_pose.pose.position.y = target_pos_lists[random_choice][1]
pub.publish(Goal)
desired_position_.x = Goal.goal.target_pose.pose.position.x
desired_position_.y = Goal.goal.target_pose.pose.position.y
print("Random target generated! x = " +
str(desired_position_.x) + ", y = " + str(desired_position_.y))
else:
dis_error = math.sqrt(pow((position_.x - desired_position_.x),2) + pow((position_.x - desired_position_.x),2))
yaw_error = abs(1-yaw_)
if dis_error < 0.1 and yaw_error < yaw_error_allowed_:
count += 1
print("Target reached! Please insert a new position")
check_feasible_target = False
while not check_feasible_target:
x = float(raw_input('x :'))
y = float(raw_input('y :'))
for i in range(6):
if x == target_pos_lists[i][0] and y == target_pos_lists[i][1]:
check_feasible_target = True
Goal.goal.target_pose.pose.position.x = x
Goal.goal.target_pose.pose.position.y = y
pub.publish(Goal)
desired_position_.x = Goal.goal.target_pose.pose.position.x
desired_position_.y = Goal.goal.target_pose.pose.position.y
break
if not check_feasible_target:
print("Sorry, the request position is not feasible...")
print("Thanks! Let's reach the next position")
# x = desired_position_.x
# y = desired_position_.y
# print("Hi! We are reaching the first position: x = " +
# str(x) + ", y = " + str(y))
# print("now positon: x = " +
# str(position_.x) + ", y = " + str(position_.y))
rate.sleep()
def pub_goal():
#publish a MoveBaseActionGoal() message on 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;
#take from parameters the position that must be reached
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');
pub.publish(msg_goal)
print("\n goal: x=" + str(rospy.get_param('des_pos_x')) + " y=" + str(rospy.get_param('des_pos_y')))
return []
def setMoveBaseGoal():
global MB_target_pub
#set the goal for move_base alg
move_base_goal = MoveBaseActionGoal()
move_base_goal.goal.target_pose.header.frame_id = "map"
move_base_goal.goal.target_pose.pose.orientation.w = 1
move_base_goal.goal.target_pose.pose.position.x = rospy.get_param("/target_x")
move_base_goal.goal.target_pose.pose.position.y = rospy.get_param("/target_y")
#publish the msg
MB_target_pub.publish(move_base_goal)
def format_goal(self, vals):
goals = []
for val in vals:
y = MoveBaseActionGoal()
y.goal.target_pose.header.frame_id = 'map'
y.goal.target_pose.pose.orientation.x = 0.0
y.goal.target_pose.pose.orientation.y = 0.0
y.goal.target_pose.pose.orientation.z = 0.0
y.goal.target_pose.pose.orientation.w = 1.0
y.goal.target_pose.pose.position.x = val[0]
y.goal.target_pose.pose.position.y = val[1]
# print y
goals.append(y)
return goals
def __init__(self,loc_goal):
self.loc_goal = loc_goal
# Starts a new node
self.velocity_publisher = rospy.Publisher('/move_base/goal', MoveBaseActionGoal, queue_size=10)
self.cmd = MoveBaseActionGoal()
#rate = rospy.Rate(0.0636) # 0.1hz
#Receiveing the user's input
print("Let's move your marslite lol")
t = threading.Thread(target = self.__motions)
t.start()
def goal_from_landmark(landmark):
action_goal = MoveBaseActionGoal()
action_goal.goal_id.id = landmark.name
goal = PoseStamped()
goal.header.frame_id = 'map'
goal.pose = Pose()
goal.pose.position.x = landmark.x
goal.pose.position.y = landmark.y
goal.pose.position.z = 0
goal.pose.orientation.w = 1
goal.pose.orientation = rotate_quaternion(goal.pose.orientation,
math.radians(landmark.angle))
action_goal.goal.target_pose = goal
return action_goal
