def main():
rospy.init_node('map_annotator_navigation')
wait_for_time()
global nav_manager
global nav_manager_pub
global server
nav_manager = WebNavigationManager()
nav_manager.loadPoses()
# Sleeping so publisher has time to get started
rospy.sleep(rospy.Duration.from_sec(2))
server = InteractiveMarkerServer("simple_marker")
for poseName in nav_manager.poses:
marker = PoseMarker(server, poseName, nav_manager.poses[poseName])
nav_manager_pub = rospy.Publisher('pose_names',
PoseNames,
latch=True,
queue_size=10)
user_action_sub = rospy.Subscriber('/user_actions', UserAction,
process_command)
# Publish the pre-existing markers
msg = PoseNames()
msg.names = nav_manager.listPoses()
nav_manager_pub.publish(msg)
rospy.spin()
def main():
global pose_marker_server
global nav_server
global target_pose_pub
global pose_names_pub
rospy.init_node('web_teleop_navigation')
wait_for_time()
pose_marker_server = InteractiveMarkerServer('simple_marker')
pose_names_pub = rospy.Publisher('/pose_names',
PoseNames,
queue_size=10,
latch=True)
nav_server = NavigationServer()
# Create
nav_server.loadMarkers()
for name in nav_server.pose_names_list:
marker = PoseMarker(pose_marker_server, name,
nav_server.pose_names_list[name])
message = PoseNames()
message.names = nav_server.pose_names_list
pose_names_pub.publish(message)
user_actions_sub = rospy.Subscriber('/user_actions', UserAction,
handle_user_actions)
rospy.spin()
def main():
global pose_marker_server
global nav_server
global target_pose_pub
global pose_names_pub
global current_pose
current_pose = geometry_msgs.msg.Pose(
orientation=geometry_msgs.msg.Quaternion(0, 0, 0, 1))
rospy.init_node('action_node')
wait_for_time()
pose_marker_server = InteractiveMarkerServer('simple_marker')
pose_names_pub = rospy.Publisher('/pose_names',
PoseNames,
queue_size=10,
latch=True)
nav_server = NavigationServer()
# Create
nav_server.loadMarkers()
arm_server = ArmServer()
message = PoseNames()
message.names = nav_server.pose_names_list
pose_names_pub.publish(message)
reader = ArTagReader()
sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, reader.callback)
controller_client = actionlib.SimpleActionClient(
'/query_controller_states', QueryControllerStatesAction)
rospy.sleep(0.5)
goal = QueryControllerStatesGoal()
state = ControllerState()
state.name = 'arm_controller/follow_joint_trajectory'
state.state = ControllerState.RUNNING
goal.updates.append(state)
controller_client.send_goal(goal)
print 'Waiting for arm to start.'
controller_client.wait_for_result()
print 'Arm has been started.'
gripper = fetch_api.Gripper()
arm = fetch_api.Arm()
print 'Arm and gripper instantiated.'
# handle user actions
user_actions_sub = rospy.Subscriber('/user_actions', UserAction,
handle_user_actions)
arm_service = rospy.Service('barbot/arm_to_pose', ArmToPose,
arm_server.findGlass)
rospy.spin()
def handle_user_actions(message):
name = message.name
if message.command == 'create':
new_pose_marker = PoseMarker(pose_marker_server, name, None)
nav_server.saveMarker(name, new_pose_marker.pose)
message = PoseNames()
message.names = nav_server.pose_names_list
pose_names_pub.publish(message)
elif message.command == 'goto':
if name in nav_server.pose_names_list:
nav_server.goToMarker(name)
else:
print 'No such pose \'{}\''.format(name)
elif message.command == 'delete':
if name in nav_server.pose_names_list:
nav_server.deleteMarker(name)
message = PoseNames()
message.names = nav_server.pose_names_list
pose_names_pub.publish(message)
pose_marker_server.erase(name)
pose_marker_server.applyChanges()
else:
pass
def main():
global map_list_data
rospy.init_node('navigation_republisher')
wait_for_time()
pose_names_pub = rospy.Publisher('/pose_names',
PoseNames,
queue_size=10,
latch=True)
rospy.sleep(1)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
try:
map_list_data = pickle.load(open(PICKLE_FILE, "rb"))
except:
print 'Pickle File Empty'
map_list_data = {}
pickle.dump(map_list_data, open(PICKLE_FILE, "wb"))
message = PoseNames()
message.names = map_list_data
pose_names_pub.publish(message)
rate.sleep()
def process_command(data):
# We update names whenever the command changes them
if data.command == data.CREATE:
marker = PoseMarker(server, data.name)
nav_manager.savePose(data.name, marker.pose)
msg = PoseNames()
msg.names = nav_manager.listPoses()
nav_manager_pub.publish(msg)
elif data.command == data.DELETE:
server.erase(data.name)
nav_manager.deletePose(data.name)
msg = PoseNames()
msg.names = nav_manager.listPoses()
nav_manager_pub.publish(msg)
server.applyChanges(
) # rviz reflects changes, applyChanges() called in create calls
elif data.command == data.GOTO:
nav_manager.goto(data.name)
else:
print "nothing done, command: ", data.command
pass
def handle_shutdown():
pn = PoseNames()
list_pub.publish(pn)
database.save()
def _publish_poses(self):
pn = PoseNames()
pn.names.extend(self._db.list())
self._list_pub.publish(pn)
def __pub_pose_info__(self):
pose_names = PoseNames()
pose_names.names = self._annotator.get_position_names()
pose_names.names.sort()
self._pose_names_pub.publish(pose_names)
def handle_shutdown():
pn = PoseNames()
pose_pub.publish(pn)
def pub_pose(self):
pose_names = PoseNames()
pose_names.names.extend(self.annotator.list())
self.pose_pub.publish(pose_names)
def _publish_pose_names(self):
pose_names = PoseNames()
pose_names.names = [key for key in self._pose_ctrl.poses]
self._pub.publish(pose_names)
def publish_pose_names(poses):
pose_names = PoseNames()
pose_names.poses = poses.keys()
pose_names_pub.publish(pose_names)