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 = {}
