def main(): rospy.init_node('gripper_teleop') arm = Arm() gripper = Gripper() im_server = InteractiveMarkerServer('gripper_im_server') auto_pick_im_server = InteractiveMarkerServer('auto_pick_im_server') teleop = GripperTeleop(arm, gripper, im_server) auto_pick = AutoPickTeleop(arm, gripper, auto_pick_im_server) teleop.start() auto_pick.start() rospy.spin()
def main(): rospy.init_node('gripper_teleop') wait_for_time() arm = fetch_api.Arm() gripper = fetch_api.Gripper() im_server = InteractiveMarkerServer('gripper_im_server', q_size=10) teleop = GripperTeleop(arm, gripper, im_server) teleop.start() pp_im_server = InteractiveMarkerServer('pick_place_im_server', q_size=10) pick_place = PickPlaceTeleop(arm, gripper, pp_im_server) pick_place.start() rospy.spin()
def __init__(self): # Public attributes if World.tf_listener is None: World.tf_listener = TransformListener() self.surface = None # Private attributes self._lock = threading.Lock() self._tf_broadcaster = TransformBroadcaster() self._im_server = InteractiveMarkerServer(TOPIC_IM_SERVER) rospy.wait_for_service(SERVICE_BB) self._bb_service = rospy.ServiceProxy( SERVICE_BB, FindClusterBoundingBox) self._object_action_client = actionlib.SimpleActionClient( ACTION_OBJ_DETECTION, UserCommandAction) self._object_action_client.wait_for_server() rospy.loginfo( 'Interactive object detection action server has responded.') # Setup other ROS machinery rospy.Subscriber( TOPIC_OBJ_RECOGNITION, GraspableObjectList, self.receive_object_info) rospy.Subscriber(TOPIC_TABLE_SEG, Marker, self.receive_table_marker) # Init self.clear_all_objects()
def __init__(self): self.goto_pub = rospy.Publisher('/move_base_simple/goal', geometry_msgs.msg.PoseStamped) self.pose_pub = rospy.Publisher('/pose_names', map_annotator.msg.PoseNames, latch=True) rospy.Subscriber('/amcl_pose', geometry_msgs.msg.PoseWithCovarianceStamped, self._handle_pose_callback) self.names = {} filename = "/home/team3/catkin_ws/src/cse481c/map_annotator/simstations.pickle" #filename = rospy.get_param('~pose_file') try: with open(filename, "r") as f: self.names = pickle.load(f) except EOFError: self.names = {} except IOError: self.names = {} self.pose = None self.server = InteractiveMarkerServer('simple_marker') self.name_markers = {} self.name_controls = {} pose_message = map_annotator.msg.PoseNames() pose_message.poses = list(self.names.keys()) self.pose_pub.publish(pose_message) print("initiated actuator nodes with", self.names, "from", filename)
def __init__(self, rate=30.0, filename=None): # Marker server self.server = InteractiveMarkerServer('camera_marker') # TF broadcaster self.tf = TransformBroadcaster() # Marker pose self.pose_mutex = Lock() self.marker_position = (0.0, 0.0, 0.0) self.marker_orientation = (0.0, 0.0, 0.0, 1.0) # Load init position self.filename = filename if self.filename: with open(self.filename, 'r') as stream: init_data = yaml.load(stream)['init_position'] self.marker_position = (init_data['x'], init_data['y'], init_data['z']) self.marker_orientation = (0.0, 0.0, 0.0, 1.0) # Register shutdown callback rospy.on_shutdown(self.shutdown) # Add marker self.add_6DOF() # Timer for TF broadcaster rospy.Timer(rospy.Duration(1.0 / rate), self.publish_transform)
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(): rospy.init_node('map_annotator') server = InteractiveMarkerServer("simple_server") pose_ctrl = PoseController() marker_ctrl = MarkerController(server, pose_ctrl) monitor = MapAnnotatorMonitor(pose_ctrl, marker_ctrl) rospy.spin()
def __init__(self): self.server = InteractiveMarkerServer("posestamped_im") self.menu_handler = MenuHandler() # self.pub = rospy.Publisher('/whole_body_kinematic_controler/wrist_right_ft_link_goal', PoseStamped, queue_size=1) # self.pub = rospy.Publisher('/whole_body_kinematic_controler/arm_right_7_link_goal', PoseStamped, queue_size=1) self.pub = rospy.Publisher('/pose_to_follow', PoseStamped, queue_size=1) rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.name)) #self.global_frame_id = 'world' self.global_frame_id = 'base_footprint' pose = Pose() pose.position.x = 0.2 # pose.position.y = 0.35 pose.position.y = -0.35 #pose.position.z = 0.1 pose.position.z = 1.1 pose.orientation.w = 1.0 # pose.orientation.x = 0.35762 # pose.orientation.y = 0.50398 # pose.orientation.z = 0.45213 # pose.orientation.w = 0.64319 #self.makeMenuMarker(pose) self.makeGraspIM(pose) self.server.applyChanges()
def main(): rospy.init_node('map_annotator_server') myargv = rospy.myargv() if len(myargv) < 2: print 'Usage: rosrun map_annotator server.py path/to/db' return db_path = myargv[1] wait_for_time() database = PoseDatabase(db_path) list_pub = rospy.Publisher('map_annotator/pose_names', PoseNames, latch=True, queue_size=1) im_server = InteractiveMarkerServer('map_annotator/map_poses') marker_server = PoseMarkers(database, im_server) move_base = actionlib.SimpleActionClient('move_base', MoveBaseAction) server = Server(database, list_pub, marker_server, move_base) user_action_sub = rospy.Subscriber('map_annotator/user_actions', UserAction, server.handle_user_action) rospy.sleep(0.5) marker_server.start() server.start() def handle_shutdown(): pn = PoseNames() list_pub.publish(pn) database.save() rospy.on_shutdown(handle_shutdown) rospy.spin()
def __init__(self, parent_instance_handle, server_name, ik_solver): self.parent_instance_handle = parent_instance_handle self.server = InteractiveMarkerServer(server_name) self.ik_solver = ik_solver self.ik_position_xyz_bounds = [0.01, 0.01, 0.01] self.ik_position_rpy_bounds = [31416.0, 31416.0, 31416.0] # NOTE: This implements position-only IK self._menu_handlers = {} self._menu_cmds = {} self.menu_handler = MenuHandler() del_sub_menu_handle = self.menu_handler.insert("Delete Waypoint") self._menu_cmds['del_wp'] = self.menu_handler.insert("Yes", parent=del_sub_menu_handle, callback=self._process_feedback) self.menu_handler.insert("No", parent=del_sub_menu_handle, callback=self._process_feedback) ins_sub_menu_handle = self.menu_handler.insert("Insert Waypoint") self._menu_cmds['ins_wp_before'] = self.menu_handler.insert("Before", parent=ins_sub_menu_handle, callback=self._process_feedback) self._menu_cmds['ins_wp_after'] = self.menu_handler.insert("After", parent=ins_sub_menu_handle, callback=self._process_feedback) self.menu_handler.insert("Cancel", parent=ins_sub_menu_handle, callback=self._process_feedback) self._int_marker_name_list = [] self.markers_created_cnt = 0 self.marker_cnt = 0 self.name_to_marker_dict = {} self.waypoint_to_marker_dict = {} self.name_to_waypoint_dict = {}
def __init__(self, step_number, arm_index, action_step, marker_click_cb): ''' Args: step_number (int): The 1-based index of the step. arm_index (int): Side.RIGHT or Side.LEFT action_step (ActionStep): The action step this marker marks. marker_click_cb (function(int,bool)): The function to call when a marker is clicked. Pass the uid of the marker (as calculated by get_uid(...) as well as whether it's selected. ''' if ActionStepMarker._im_server is None: im_server = InteractiveMarkerServer(TOPIC_IM_SERVER) ActionStepMarker._im_server = im_server self.action_step = action_step self.arm_index = arm_index self.arm_name = ARM_NAMES[arm_index] self.step_number = step_number self.is_requested = False self.is_deleted = False self.is_control_visible = False self.is_edited = False self.has_object = False self._sub_entries = None self._menu_handler = None self._prev_is_reachable = None ActionStepMarker._marker_click_cb = marker_click_cb
def __init__(self): rospy.loginfo("Constructor got called") self.frame_id = rospy.get_param("~frame_id") self.mesh_resource = rospy.get_param("~mesh_resource") self.scale = rospy.get_param("~marker_scale") self.server = InteractiveMarkerServer("tool_calib") self.server.insert(self.create_6dof_marker(), self.marker_callback) self.server.applyChanges() self.tool_marker = Marker() self.tool_marker.header.frame_id = self.frame_id self.tool_marker.header.stamp = rospy.get_rostime() self.tool_marker.pose.orientation.w = 1 self.tool_marker.ns = "tool_marker" self.tool_marker.id = 1 self.tool_marker.action = Marker.ADD self.tool_marker.type = Marker.MESH_RESOURCE self.tool_marker.color = ColorRGBA(1.0, 1.0, 1.0, 1.0) self.tool_marker.scale = Vector3(self.scale, self.scale, self.scale) self.tool_marker.frame_locked = True self.tool_marker.mesh_resource = self.mesh_resource self.tool_marker.mesh_use_embedded_materials = True self.marker_pub = rospy.Publisher("/visualization_marker", Marker, queue_size=1)
def __init__(self): super(ParkingSpotServer, self).__init__('parking_spot_server') self.declare_parameter('map_yaml') self.markers = {} self.poses = {} self.marker_server = InteractiveMarkerServer(self, 'markers') map_param = self.get_parameter('map_yaml').value self.map_path = Path(map_param) self.load_map() self.name_counter = 0 # Create a timeout to throttle saving data on feedback # We reset it on marker feedback so that once the user stops editing, save is triggered self.save_timer = self.create_timer(0.1, self.save_map) self.save_timer.cancel() self.add_srv = self.create_service(AddParkingSpot, 'add_parking_spot', self.add_spot) self.del_srv = self.create_service(DeleteParkingSpot, 'delete_parking_spot', self.delete_spot) self.get_srv = self.create_service(GetParkingSpot, 'get_parking_spot', self.get_spot) self.rename_srv = self.create_service(RenameParkingSpot, 'rename_parking_spot', self.rename_spot)
def main(args): server = InteractiveMarkerServer("guess_markers") guess = Guess() for i in range(len(guess.items)): server.insert(guess.markers[i], guess.process_feedback) server.applyChanges() rospy.spin()
def __init__(self): self.goto_pub = rospy.Publisher('/move_base_simple/goal', geometry_msgs.msg.PoseStamped) self.pose_pub = rospy.Publisher('/pose_names', order_system.msg.PoseNames, latch=True) rospy.Subscriber('/amcl_pose', geometry_msgs.msg.PoseWithCovarianceStamped, self._handle_pose_callback) self.names = {} try: with open( "/home/team3/catkin_ws/src/cse481c/order_service/LOL.pickle", "r") as f: self.names = pickle.load(f) except EOFError: self.names = {} self.pose = None self.server = InteractiveMarkerServer('simple_marker') self.name_markers = {} self.name_controls = {}
def main(): rospy.init_node('interactive_marker_node') server = InteractiveMarkerServer("simple_marker") # create an interative marker int_marker = InteractiveMarker() int_marker.header.frame_id = "base_link" int_marker.name = "my_marker" int_marker.description = "Simple Click Control" int_marker.pose.position.x = 1 int_marker.pose.orientation.w = 1 # create a cube for the interactive marker box_marker = Marker() box_marker.type = Marker.CUBE box_marker.pose.orientation.w = 1 box_marker.scale.x = 0.45 box_marker.scale.y = 0.45 box_marker.scale.z = 0.45 box_marker.color.r = 0.0 box_marker.color.g = 0.5 box_marker.color.b = 0.5 box_marker.color.a = 1.0 # create a control for the interactive marker button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True button_control.markers.append(box_marker) int_marker.controls.append(button_control) server.insert(int_marker, handle_viz_input) server.applyChanges() rospy.spin()
def __init__(self, from_frame, to_frame, position, orientation): self.server = InteractiveMarkerServer("posestamped_im") o = orientation r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w]) rospy.loginfo("Publishing transform and PoseStamped from: " + from_frame + " to " + to_frame + " at " + str(position.x) + " " + str(position.y) + " " + str(position.z) + " and orientation " + str(o.x) + " " + str(o.y) + " " + str(o.z) + " " + str(o.w) + " or rpy " + str(r) + " " + str(p) + " " + str(y)) self.menu_handler = MenuHandler() self.from_frame = from_frame self.to_frame = to_frame # Transform broadcaster self.tf_br = tf.TransformBroadcaster() self.pub = rospy.Publisher('/posestamped', PoseStamped, queue_size=1) rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.name)) pose = Pose() pose.position = position pose.orientation = orientation self.last_pose = pose self.print_commands(pose) self.makeGraspIM(pose) self.server.applyChanges()
def __init__(self, root_link, tip_links): # tf self.tfBuffer = Buffer(rospy.Duration(1)) self.tf_listener = TransformListener(self.tfBuffer) # giskard goal client # self.client = SimpleActionClient('move', ControllerListAction) self.client = SimpleActionClient('qp_controller/command', ControllerListAction) self.client.wait_for_server() # marker server self.server = InteractiveMarkerServer("eef_control") self.menu_handler = MenuHandler() for tip_link in tip_links: int_marker = self.make6DofMarker( InteractiveMarkerControl.MOVE_ROTATE_3D, root_link, tip_link) self.server.insert( int_marker, self.process_feedback(self.server, self.client, root_link, tip_link)) self.menu_handler.apply(self.server, int_marker.name) self.server.applyChanges()
def __init__(self, root_tips, suffix=u''): """ :param root_tips: list containing root->tip tuple for each interactive marker. :type root_tips: list :param suffix: the marker will be called 'eef_control{}'.format(suffix) :type suffix: str """ self.enable_self_collision = rospy.get_param(u'~enable_self_collision', True) self.giskard = GiskardWrapper() if len(root_tips) > 0: self.roots, self.tips = zip(*root_tips) else: self.roots = [] self.tips = [] self.suffix = suffix self.markers = {} # marker server self.server = InteractiveMarkerServer(u'eef_control{}'.format( self.suffix)) self.menu_handler = MenuHandler() for root, tip in zip(self.roots, self.tips): int_marker = self.make_6dof_marker( InteractiveMarkerControl.MOVE_ROTATE_3D, root, tip) self.server.insert( int_marker, self.process_feedback(self.server, int_marker.name, root, tip, self.giskard, self.enable_self_collision)) self.menu_handler.apply(self.server, int_marker.name) self.server.applyChanges()
def __init__(self, name="interactive_markers"): self.server = InteractiveMarkerServer(name) self.mg = MarkerGenerator() self.mg.type = 1 self.mg.scale = [.25] * 3 self.c = 0 self.markers = {}
def main(): rospy.init_node("rviz_stuff") server = InteractiveMarkerServer("rviz_stuff") cube = assign( Marker(), type=Marker.CUBE, scale=assign(x=0.1, y=0.1, z=0.1), color=assign(r=1, g=0, b=0, a=1), pose=assign(Pose(), position=assign(x=0.1, y=0, z=0)), ) sphere = assign( Marker(), type=Marker.SPHERE, scale=assign(x=0.1, y=0.1, z=0.1), color=assign(r=1, g=0, b=0, a=1), pose=assign(Pose(), position=assign(x=-0.1, y=0.1, z=0)), ) X_WA = assign(Pose(), position=assign(x=1, y=0, z=1), orientation=assign(w=1, x=0, y=0, z=0)) a = SimpleInteractiveMarker(server, "a", X_WA, [cube]) X_WB = assign(Pose(), position=assign(x=1, y=1, z=2), orientation=assign(w=1, x=0, y=0, z=0)) b = SimpleInteractiveMarker(server, "b", X_WB, [cube, sphere]) b.set_pose(assign(Pose(), position=assign(x=-1))) X_WC = Pose() c = SimpleInteractiveMarker(server, "c", X_WC, [sphere]) c.delete() c.delete() rospy.spin()
def __init__(self, network_file): self.network_file = network_file with open(network_file, 'r') as file_obj: network = yaml.safe_load(file_obj) self.nodes = { node['id']: Node.from_dict(node) for node in network['nodes'] } self.connections = network['connections'] self.node_id_counter = max(self.nodes.keys()) # ros params self.frame = rospy.get_param('frame', 'map') self._topology_pub = rospy.Publisher('/topology', MarkerArray, queue_size=1) self.interactive_marker_server = InteractiveMarkerServer( "two_dof_marker") rospy.Subscriber('/clicked_point', PointStamped, self.clicked_point_cb) rospy.Subscriber('~command', String, self.command_cb) rospy.sleep(1.0) self._topology_pub.publish(self.get_marker_array()) rospy.sleep(1.0) for node in self.nodes.values(): self._add_interactive_marker(node.x, node.y, node.id) rospy.loginfo('Initialised')
def __init__(self): rospy.init_node('look_at_marker_node') self._server = InteractiveMarkerServer('look_at_marker_server') self.makeQuadrocopterMarker(Point(3, 0, 1)) self.makeStaticMarker(Point(0, 0, 1)) self._tf_listener = tf.TransformListener() self._head = robot_api.FullBodyLookAt(tf_listener=self._tf_listener) self._look_at_target = None
def main(): rospy.init_node('interactive_gripper_demo') im_server = InteractiveMarkerServer('gripper_im_server', q_size=10) arm = fetch_api.Arm() gripper = fetch_api.Gripper() teleop = GripperTeleop(arm, gripper, im_server) #auto_pick = AutoPickTeleop(arm, gripper, im_server) rospy.spin()
def main(): # Initialize the interactive marker server for the gripper rospy.init_node('gripper_demo') im_server = InteractiveMarkerServer('gripper_im_server', q_size=2) auto_pick_im_server = InteractiveMarkerServer('auto_pick_im_server', q_size=2) down_im_server = InteractiveMarkerServer('down_gripper_im_server', q_size=2) arm = fetch_api.Arm() gripper = fetch_api.Gripper() teleop = GripperTeleop(arm, gripper, im_server) auto_pick = AutoPickTeleop(arm, gripper, auto_pick_im_server) down_teleop = GripperTeleopDown(arm, gripper, down_im_server) teleop.start() auto_pick.start() down_teleop.start() rospy.spin()
def __init__(self, marker_name): host = rospy.get_param("mongodb_host") port = rospy.get_param("mongodb_port") self._client = pymongo.MongoClient(host, port) self._traj = dict() self._retrieve_logs(marker_name) self._server = InteractiveMarkerServer(marker_name)
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 main(): rospy.init_node('mmmm') # Initializing interactive markers marker_server = InteractiveMarkerServer("simple_server") monitor = InteractiveMarkerMonitor(marker_server) marker_server.applyChanges() rospy.spin()
def __init__(self, setup, world_frame): super(IntCollisionEnv, self).__init__(setup, world_frame) self.im_server = InteractiveMarkerServer(self.NS + "markers") self.check_attached_timer = rospy.Timer(rospy.Duration(0.1), self.check_attached_timer_cb) self.create_menus()
def __init__(self, server_ns='interactive_markers'): self.server = InteractiveMarkerServer(server_ns) self._menu_handlers = {} self._menu_cmds = {} self.markers_created_cnt = 0 self.marker_cnt = 0 self._int_marker_name_list = [] self.name_to_marker_dict = {} self.name_to_position_only_flag = {}