def main(stdscr):
# Initialize
node = MapNode(stdscr)
rospy.Subscriber(STATE_TOPIC, FuriousState, node.on_state_update)
rospy.Subscriber(SERVO_TOPIC, ServoCommand, node.on_servo_update)
rospy.init_node(NODE_NAME)
rate = rospy.Rate(LOOP_FREQ)
# Process command-line args
mapfile_name = None
args = rospy.myargv()[1:]
while len(args) > 0:
arg = args.pop(0)
mapfile_name = os.path.abspath(arg)
if not mapfile_name:
return "Missing required argument MAPFILE".append(
__doc__ % os.path.basename(rospy.myargv()[0]) )
try:
node.load_mapfile(mapfile_name)
except CourseMapError, e:
return "Error: %s" % e
def main():
rospy.init_node('stamina_calibration_tf_publisher')
robot_description = rospy.get_param("robot_description")
r = rospy.Rate(5)
if (len(rospy.myargv()) < 3):
rospy.logerr(usage())
return
else:
# the keys are the optical frames of the calibrated cameras
# the values are the fixed joints of the cameras and their base
cam_dict = {}
urdf_xml = rospy.myargv()[1]
try:
for i in xrange(2, len(rospy.myargv())):
[cam_link, joint] = map(lambda x: x.strip("/"), rospy.myargv()[i].split(':'))
cam_dict[cam_link] = joint
except:
rospy.logerror(usage())
return
print cam_dict
c = CalibrationManager(robot_description, urdf_xml, cam_dict)
while not rospy.is_shutdown():
if c.is_updated():
c.update()
try:
r.sleep()
except:
rospy.loginfo("Shutting down")
def main():
rospy.init_node('simple_publisher')
icp_align = rospy.ServiceProxy('icp_align', AlignModelSrv)
for id1,filename1 in enumerate(rospy.myargv()[1:]):
for id2,filename2 in enumerate(rospy.myargv()[1:]):
if id1 >= id2:
continue
track1 = readtrack(filename1)
track2 = readtrack(filename2)
print "calling service.."
request = AlignModelSrvRequest()
request.model.track = track1
request.data.track = track2
response = icp_align(request)
rospy.sleep(0.05)
request.model.track.header.frame_id="/"
request.data.track.header.frame_id="/"
response.data_aligned.track.header.frame_id="/"
model_pub.publish(request.model.track)
data_pub.publish(request.data.track)
data_aligned_pub.publish(response.data_aligned.track)
rospy.sleep(0.05)
def main():
# rospy gets first crack at sys.argv
rospy.myargv(argv=sys.argv)
(options,args) = getopt.getopt(sys.argv[1:], 'v:f:s:t:', ['vm=','feedback=','source=','target='])
rospy.init_node('pr2_driver')
source_frameid = rospy.get_param("/r_cart/root_name","torso_lift_link")
target_frameid = rospy.get_param("/r_cart/tip_name","r_gripper_tool_frame")
vm = rospy.get_param("/pr2_driver/velocity_multiplier", 10.0)
feedback = rospy.get_param("/pr2_driver/feedback", 'hard')
for o,a in options:
if o in ('-v','--vm'):
vm = float(a)
elif o in ('-f','--feedback'):
assert a in ('none','hard','adaptive')
feedback = a
elif o in ('-s','--source'):
source_frameid = a
elif o in ('-t','--target'):
target_frameid = a
driver = PR2Driver("pr2_driver", vm, feedback, 100, source_frameid, target_frameid, False) # start node
driver.spin()
def __init__(self):
rospy.init_node('pyturtlebot', anonymous=True)
rospy.myargv(argv=sys.argv)
self.__x = None
self.__y = None
self.__angle = None
self.__cumulative_angle = 0.0
self.__have_odom = False
self.on_bumper = None
self.movement_enabled = True
self.__cmd_vel_pub = rospy.Publisher('/mobile_base/commands/velocity', Twist)
self.__bumper_sub = rospy.Subscriber('/mobile_base/events/bumper', BumperEvent, self.__bumper_handler)
self.__odom_sub = rospy.Subscriber('/odom', Odometry, self.__odom_handler)
self.__wheeldrop_sub = rospy.Subscriber('/mobile_base/events/wheel_drop',
WheelDropEvent, self.__wheeldrop_handler)
self.__scan_sub = rospy.Subscriber('/scan', LaserScan, self.__scan_handler)
self.__sound_pub = rospy.Publisher('/mobile_base/commands/sound', Sound)
self.__led_pubs = {
'1': rospy.Publisher('/mobile_base/commands/led1', Led),
'2': rospy.Publisher('/mobile_base/commands/led2', Led),
}
def main():
#Example usage:
#rosrun wam2seds.py -b <path_to_wam_bag_files> -o <path_to_seds_bagfile>
"""
Right now process_bags only uses x,y,z position information.
"""
# rospy gets first crack at sys.argv
rospy.myargv(argv=sys.argv)
(options,args) = getopt.getopt(sys.argv[1:], 'b:o:s:t:', ['bags=','ouptut=','source=','target='])
rospy.init_node("wam2seds")
path = rospy.get_param("/wam2seds/source_directory", None)
outfile = rospy.get_param("/wam2seds/outputfile", None)
# or specify any of these options on the command line
for o,a in options:
if o in ('-b','--bags'):
path = a
elif o in ('-o','--output'):
outfile = a
elif o in ('-s','--source'):
source_frameid = a
elif o in ('-t','--target'):
target_frameid = a
if not path or not outfile:
raise Exception, "You must specify bagfile input and output locations."
bagfiles = glob.glob(os.path.normpath(path + "/*.bag"))
bagfiles.sort() # given the default naming these should be sorted by time
rospy.loginfo("dir: %s out: %s", path, outfile)
process_bags(outfile, bagfiles)
def run(self, *args, **kwargs):
"""
Correctly initialize ROS, add a callback handler for shutdown and keep running ROS until either the GUI shuts
down or rospy shuts down.
"""
rospy.init_node("main", disable_signals=True)
rospy.myargv(argv=sys.argv)
rospy.on_shutdown(self.stop)
#rospy.wait_for_service('authenticate')
#AuthenticationClient.login = rospy.ServiceProxy('authenticate', Authenticate)
self.solenoid_publisher = rospy.Publisher("solenoids", Solenoid)
login_subscriber = rospy.Subscriber('authentication', Authentication, AuthenticationClient.login_handler)
while not rospy.is_shutdown() and self.running:
try:
if len(self.functions) > 0:
func, args, kwargs = self.functions.pop()
try:
func(*args, **kwargs)
except Exception as e:
rospy.logerr(e.message)
rospy.sleep(0.1)
except rospy.ROSInterruptException:
break
rospy.signal_shutdown("is_shutdown() evaluated to True" if self.running else "ROS thread manually stopped")
self.done = True
def main():
rospy.init_node('ua_cmder')
if len(rospy.myargv()) > 1 and rospy.myargv()[1] == '--low':
low = True
else:
low = False
pan_pos = rospy.Publisher('r_shoulder_pan_position_controller/command', Float64)
lift_pos = rospy.Publisher('r_shoulder_lift_position_controller/command', Float64)
roll_pos = rospy.Publisher('r_upper_arm_roll_position_controller/command', Float64)
my_rate = rospy.Rate(float(rospy.get_param('cycle_rate', 1.0)) * 2)
#my_rate = rospy.Rate(1.0)
even = True
try:
while not rospy.is_shutdown():
pan_pos.publish(Float64(random.uniform(-2.0, 0.4)))
if low:
even = not even
if even:
lift_pos.publish(Float64(random.uniform(0.8, 0.88)))
else:
lift_pos.publish(Float64(random.uniform(1.17, 1.25)))
else:
lift_pos.publish(Float64(random.uniform(-0.40, 1.25)))
roll_pos.publish(Float64(random.uniform(-3.70, 0.5)))
my_rate.sleep()
except KeyboardInterrupt, e:
pass
def __init__(self):
rospy.init_node('pyturtlebot', anonymous=True)
rospy.myargv(argv=sys.argv)
self.on_bumper = None
self.__cmd_vel_pub = rospy.Publisher('/mobile_base/commands/velocity', Twist)
self.__bumper_sub = rospy.Subscriber('/mobile_base/events/bumper', BumperEvent, self.__bumper_handler)
def __init__(self):
pygame.init()
rospy.init_node('pyturtlebot', anonymous=True)
rospy.myargv(argv=sys.argv)
self.horrible = None
self.image_procesor = RobotinaImage(friend=True)
self.current_maze_state = None
self.__x = None
self.__y = None
self.__angle = None
self.__cumulative_angle = 0.0
self.__have_odom = False
self.bridge = CvBridge()
self.on_bumper = None
self.current_state = None
self.current_substate = None
self.current_min_dist = None
self.stop_dist = 0.8
self.speed_const = 0.7 / 1.4
self.movement_enabled = True
self.current_laser_msg = None
self.current_cv_image = None
self.current_cv_rgb_image = None
self.current_mask = None
self.current_depth_msg = None
self.current_rgb_image = None
self.current_img_track = []
self.current_depth_track = []
self.current_target_x = None
self.current_target_y = None
self.current_target_depth = None
self.crop_h = 480
self.crop_w = 480
self.__cmd_vel_pub = rospy.Publisher('/mobile_base/commands/velocity', Twist)
self.__bumper_sub = rospy.Subscriber('/mobile_base/events/bumper', BumperEvent, self.__bumper_handler)
self.__odom_sub = rospy.Subscriber('/odom', Odometry, self.__odom_handler)
self.__wheeldrop_sub = rospy.Subscriber('/mobile_base/events/wheel_drop',
WheelDropEvent, self.__wheeldrop_handler)
self.__scan_sub = rospy.Subscriber('/scan', LaserScan, self.__scan_handler)
self.__sound_pub = rospy.Publisher('/mobile_base/commands/sound', Sound)
self.__led_pubs = {
'1': rospy.Publisher('/mobile_base/commands/led1', Led),
'2': rospy.Publisher('/mobile_base/commands/led2', Led),
}
self.__depth_img = rospy.Subscriber('/camera/depth/image',Image ,self.__depth_handler)
self.__rgb_img= rospy.Subscriber('/camera/rgb/image_color',Image,self.__rgb_handler)
def run(self):
print "running"
gears = [float(rospy.myargv()[1]), float(rospy.myargv()[2]), float(rospy.myargv()[3])]
spr = SetParametersRequest()
for i in xrange(0,3):
spr.parameters.append(Parameter())
spr.parameters[i].val_type = Parameter.FLOAT
spr.parameters[i].name = "m%d_gear" % i
spr.parameters[i].f_val = float(rospy.myargv()[i+1])
self.sp(spr)
def main():
#Example usage:
#rosrun tf2seds.py -b <path_to_tf_bag_files> -o <path_to_seds_bagfile>
"""
Rotations may cause a problem for SEDS optimization. The
quaternion representation requires that the magnitude be
normalized, but the SEDS optimization process does not include
this constraint. If the magnitude differs from 1.00 then the planned
motion will not succeed. An Euler representation is also possible,
but this has not (to my knowledge) been exposed to the Python tf
API. I'm not sure if there are also constraints for that choice of
rotation representation.
So right now process_bags only uses x,y,z position information.
"""
# rospy gets first crack at sys.argv
rospy.myargv(argv=sys.argv)
(options,args) = getopt.getopt(sys.argv[1:], 'b:o:s:t:', ['bags=','ouptut=','source=','target='])
rospy.init_node("tf2seds")
# should be in tf2seds namespace
source_frameid = rospy.get_param("/tf2seds/source_frameid","torso_lift_link")
target_frameid = rospy.get_param("/tf2seds/target_frameid","r_gripper_tool_frame")
path = rospy.get_param("/tf2seds/source_directory", None)
outfile = rospy.get_param("/tf2seds/outputfile", None)
# or specify any of these options on the command line
for o,a in options:
if o in ('-b','--bags'):
path = a
elif o in ('-o','--output'):
outfile = a
elif o in ('-s','--source'):
source_frameid = a
elif o in ('-t','--target'):
target_frameid = a
if not path or not outfile:
raise Exception, "You must specify bagfile input and output locations."
bagfiles = glob.glob(os.path.normpath(path + "/*.bag"))
bagfiles.sort() # given the default naming these should be sorted by time
rospy.loginfo("dir: %s out: %s sf: %s tf: %s", path, outfile, source_frameid, target_frameid)
process_bags(outfile, bagfiles, source_frameid,target_frameid)
def main():
"""RSDK Inverse Kinematics Example
A simple example of using the Rethink Inverse Kinematics
Service which returns the joint angles and validity for
a requested Cartesian Pose.
Run this example, passing the *limb* to test, and the
example will call the Service with a sample Cartesian
Pose, pre-defined in the example code, printing the
response of whether a valid joint solution was found,
and if so, the corresponding joint angles.
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
parser.add_argument(
'-l', '--limb', choices=['left', 'right'], required=True,
help="the limb to test"
)
args = parser.parse_args(rospy.myargv()[1:2])
point = Point(x = float(rospy.myargv()[2]),
y = float(rospy.myargv()[3]),
z = float(rospy.myargv()[4]))
orient = Quaternion(x = float(rospy.myargv()[5]),
y = float(rospy.myargv()[6]),
z = float(rospy.myargv()[7]),
w = float(rospy.myargv()[8]))
print "Point: ", point
print "Orientation: ", orient
return ik_test(args.limb, point, orient)
def __init__(self):
rospy.init_node("pyturtlebot", anonymous=True)
rospy.myargv(argv=sys.argv)
self.__x = None
self.__y = None
self.__angle = None
self.__cumulative_angle = 0.0
self.__have_odom = False
self.on_bumper = None
self.__cmd_vel_pub = rospy.Publisher("/mobile_base/commands/velocity", Twist)
self.__bumper_sub = rospy.Subscriber("/mobile_base/events/bumper", BumperEvent, self.__bumper_handler)
self.__odom_sub = rospy.Subscriber("/odom", Odometry, self.__odom_handler)
def main(name):
try:
from python_qt_binding.QtGui import QApplication
except:
print >> sys.stderr, "please install 'python_qt_binding' package!!"
sys.exit(-1)
masteruri = init_cfg_path()
parser = init_arg_parser()
args = rospy.myargv(argv=sys.argv)
parsed_args = parser.parse_args(args[1:])
# Initialize Qt
global app
app = QApplication(sys.argv)
# decide to show main or echo dialog
global main_form
if parsed_args.echo:
main_form = init_echo_dialog(name, masteruri, parsed_args.echo[0], parsed_args.echo[1], parsed_args.hz)
else:
main_form = init_main_window(name, masteruri, parsed_args.file)
# resize and show the qt window
if not rospy.is_shutdown():
os.chdir(PACKAGE_DIR) # change path to be able to the images of descriptions
main_form.resize(1024, 720)
screen_size = QApplication.desktop().availableGeometry()
if main_form.size().width() >= screen_size.width() or main_form.size().height() >= screen_size.height()-24:
main_form.showMaximized()
else:
main_form.show()
exit_code = -1
rospy.on_shutdown(finish)
exit_code = app.exec_()
def main():
rospy.init_node('axclient', anonymous=True)
parser = OptionParser(__doc__.strip())
# parser.add_option("-t","--test",action="store_true", dest="test",default=False,
# help="A testing flag")
# parser.add_option("-v","--var",action="store",type="string", dest="var",default="blah")
(options, args) = parser.parse_args(rospy.myargv())
if (len(args) != 2):
parser.error("You must specify the action topic name Eg: ./axclient.py my_action_topic")
# get action type from topic
topic_type = rostopic._get_topic_type("%s/goal"%args[1])[0]
# remove "Goal" string from action type
assert("Goal" in topic_type)
topic_type = topic_type[0:len(topic_type)-4]
action = DynamicAction(topic_type)
app = AXClientApp(action, args[1])
app.MainLoop()
app.OnQuit()
rospy.signal_shutdown('GUI shutdown')
def main():
parser = argparse.ArgumentParser()
required = parser.add_mutually_exclusive_group(required=True)
required.add_argument(
"-l",
"--list",
action="store_const",
dest="cmd",
const="list",
default="update",
help="List available updates and UUID's",
)
required.add_argument("-u", "--update", dest="uuid", default="", help="Launch the given update")
args = parser.parse_args(rospy.myargv()[1:])
cmd = args.cmd
uuid = args.uuid
rospy.init_node("rsdk_update_robot")
updater = Updater()
if cmd == "list":
updates = updater.list()
if not len(updates):
print ("No available updates")
else:
print ("%-30s%s" % ("Version", "UUID"))
for update in updates:
print ("%-30s%s" % (update[0], update[1]))
return 0
elif cmd == "update":
if uuid == "":
print "Error: no update uuid specified"
return 1
return run_update(updater, uuid)
def main():
parser = argparse.ArgumentParser()
#parser.add_argument('-f', '--freq', type=float, default=2, help='Frequency Markers are published (default: 2 Hz)')
#parser.add_argument('-p', '--prefix', default='', help='Publish with prefix')
parser.add_argument('sdf', help='SDF model to publish (e.g. coke_can)')
args = parser.parse_args(rospy.myargv()[1:])
rospy.init_node('sdf2moveit_collision')
global ignored_submodels
ignored_submodels = rospy.get_param('~ignore_submodels', '').split(';')
rospy.loginfo('Ignoring submodels of: %s' % ignored_submodels)
planning_scene_pub = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10)
sdf = pysdf.SDF(model=args.sdf)
world = sdf.world
world.for_all_links(convert_to_collision_object)
planning_scene_msg = PlanningScene()
planning_scene_msg.is_diff = True
for (collision_object_root, collision_object) in collision_objects.iteritems():
if collision_object_root in ignored_submodels:
print('TODO2') # attached object instead of collision object
else:
planning_scene_msg.world.collision_objects.append(collision_object)
planning_scene_msg.world.collision_objects[-1].header.frame_id = 'world'
while planning_scene_pub.get_num_connections() < 1:
rospy.sleep(0.1)
planning_scene_pub.publish(planning_scene_msg)
def __init__(self, *args):
super(TestFailInit, self).__init__(*args)
parser = OptionParser(usage="usage ./%prog [options]", prog="fail_init_test.py")
parser.add_option('--ns', action="store", default=None,
dest="ns", metavar="NAMESPACE",
help="Expected namespace that analyzer will fail in")
# Option comes with rostest, will fail w/o this line
parser.add_option('--gtest_output', action="store",
dest="gtest")
options, args = parser.parse_args(rospy.myargv())
if not options.ns:
parser.error("Option --ns is mandatory. Unable to parse args")
self._ns = options.ns
self._item = None
self._mutex = threading.Lock()
rospy.init_node('test_fail_init')
self._starttime = rospy.get_time()
sub_agg = rospy.Subscriber("/diagnostics_agg", DiagnosticArray, self.diag_agg_cb)
def main():
rospy.loginfo("Starting up...")
# command line args
mapfile = None
runmap = False
args = rospy.myargv()[1:]
while len(args) > 0:
opt = args.pop(0)
if opt == "-r":
runmap = True
else:
mapfile = opt
if not mapfile:
rospy.logfatal("Missing param 'mapfile'")
exit(1)
if runmap:
rospy.loginfo("Running file %s" % mapfile)
runner = MapRunner(mapfile)
runner.run()
else:
mapper = MapBuilder(mapfile)
rospy.Subscriber(SERVO_TOPIC, ServoCommand, mapper.on_servo_command)
rospy.Subscriber(STATE_TOPIC, FuriousState, mapper.on_state_update)
rospy.init_node(NODE_NAME)
rospy.loginfo("Ready.")
while not rospy.is_shutdown():
rospy.spin()
mapper.shutdown()
print "Shutting down..."
def main():
"""RSDK URDF Fragment Example:
This example shows a proof of concept for
adding your URDF fragment to the robot's
onboard URDF (which is currently in use).
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
required = parser.add_argument_group('required arguments')
required.add_argument(
'-f', '--file', metavar='PATH', required=True,
help='Path to URDF file to send'
)
required.add_argument(
'-l', '--link', required=False, default="left_hand",
help='URDF Link already to attach fragment to (usually <left/right>_hand)'
)
required.add_argument(
'-j', '--joint', required=False, default="left_gripper_base",
help='Root joint for fragment (usually <left/right>_gripper_base)'
)
args = parser.parse_args(rospy.myargv()[1:])
rospy.init_node('rsdk_configure_urdf', anonymous=True)
if not os.access(args.file, os.R_OK):
rospy.logerr("Cannot read file at '%s'" % (args.file,))
return 1
send_urdf(args.link, args.joint, args.file)
return 0
def main():
"""RSDK End Effector Position Example: Keyboard Control
Use your dev machine's keyboard to control end effector positions.
"""
epilog = """
See help inside the example with the '?' key for key bindings.
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__,
epilog=epilog)
parser.parse_args(rospy.myargv()[1:])
print("Initializing node... ")
rospy.init_node("rsdk_joint_position_keyboard")
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
def clean_shutdown():
print("\nExiting example...")
if not init_state:
print("Disabling robot...")
rs.disable()
rospy.on_shutdown(clean_shutdown)
print("Enabling robot... ")
rs.enable()
map_keyboard()
print("Done.")
def __call__(cls, *args, **kwargs):
"""Called when you call ConnectionBasedTransport()"""
obj = type.__call__(cls, *args, **kwargs)
# display node input/output topics
parser = argparse.ArgumentParser()
parser.add_argument('--inout', action='store_true')
args = parser.parse_args(rospy.myargv()[1:])
if args.inout:
obj.subscribe()
tp_manager = rospy.topics.get_topic_manager()
print('Publications:')
for topic, topic_type in tp_manager.get_publications():
if topic == '/rosout':
continue
topic = unresolve_name(rospy.get_name(), topic)
print(' * {0} [{1}]'.format(topic, topic_type))
print('Subscriptions:')
for topic, topic_type in tp_manager.get_subscriptions():
topic = unresolve_name(rospy.get_name(), topic)
print(' * {0} [{1}]'.format(topic, topic_type))
sys.exit(0)
obj._post_init()
return obj
def main():
parser = argparse.ArgumentParser()
subparsers = parser.add_subparsers(dest='command')
# fit command
fit_parser = subparsers.add_parser('fit',
help='fit feature extractor using dataset')
fit_parser.add_argument('data_path', help='data path')
fit_parser.add_argument('-O', '--output', default='bof.pkl.gz',
help='bof feature extractor instance save path')
# dataset command
dataset_parser = subparsers.add_parser('dataset',
help='create bof histogram dataset')
dataset_parser.add_argument('data_path', help='data path')
dataset_parser.add_argument('bof_path', help='bof data path')
dataset_parser.add_argument('-O', '--output', default='bof_hist.pkl.gz',
help='save path of bof histogram (default: bof_hist.pkl.gz)')
# extract command
extract_parser = subparsers.add_parser('extract',
help='extract feature in realtime')
extract_parser.add_argument('bof_path', help='bof data path')
args = parser.parse_args(rospy.myargv(sys.argv[1:]))
if args.command == 'fit':
fit_bof_extractor(data_path=args.data_path, bof_path=args.output)
elif args.command == 'dataset':
create_dataset(data_path=args.data_path,
bof_path=args.bof_path,
bof_hist_path=args.output)
elif args.command == 'extract':
rospy.init_node('extract_bof')
ex_real = ExtractInRealtime(bof_path=args.bof_path)
rospy.spin()
def main():
wd = roslib.packages.get_pkg_dir('flyvr')
default_fname = os.path.join(wd,'data','vienna-morning.jpg')
parser = argparse.ArgumentParser()
parser.add_argument('fname',nargs='?',default=default_fname)
parser.add_argument('--rgb', help='RGB tuple r,g,b (float, 0...1)', default="-1,-1,-1")
parser.add_argument('--white', action='store_true', help='show a white screen')
parser.add_argument('--black', action='store_true', help='show a black screen')
parser.add_argument('--viewport', type=str, help='only show on this viewport')
parser.add_argument(
'--display-server', type=str, metavar='/display_server', required=True, help=\
'the path of the display server to configure')
parser.add_argument('--pixel', type=str, help='light this pixel', metavar='x,y')
parser.add_argument('--scale', action='store_true', help='scale the image to fullscreen')
parser.add_argument('--pxsize', type=int, default=2)
argv = rospy.myargv()
args = parser.parse_args(argv[1:])
show_image(args.display_server, args.viewport, args.fname,
args.white,
args.black,
tuple(map(int,args.rgb.split(','))),
args.pixel,
args.pxsize,
scale = args.scale,
)
def main():
"""RSDK Navigator Input/Output Example
Demonstrates Navigator output by blinking the lights, or
Navigator input by echoing input values from wheels and
buttons.
Run this example, selecting either the input or output action
with the corresponding arguments, then follow the instructions
on screen.
Uses the baxter_interface.Navigator class to interface with the
four Navigator button/LED controls. Also shows a nice example of
using the button_changed Signal feature.
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
action_grp = parser.add_mutually_exclusive_group(required=True)
action_grp.add_argument(
'-b', '--blink', dest='action', action='store_const', const=blink,
help='Blink navigator lights for 10 seconds'
)
action_grp.add_argument(
'-i', '--input', dest='action', action='store_const', const=echo_input,
help='Show input of left arm for 10 seconds'
)
args = parser.parse_args(rospy.myargv()[1:])
rospy.init_node('rsdk_navigator_io', anonymous=True)
args.action()
return 0
def main():
#parse command line arguments
ros_argv = rospy.myargv()
script_name = os.path.basename(ros_argv[0])
parser = argparse.ArgumentParser(prog=script_name, description='Generates unified MDP state from vector of sub-states.')
parser.add_argument('input_file', help='Contains vector state space in YAML format with name "state_space" under document root.')
parser.add_argument('output_file', help='MDP Policy file to which comments will be appended.')
args = parser.parse_args(ros_argv[1:])
#open the input file
state_space_file = open(args.input_file, 'r')
#parse the YAML file into a node called state_space
state_space = yaml.load(state_space_file)
state_space_file.close()
#convert YAML node into a VectorFeatureSpace
vector_feature_space = VectorFeatureSpace(state_space['state_space'])
#open output file to write state comments to
output_file = open(args.output_file, 'a')
#iterate over sub-featurespaces to produce all combinations of states
for x in range(vector_feature_space.num_values()):
dehash = vector_feature_space.dehash(x)
output_file.write("\n#")
for i in range(0, len(vector_feature_space.values)):
output_file.write(vector_feature_space.values[i].toString(dehash[i]))
# if i < (len(vector_feature_space.values) - 1):
output_file.write(" ")
output_file.close()
def main():
# Parse arguments
parser = argparse.ArgumentParser(description="Delete frames being broadcast by a multi_static_transform_publisher")
parser.add_argument(
"--node-name",
metavar=("node_name"),
default=["multi_tf_pub"],
type=str,
nargs=1,
help="The name of the multi publisher that should publish this transform. Default is 'multi_tf_pub'",
)
parser.add_argument(
"frame_ids", metavar=("frame_id"), type=str, nargs="+", help="The frame_ids of each frame to stop broadcasting."
)
myargv = rospy.myargv(argv=sys.argv)
args = parser.parse_args(args=myargv[1:])
rospy.init_node("multi_static")
topic_name = args.node_name[0] + "/del_frame"
# Publish the transform
pub = rospy.Publisher(topic_name, std_msgs.String, latch=True)
for frame_id in args.frame_ids:
pub.publish(std_msgs.String(frame_id))
# Wait for sigint
rospy.spin()
def ntp_monitor_main(argv=sys.argv):
import optparse
parser = optparse.OptionParser(usage="usage: ntp_monitor ntp-hostname []")
parser.add_option("--offset-tolerance", dest="offset_tol",
action="store", default=500,
help="Offset from NTP host", metavar="OFFSET-TOL")
parser.add_option("--error-offset-tolerance", dest="error_offset_tol",
action="store", default=5000000,
help="Offset from NTP host. Above this is error", metavar="OFFSET-TOL")
parser.add_option("--self_offset-tolerance", dest="self_offset_tol",
action="store", default=500,
help="Offset from self", metavar="SELF_OFFSET-TOL")
parser.add_option("--diag-hostname", dest="diag_hostname",
help="Computer name in diagnostics output (ex: 'c1')",
metavar="DIAG_HOSTNAME",
action="store", default=None)
options, args = parser.parse_args(rospy.myargv())
if (len(args) != 2):
parser.error("Invalid arguments. Must have HOSTNAME [args]. %s" % args)
try:
offset = int(options.offset_tol)
self_offset = int(options.self_offset_tol)
error_offset = int(options.error_offset_tol)
except:
parser.error("Offsets must be numbers")
ntp_monitor(args[1], offset, self_offset, options.diag_hostname, error_offset)
def main():
max_gain = 3.0
min_gain = 0.1
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
required = parser.add_argument_group('required arguments')
required.add_argument(
"-l", "--limb", required=True, choices=['left', 'right'],
help="Specify Control Limb"
)
parser.add_argument(
"-a", "--amplification", type=float, default=1.0,
help=("amplification to apply to the puppeted arm [%g, %g]"
% (min_gain, max_gain))
)
args = parser.parse_args(rospy.myargv()[1:])
if (args.amplification < min_gain or max_gain < args.amplification):
print("Exiting: Amplification must be between: [%g, %g]" %
(min_gain, max_gain))
return 1
rospy.init_node("pos_file_record")
name = "posFile.dat"
dummyFile = open(name, "w");
cPickle.dump(args.limb, dummyFile);
dummyFile.close()
puppeteer = Puppeteer(args.limb, args.amplification)
rospy.on_shutdown(puppeteer.clean_shutdown)
puppeteer.puppet()
print("Done.")
return 0
cmd_vel.angular.z = self.max_angular_vel
# Drive forwards to goal position
elif dist > self.position_deadband:
# print("Moving to destination position",dist)
cmd_vel.linear.x = self.max_linear_vel * random.uniform(0.4, 0.8)
else:
# Reached deadband about goal position, remove waypoint from history
if(len(self.pose_goal_buffer) > 1):
self.pose_goal_buffer.pop(0)
self.pose_goal = self.pose_goal_buffer[0] # Set the new goal
else:
self.pose_goal = None
self.vel_pub.publish(cmd_vel)
self.waypoint_pub.publish(self.marker_array)
if __name__ == '__main__':
node_args = rospy.myargv(argv=sys.argv) # takes in arguments
rospy.init_node('leader_controller')
if len(node_args) == 0:
cw = VehicleController(False)
else:
cw = VehicleController(node_args[0])
try:
rospy.spin()
except rospy.ROSInterruptException:
pass
print("Emergency Saving")
self.data_file_name += "_emr"
self.saveLog()
rospy.logwarn("Emergency save data")
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Logs forces of robots')
parser.add_argument('-f',
'--filename',
type=str,
help='filename for logfile')
parser.add_argument('-d', '--directory', type=str, help='directory')
parser.add_argument('-b',
'--breakSize',
type=int,
help='size when to start emergency saving in MB')
parser.add_argument(
'-m',
'--minspace',
type=int,
help='stop saving data if not at least this much space is left in MB')
args = parser.parse_args(rospy.myargv(
)[1:]) # from ROS there could be arguments which interfere with argparse
a = Forcelogger(filename=args.filename,
directory=args.directory,
breaksize=args.breakSize)
a.listener()
def ibeoScan(self, newMultiLaserScanData):
if (not (self.plp is None)):
self.plp.parameters_updated(newMultiLaserScanData,
rospy.get_time())
def scan_frequency_test(self, timer_event):
"""Test that the last scan is not late"""
detection = self.plp.test_scan_frequency(rospy.get_time())
if not (detection is None):
self.condition_detected(detection)
def condition_detected(self, detection_message):
self.publisher.publish(
PlpMessage(None, "IBEO", "detect", repr(detection_message)))
# Main entry point for the IBEO PLP ROS node.
if __name__ == '__main__':
try:
rospy.loginfo("Starting plp_ibeo node...")
start_args = rospy.myargv(argv=sys.argv)
harness = PlpIbeoRosHarness(config)
rospy.loginfo("started")
# The detection start on this PLP is system startup, so we start it here.
harness.trigger_detection_start()
rospy.spin()
except rospy.ROSInterruptException:
pass
def main():
"""RSDK Joint Trajectory Example: File Playback
Plays back joint positions honoring timestamps recorded
via the joint_recorder example.
Run the joint_recorder.py example first to create a recording
file for use with this example. Then make sure to start the
joint_trajectory_action_server before running this example.
This example will use the joint trajectory action server
with velocity control to follow the positions and times of
the recorded motion, accurately replicating movement speed
necessary to hit each trajectory point on time.
"""
epilog = """
Related examples:
joint_recorder.py; joint_position_file_playback.py.
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__,
epilog=epilog)
parser.add_argument(
'-f', '--file', metavar='PATH', required=True,
help='path to input file'
)
parser.add_argument(
'-l', '--loops', type=int, default=1,
help='number of playback loops. 0=infinite.'
)
parser.add_argument(
'-p', '--pickle', type=str, required=True,
help='path to save pickled dump file'
)
# remove ROS args and filename (sys.arv[0]) for argparse
args = parser.parse_args(rospy.myargv()[1:])
print("Initializing node... ")
rospy.init_node("rollout_execution")
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
print("Enabling robot... ")
rs.enable()
print("Running. Ctrl-c to quit")
current = [0.42031073588060336, -0.054072822772960834, 1.1685098651717138, 1.5240099127641586, -1.5067526289004476, -0.21974274786458553, 3.0092868106342103]
des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.7]
sweep_dict = {'name':'right_s1', 'fixed':des, 'offset':0, 'iterations':1000}
loop_cnt = 1
loopstr = str(args.loops)
if args.loops == 0:
args.loops = float('inf')
loopstr = "forever"
traj = RolloutExecuter(sweep_joint_dict=sweep_dict, debug=True)
rospy.on_shutdown(traj.stop_iteration)
while (loop_cnt <= args.loops and not rospy.is_shutdown()):
print("Playback loop %d of %s" % (loop_cnt, loopstr,))
traj.parse_file(path.expanduser(args.file))
traj.init_trajectory()
traj.init_state_vector()
#for safe interrupt handling
traj.goal_iteration()
print("Dumping variables\n")
pickle_name = str(args.pickle) + "loop%d"%loop_cnt
traj.dump_variables(pickle_name)
print("Dumped\n")
time.sleep(5)
loop_cnt += 1
print("Exiting - File Playback Complete")
marker.type = Marker.SPHERE_LIST
marker.action = Marker.ADD
marker.scale.x = 2 * self.disk_radius
marker.scale.y = 2 * self.disk_radius
marker.scale.z = 2 * self.disk_radius
marker.color.a = 0.3
marker.color.r = 0.5
marker.color.g = 0.5
marker.color.b = 0.5
marker.lifetime = rospy.Duration.from_sec(1 / self.rate_hz)
for robot in self.robots:
marker.points.append(self.robots[robot].pose.position)
markers.markers.append(marker)
return markers
if __name__ == "__main__":
# pylint: disable=invalid-name
parser.add_argument("radius",
default=5.0,
type=float,
help="The radius of detection/communication. " +
"Default is 5.0")
myargv = rospy.myargv(sys.argv)[1:]
args = parser.parse_args(myargv)
DeltaDiskEmulator(rate=args.rate, disk_radius=args.radius)\
.run(vizualize=args.viz)
cmd += ['gui:=false']
if args.load_moveit:
cmd += ['load_moveit:=true']
if args.load_kitting_moveit:
cmd += ['load_kitting_moveit:=true']
if args.load_gantry_moveit:
cmd += ['load_gantry_moveit:=true']
if not args.development_mode:
os.environ['ARIAC_COMPETITION'] = '1'
print('Running command: ' + ' '.join(cmd))
if not args.dry_run:
try:
p = subprocess.Popen(cmd)
p.wait()
except KeyboardInterrupt:
pass
finally:
p.wait()
return p.returncode
if __name__ == '__main__':
# Filter out any special ROS remapping arguments.
# This is necessary if the script is being run from a ROS launch file.
import rospy
filtered_argv = rospy.myargv(sys.argv)
sys.exit(main(filtered_argv[1:]))
self.dim = dim
self.y = np.zeros((self.dim, 2), dtype=float)
def filter(self, x):
self.y[:, 0] = self.y[:, 1]
for i in range(self.dim):
self.y[i,
1] = (1.0 - self.alpha) * self.y[i, 0] + self.alpha * x[i]
return self.y[:, 1]
if __name__ == '__main__':
try:
sys.argv = rospy.myargv()
parser = argparse.ArgumentParser(
description='Fingervision args parser')
parser.add_argument(
'cam_id',
type=int, # default type is string
help='id of camera input') # required args
parser.add_argument(
'fitting_params_file', # default type is string
help='file path of linear fitting parameters') # required args
parser.add_argument(
'--interp_nx', type=int,
help='size of interpolation on X axis') # optional args
parser.add_argument(
'--interp_ny', type=int,
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-r', '--rate', type=int, metavar='FPS',
help="Rate at which to publish captured images", default=30)
parser.add_argument('-n', '--namespace', type=str, metavar='NS',
help="Namespace to publish images in", default='capture')
parser.add_argument('--no-raw', action='store_true',
help="Disable image_raw publisher")
parser.add_argument('--no-compressed', action='store_true',
help="Disable compressed publisher")
parser.add_argument('-f', '--raise-to-front', action='store_true',
help="When capturing a window: raise window to the front")
parser.add_argument('--region', nargs=4, type=int,
help="Region to capture relative to the extents of the window (x y w h). "
"To capture a 640*480 region offset by 360 from the left of captured "
"window and 193 from the top of the captured window, the arguments would "
"be \"360 193 640 480\"")
parser.add_argument('window', metavar='WINDOW', type=str,
help="Title of window to capture")
# Use rospy.myargv(..) to remove ROS remaps before passing argv to argparse, and then
# pass remaining args to argparse, except the first element (name of the script).
# This makes it possible to use ROS remapping arguments (such as '__name' and
# '__master') without upsetting argparse.
args = parser.parse_args(args=rospy.myargv(argv=sys.argv)[1:])
try:
# find the window with the name specified by the user
print(f"Searching for window '{args.window}' ..")
# TODO: consider using https://github.com/asweigart/PyGetWindow
# TODO: or https://pypi.org/project/screeninfo
extents = list(get_window_extents_by_title(args.window, exclude_titlebar=True))
E_LEFT=0; E_TOP=1; E_RIGHT=2; E_BOTTOM=3
img_width = extents[E_RIGHT] - extents[E_LEFT]
img_height = extents[E_BOTTOM] - extents[E_TOP]
print(f"Window extents: {extents}, dims: {img_width}x{img_height}")
if args.region:
# modify window extents based on region specified by user
extents[E_LEFT] += args.region[E_LEFT]
extents[E_TOP] += args.region[E_TOP]
extents[E_RIGHT] = extents[E_LEFT] + args.region[2]
extents[E_BOTTOM] = extents[E_TOP] + args.region[3]
img_width = extents[E_RIGHT] - extents[E_LEFT]
img_height = extents[E_BOTTOM] - extents[E_TOP]
print(f"Updated extents: {extents}, dims: {img_width}x{img_height}")
if args.raise_to_front:
print(f"Will try to push window '{args.window}' to the front ..")
bring_window_to_front(args.window)
except NoSuchWindowException as e:
sys.stderr.write(f"Couldn't find window '{args.window}, aborting'\n")
sys.exit(1)
# get d3dshot instance
# note: we pass the 'args.rate' arg as fps here, as we need d3dshot to
# capture at at least that rate to be able to achieve the requested
# publication rate
# TODO: move this to ctor of D3DShotPublisher, to avoid creating the instance
# but then not destroying it if we don't get to pass it to D3DShotPublisher
# below
dshot = setup_d3dshot(fps=args.rate, region=extents)
# wait a bit to give d3dshot time to fill the queue
print("Warming up d3dshot queue ..")
time.sleep(1)
print("ROS settings:\n"
f" Node name: '{rospy.get_name()}'\n"
f" Local IP: {rosgraph.network.get_local_address()}\n"
f" ROS Master: {rosgraph.get_master_uri()}"
)
# init ROS
# note: we do this here to avoid polluting the master with all sorts of ghost
# nodes when d3dshot errors-out during initialisation and we can't properly
# clean up after ourselves.
# At this point we can be reasonably certain d3dshot will work
rospy.init_node('d3dshot_publisher')
# TODO: could perhaps wrap the ROS side in a try-except to handle the
# tcpros exceptions thrown on SIGINT
print(f"Starting {args.rate} Hz publisher(s) for\n"
f" display: {dshot.display.name}\n"
f" adapter: {dshot.display.adapter_name}\n"
f" resolution: {dshot.display.resolution[0]}x{dshot.display.resolution[1]}"
)
# start publishing frames as ROS messages
with setup_d3dshot_pub(dshot, args, img_width, img_height) as shot_pub:
# exit from this context will also take down the d3dshot instance
shot_pub.spin()
def main():
rospy.init_node('ur_driver', disable_signals=True)
if rospy.get_param("use_sim_time", False):
rospy.logwarn("use_sim_time is set!!!")
global prevent_programming
reconfigure_srv = Server(URDriverConfig, reconfigure_callback)
prefix = rospy.get_param("~prefix", "")
print "Setting prefix to %s" % prefix
global joint_names
joint_names = [prefix + name for name in JOINT_NAMES]
# Parses command line arguments
parser = optparse.OptionParser(
usage="usage: %prog robot_hostname [reverse_port]")
(options, args) = parser.parse_args(rospy.myargv()[1:])
if len(args) < 1:
parser.error("You must specify the robot hostname")
elif len(args) == 1:
robot_hostname = args[0]
reverse_port = DEFAULT_REVERSE_PORT
elif len(args) == 2:
robot_hostname = args[0]
reverse_port = int(args[1])
if not (0 <= reverse_port <= 65535):
parser.error("You entered an invalid port number")
else:
parser.error("Wrong number of parameters")
# Reads the calibrated joint offsets from the URDF
global joint_offsets
joint_offsets = load_joint_offsets(joint_names)
if len(joint_offsets) > 0:
rospy.loginfo("Loaded calibration offsets from urdf: %s" %
joint_offsets)
else:
rospy.loginfo("No calibration offsets loaded from urdf")
# Reads the maximum velocity
# The max_velocity parameter is only used for debugging in the ur_driver. It's not related to actual velocity limits
global max_velocity
max_velocity = rospy.get_param("~max_velocity", MAX_VELOCITY) # [rad/s]
rospy.loginfo("Max velocity accepted by ur_driver: %s [rad/s]" %
max_velocity)
# Reads the minimum payload
global min_payload
min_payload = rospy.get_param("~min_payload", MIN_PAYLOAD)
# Reads the maximum payload
global max_payload
max_payload = rospy.get_param("~max_payload", MAX_PAYLOAD)
rospy.loginfo("Bounds for Payload: [%s, %s]" % (min_payload, max_payload))
# Sets up the server for the robot to connect to
server = TCPServer(("", reverse_port), CommanderTCPHandler)
thread_commander = threading.Thread(name="CommanderHandler",
target=server.serve_forever)
thread_commander.daemon = True
thread_commander.start()
with open(roslib.packages.get_pkg_dir('ur_driver') + '/prog') as fin:
program = fin.read() % {
"driver_hostname": get_my_ip(robot_hostname, PORT),
"driver_reverseport": reverse_port
}
connection = URConnection(robot_hostname, PORT, program)
connection.connect()
connection.send_reset_program()
connectionRT = URConnectionRT(robot_hostname, RT_PORT)
connectionRT.connect()
set_io_server()
service_provider = None
action_server = None
try:
while not rospy.is_shutdown():
# Checks for disconnect
if getConnectedRobot(wait=False):
time.sleep(0.2)
try:
prevent_programming = rospy.get_param(
"~prevent_programming")
update = {'prevent_programming': prevent_programming}
reconfigure_srv.update_configuration(update)
except KeyError, ex:
print "Parameter 'prevent_programming' not set. Value: " + str(
prevent_programming)
pass
if prevent_programming:
print "Programming now prevented"
connection.send_reset_program()
else:
print "Disconnected. Reconnecting"
if action_server:
action_server.set_robot(None)
rospy.loginfo("Programming the robot")
while True:
# Sends the program to the robot
while not connection.ready_to_program():
print "Waiting to program"
time.sleep(1.0)
try:
prevent_programming = rospy.get_param(
"~prevent_programming")
update = {'prevent_programming': prevent_programming}
reconfigure_srv.update_configuration(update)
except KeyError, ex:
print "Parameter 'prevent_programming' not set. Value: " + str(
prevent_programming)
pass
connection.send_program()
r = getConnectedRobot(wait=True, timeout=1.0)
if r:
break
rospy.loginfo("Robot connected")
#provider for service calls
if service_provider:
service_provider.set_robot(r)
else:
service_provider = URServiceProvider(r)
if action_server:
action_server.set_robot(r)
else:
action_server = URTrajectoryFollower(
r, rospy.Duration(1.0))
action_server.start()
def test_dialogue_irl(self):
# Get command line arguments
args = rospy.myargv()
path = args[1]
# Read file
file = open(path, 'r')
# Peek at first data line to discover number of columns
for line in file:
# Comment line, skips.
if (re.search("^\s*#", line)):
continue
# Data line trips.
if (re.search("(\s*[0-9\.]+\s+)+", line)):
# In-line comment, strips.
line = re.sub("#.*$", "", line)
# Count data elements
n_cols = len(line.split())
break
file.seek(0) # Return to start of file
# Read data.
obs = np.genfromtxt(path, usecols=(0), dtype='int', comments='#')
phrases = np.genfromtxt(path,
usecols=(1),
dtype='string',
comments='#')
vals = np.genfromtxt(path,
usecols=(range(2, n_cols)),
dtype='float',
comments='#')
# Check data dimensions
n_rows = len(vals)
self.assertGreaterEqual(
n_cols, 6,
"Need at least six columns of data for meaningful test file.")
self.assertGreaterEqual(
n_rows, 1,
"Need at least one row of data for meaningful test file.")
self.assertEquals(
n_cols % 2, 0,
"Mismatch between size of model likelihood and size of actions.")
n_models = (n_cols - 2) / 2
self.assertGreaterEqual(
n_models, 2, "Need at least two models for meaningful test file.")
# Slice data and convert type.
p_models = vals[:, :n_models].astype(float)
a_models = vals[:, n_models:].astype(int)
phrases = [phrase.split(',') for phrase in phrases]
# self.fail("data: %s\n%s\n%s\n%s\n"%(obs, phrases, p_models, a_models))
# Call the actual test function for each row
for i in range(0, n_rows):
self._test_oracle_irl(i, obs[i], phrases[i], p_models[i],
a_models[i])
twist = Twist()
keycode = -1
stdscr.addstr("Command\n")
stdscr.addstr(" - UP/DOWN : control linear x (+= or -=)\n")
stdscr.addstr(" - LEFT/RIGHT : control linear y (+= or -=)\n")
stdscr.addstr(" - e/r : control angular z (+= or -=)\n")
stdscr.addstr(" - any key : reset of the twist\n")
stdscr.addstr(" - ESC : reset twist and exit\n")
# We set the "wait for a key press" period to 100 ms.
if(persist): stdscr.timeout(100)
while (not rospy.is_shutdown()) and (keycode != 27): # 27 is escape
keycode = stdscr.getch() # Wait for a key press for at most 100ms
if keycode == -1 : pass # No key has been pressed, we keep current twist.
elif keycode == curses.KEY_UP : twist.linear.x = twist.linear.x + .1
elif keycode == curses.KEY_DOWN : twist.linear.x = twist.linear.x - .1
elif keycode == curses.KEY_LEFT : twist.linear.y = twist.linear.y + .1
elif keycode == curses.KEY_RIGHT : twist.linear.y = twist.linear.y - .1
elif keycode == 101 : twist.angular.z = twist.angular.z + .2
elif keycode == 114 : twist.angular.z = twist.angular.z - .2
else : twist = Twist()
pub.publish(twist)
rate.sleep()
# Starts curses (terminal handling) and run our main function.
if __name__ == '__main__':
persist = '--persist' in rospy.myargv(argv=sys.argv)
try:
curses.wrapper(lambda w: main(w,persist))
except rospy.ROSInterruptException:
pass
def main():
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
required = parser.add_argument_group('required arguments')
required.add_argument(
'-c', '--config', dest='config', required=True,
help='the file path of the demonstration '
)
required.add_argument(
'-i', '--input', dest='input_directory', required=True,
help='the input directory to pull in raw demonstration .json files'
)
required.add_argument(
'-o', '--output', dest='output_directory', required=True,
help='the output directory for saving relative distance labeled demonstration .json files'
)
args = parser.parse_args(rospy.myargv()[1:])
config_filepath = args.config
configs = import_configuration(config_filepath)
# Import the data
importer = DataImporter()
rospy.loginfo("Importing demonstration .json files...")
raw_demonstrations = importer.load_json_files(args.input_directory + "/*.json")
# Convert imported data into Demonstrations and Observations
demonstrations = []
for datum in raw_demonstrations["data"]:
observations = []
for entry in datum:
observations.append(Observation(entry))
demonstrations.append(Demonstration(observations))
args = parser.parse_args(rospy.myargv()[1:])
# Process data
ordp = ObjectRelativeDataProcessor([2, 3], 1)
ocp = ObjectContactProcessor([2, 3], 1, .06, .5)
soip = SphereOfInfluenceProcessor([2, 3], 1, .25)
for idx, demo in enumerate(demonstrations):
ordp.generate_relative_data(demo.observations)
ocp.generate_object_contact_data(demo.observations)
soip.generate_SOI_data(demo.observations)
print("\nDone processing.")
# Segment data
segmenter = ManualSegmentation()
for idx, demo in enumerate(demonstrations):
segmenter.segment(demo.observations)
print("\nDone segment.")
# Export data
rospy.loginfo("Exporting demonstrations.")
exp = DataExporter()
for idx, demo in enumerate(demonstrations):
labeled_data = [obs.data for obs in demo.observations]
exp.export_to_json(args.output_directory + "/labeled_demonstration{}.json".format(idx), labeled_data)
print("\nJust plain done.")
def callback(data):
""" function docstring, yo! """
tmpp = "[" + str(data.linear.x) + "," + str(data.linear.y) + "," + str(
data.angular.z) + "]"
try:
result = requests.post(url=URL, params=PARAMS, data=tmpp)
if result.status_code != 200:
rospy.logwarn("post to %s with params %s failed", URL, PARAMS)
except requests.exceptions.RequestException as exception_e:
rospy.logerr("%s", exception_e)
def listener():
""" function docstring, yo! """
rospy.init_node('robotino_omnidrive', anonymous=True)
rospy.Subscriber("cmd_vel", Twist, callback)
rospy.spin()
if __name__ == '__main__':
MYARGV = rospy.myargv(argv=sys.argv)
if len(MYARGV) > 1:
URL = URL.replace("127.0.0.1", MYARGV[1])
rospy.loginfo("connecting to: %s", URL)
try:
listener()
except rospy.ROSInterruptException:
pass
def __init__(self):
parser = argparse.ArgumentParser(
description='Spawn a model in gazebo using the ROS API')
xmlformat = parser.add_mutually_exclusive_group(required=True)
xmlformat.add_argument('-urdf',
action='store_true',
help='Incoming xml is in urdf format')
xmlformat.add_argument('-sdf',
action='store_true',
help='Incoming xml is in sdf format')
source = parser.add_mutually_exclusive_group(required=True)
source.add_argument('-file',
type=str,
metavar='FILE_NAME',
help='Load model xml from file')
source.add_argument('-param',
type=str,
metavar='PARAM_NAME',
help='Load model xml from ROS parameter')
source.add_argument(
'-database',
type=str,
metavar='MODEL_NAME',
help='Load model XML from specified model in Gazebo Model Database'
)
source.add_argument('-stdin',
action='store_true',
help='Load model from stdin')
parser.add_argument('-model',
required=True,
type=str,
metavar='MODEL_NAME',
help='Name of model to spawn')
parser.add_argument(
'-reference_frame',
type=str,
default='',
help='Name of the model/body where initial pose is defined.\
If left empty or specified as "world", gazebo world frame is used'
)
parser.add_argument(
'-gazebo_namespace',
type=str,
default='/gazebo',
help=
'ROS namespace of gazebo offered ROS interfaces. Defaults to /gazebo/'
)
parser.add_argument('-robot_namespace',
type=str,
default=rospy.get_namespace(),
help='change ROS namespace of gazebo-plugins')
parser.add_argument(
'-unpause',
action='store_true',
help='!!!Experimental!!! unpause physics after spawning model')
parser.add_argument('-wait',
type=str,
metavar='MODEL_NAME',
help='!!!Experimental!!! wait for model to exist')
parser.add_argument('-x',
type=float,
default=0,
help='x component of initial position, meters')
parser.add_argument('-y',
type=float,
default=0,
help='y component of initial position, meters')
parser.add_argument('-z',
type=float,
default=0,
help='z component of initial position, meters')
parser.add_argument('-R',
type=float,
default=0,
help='roll angle of initial orientation, radians')
parser.add_argument('-P',
type=float,
default=0,
help='pitch angle of initial orientation, radians')
parser.add_argument('-Y',
type=float,
default=0,
help='yaw angle of initial orientation, radians')
parser.add_argument(
'-J',
dest='joints',
default=[],
action='append',
metavar=('JOINT_NAME', 'JOINT_POSITION'),
type=str,
nargs=2,
help='initialize the specified joint at the specified position')
parser.add_argument(
'-package_to_model',
action='store_true',
help=
'convert urdf <mesh filename="package://..." to <mesh filename="model://..."'
)
parser.add_argument(
'-b',
dest='bond',
action='store_true',
help=
'bond to gazebo and delete the model when this program is interrupted'
)
args = rospy.myargv()
self.args = parser.parse_args(args[1:])
# Convert position of joints to floats
for i in range(len(self.args.joints)):
self.args.joints[i][1] = float(self.args.joints[i][1])
from sensor_msgs.msg import Image
class FilterDepth:
def __init__(self, min_depth, max_depth):
self.min_depth = min_depth
self.max_depth = max_depth
self.subscriber = rospy.Subscriber('input', Image, self.depthCallback)
self.publisher = rospy.Publisher("output", Image, queue_size=10)
self.cv_bridge = cv_bridge.CvBridge()
def depthCallback(self, input_msg):
input_depth = self.cv_bridge.imgmsg_to_cv2(
input_msg, desired_encoding='passthrough')
output_depth = np.where(
np.logical_and(input_depth >= self.min_depth,
input_depth <= self.max_depth), input_depth, np.inf)
output_msg = self.cv_bridge.cv2_to_imgmsg(output_depth,
encoding="passthrough")
output_msg.header = input_msg.header
self.publisher.publish(output_msg)
if __name__ == "__main__":
rospy.init_node("filter_depth", log_level=rospy.DEBUG)
overrider = FilterDepth(
*[float(limit) for limit in rospy.myargv(sys.argv)[1:3]])
rospy.spin()
import sys
from cv2 import cv2
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
def start_node(filename):
rospy.init_node('image_pub')
rospy.loginfo('image_pub node has started')
img = cv2.imread(filename=filename)
# cv2.imshow("image", img)
# cv2.waitKey(2000)
bridge = CvBridge()
imgMsg = bridge.cv2_to_imgmsg(cvim=img, encoding="bgr8")
pub = rospy.Publisher(name='image', data_class=Image, queue_size=10)
# pub = rospy.Publisher('image', Image, queue_size=10)
while not rospy.is_shutdown():
pub.publish(imgMsg)
rospy.Rate(1.0).sleep() # 1 Hz
if __name__ == "__main__":
try:
start_node(rospy.myargv(argv=sys.argv)[1])
except rospy.ROSInterruptException:
pass
def main():
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
required = parser.add_argument_group('required arguments')
required.add_argument(
'-c',
'--config',
dest='config',
required=True,
help='the file path of configuration config.json file ')
required.add_argument(
'-d',
'--directory',
dest='directory',
required=True,
help=
'the directory from which to input labeled demonstration .json files')
parser.add_argument('-b',
'--bandwidth',
type=float,
default=.025,
metavar='BANDWIDTH',
help='gaussian kernel density bandwidth')
parser.add_argument('-t',
'--threshold',
type=int,
metavar='THRESHOLD',
help='log-liklihood threshold value')
parser.add_argument(
'-n',
'--number_of_samples',
type=int,
default=50,
metavar='NUMBEROFSAMPLES',
help='the number of samples to validate for each keyframe')
args = parser.parse_args(rospy.myargv()[1:])
rospy.init_node("graph_traverse")
# Import the data
importer = DataImporter()
config_filepath = args.config
configs = import_configuration(config_filepath)
labeled_demonstrations = importer.load_json_files(args.directory +
"/*.json")
# Convert imported data into Demonstrations and Observations
demonstrations = []
for datum in labeled_demonstrations["data"]:
observations = []
for entry in datum:
observations.append(Observation(entry))
demonstrations.append(Demonstration(labeled_observations=observations))
if len(demonstrations) == 0:
rospy.logwarn("No demonstration data to model!!")
return 0
""" Create the moveit_interface """
moveit_interface = SawyerMoveitInterface()
moveit_interface.set_velocity_scaling(.35)
moveit_interface.set_acceleration_scaling(.25)
acclfd = ACC_LFD(configs, moveit_interface)
acclfd.build_environment()
acclfd.build_keyframe_graph(demonstrations, args.bandwidth)
acclfd.generate_metaconstraints(demonstrations)
if args.threshold is not None:
cclfd.sample_keyframes(args.number_of_samples,
automate_threshold=False,
culling_threshold=args.threshold)
else:
cclfd.sample_keyframes(args.number_of_samples, automate_threshold=True)
acclfd.perform_skill()
self.frame.SetSizer(self.sz)
self.set_goal(None)
self.sz.Layout()
self.frame.Show()
return True
if __name__ == '__main__':
rospy.init_node('axserver', anonymous=True)
parser = OptionParser(__doc__.strip())
# parser.add_option("-t","--test",action="store_true", dest="test",default=False,
# help="A testing flag")
# parser.add_option("-v","--var",action="store",type="string", dest="var",default="blah")
(options, args) = parser.parse_args(rospy.myargv())
if (len(args) != 3):
parser.error(
"You must specify the action name and type. Eg: ./axserver.py my_action actionlib/Test"
)
action = DynamicAction(args[2])
app = AXServerApp(action, args[1])
app.MainLoop()
rospy.signal_shutdown('GUI shutdown')
[pose_stamped.pose.orientation.x, pose_stamped.pose.orientation.y, \
pose_stamped.pose.orientation.z, pose_stamped.pose.orientation.w], \
dtype=np.float64)
new_trans = np.matmul(self.R,
(src_trans - self.gnss_cog)) + self.map_cog
new_quat = quaternion_multiply(self.quat, src_quat)
new_pose = PoseStamped()
new_pose.header = pose_stamped.header
new_pose.pose.position.x = new_trans[0]
new_pose.pose.position.y = new_trans[1]
new_pose.pose.position.z = new_trans[2]
new_pose.pose.orientation.x = new_quat[0]
new_pose.pose.orientation.y = new_quat[1]
new_pose.pose.orientation.z = new_quat[2]
new_pose.pose.orientation.w = new_quat[3]
self.pub.publish(new_pose)
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('-world_to_map_json', type=str, required=True)
args = parser.parse_args(rospy.myargv()[1:])
# X. Run subscriber.
transformer = PoseTransformer(args.world_to_map_json)
#movec(-0.6,0.9,0) #-0
goal = "B"
elif(state==2):
#moveb(0,0,0)
moveb(0,0,0)
goal = "O"
elif(state==4):
#moveb(0,0,0)
moved(0,0,0)
goal = "O"
else:
moveb(0,0,0)
goal = "O"
return navigoalResponse(
success=True,
message="navigate to "+goal+" is executed!"
)
if __name__ == '__main__':
rospy.init_node('pmcT', anonymous=True)
rospy.Subscriber("/pause", Bool, callback)
ns = rospy.myargv(argv=sys.argv)[1]
cmdVelTopic=os.path.join(ns,'diff_drive_controller','cmd_vel')
twistPub = rospy.Publisher(cmdVelTopic, Twist, queue_size=1)
rospy.wait_for_service('robot_tf_server')
tfReq = rospy.ServiceProxy('robot_tf_server', RobotTF)
navi_srv = rospy.Service('triggerNavigating',navigoal, naviflow)
print("Patrol Start!")
rospy.spin()
def get_array_from_quaternion(quaternion):
arry = (quaternion.x, quaternion.y, quaternion.z, quaternion.w)
return tf.transformations.euler_from_quaternion(arry)
def get_quaternion_from_array(arry):
return Quaternion(x=arry[0], y=arry[1], z=arry[2], w=arry[3])
def writeGrasps(grasps, filename):
stream = file(filename, 'w')
args = []
for grasp in grasps:
str = genpy.message.strify_message(grasp)
args.append(yaml.load(str))
yaml.dump(args, stream)
print(yaml.dump(args))
if __name__ == '__main__':
argv = sys.argv
argv = rospy.myargv(argv)
if (len(argv) == 1):
usage()
filename = argv[1]
output = argv[2]
base_grasp = load_grasps(filename)
grasps = generate_grasps(base_grasp, 36)
writeGrasps(grasps, output)
def main():
topic = rospy.myargv()[1]
rospy.init_node("channelecho", anonymous=True)
rospy.Subscriber(topic, BufferedData, dataCB)
rospy.spin()
port_name = rospy.get_param('~port', '/dev/ttyACM0')
baud = int(rospy.get_param('~baud', '57600'))
# for systems where pyserial yields errors in the fcntl.ioctl(self.fd, TIOCMBIS, \
# TIOCM_DTR_str) line, which causes an IOError, when using simulated port
fix_pyserial_for_test = rospy.get_param('~fix_pyserial_for_test', False)
# TODO: should these really be global?
tcp_portnum = int(
rospy.get_param('/rosserial_embeddedlinux/tcp_port', '11411'))
fork_server = rospy.get_param('/rosserial_embeddedlinux/fork_server',
False)
# TODO: do we really want command line params in addition to parameter server params?
sys.argv = rospy.myargv(argv=sys.argv)
if len(sys.argv) >= 2:
port_name = sys.argv[1]
if len(sys.argv) == 3:
tcp_portnum = int(sys.argv[2])
if port_name == "tcp":
server = RosSerialServer(tcp_portnum, fork_server)
rospy.loginfo("Waiting for socket connections on port %d" %
tcp_portnum)
try:
server.listen()
except KeyboardInterrupt:
rospy.loginfo("got keyboard interrupt")
finally:
rospy.loginfo("Shutting down")
detectedPerson.pose = compositeDetectedPerson.pose
involvedModalities = list(
set([
originalDetection.modality for originalDetection in
compositeDetectedPerson.original_detections
]))
detectedPerson.modality = ",".join(sorted(involvedModalities))
detectedPersons.detections.append(detectedPerson)
detectedPersonsPublisher.publish(detectedPersons)
if __name__ == '__main__':
arguments = rospy.myargv()
rospy.init_node("composite_detections_to_detections")
compositeDetectedPersonsTopic = "/spencer/perception/detected_persons_composite"
compositeDetectedPersonsSubscriber = rospy.Subscriber(
compositeDetectedPersonsTopic,
CompositeDetectedPersons,
newCompositeDetectedPersonsAvailable,
queue_size=5)
global detectedPersonsPublisher
detectedPersonsTopic = "/spencer/perception/detected_persons"
detectedPersonsPublisher = rospy.Publisher(detectedPersonsTopic,
DetectedPersons,
queue_size=5)
stats = neat.StatisticsReporter()
p.add_reporter(stats)
p.add_reporter(
neat.Checkpointer(generation_interval=10, time_interval_seconds=None))
p.run(eval_genomes, max_iter)
if __name__ == '__main__':
# Determine path to configuration file. This path manipulation is
# here so that the script will run successfully regardless of the
# current working directory.
# rospy.init_node('network_gen', anonymous=True)
rospy.init_node('network_gen')
argv = rospy.myargv()
for i in range(20):
gene_id_publisher('-')
time.sleep(0.25)
local_dir = os.path.dirname(__file__)
config_path = os.path.join(local_dir, 'config-feedforward')
if not os.path.exists("%s/genes" % local_dir):
os.makedirs("%s/genes" % local_dir, 0766)
else:
gene_clear()
if argv[2] == '-r':
run(config_path, argv[1])
dest="ignore_self",
help="Ignore self NTP test",
action="store_true")
options, args = parser.parse_args(rospy.myargv())
if (len(args) != 2):
parser.error("Invalid arguments. Must have HOSTNAME [args]. %s" % args)
try:
offset = int(options.offset_tol)
self_offset = int(options.self_offset_tol)
error_offset = int(options.error_offset_tol)
ignore_self = options.ignore_self
except:
parser.error("Offsets must be numbers")
ntp_monitor(args[1], offset, self_offset, options.diag_hostname,
error_offset, ignore_self)
if __name__ == "__main__":
try:
ntp_monitor_main(rospy.myargv())
except KeyboardInterrupt:
pass
except SystemExit:
pass
except:
import traceback
traceback.print_exc()
def main():
"""
Set the desired interaction control options in the current configuration.
Note that the arm is not commanded to move but it will have the specified
interaction control behavior.
If publish the rate is 0, the interaction control command is only published
once; otherwise a last position command will be sent when the script exits.
For non-zero publishing rates, the arm will go back into constrained zero-G
if the arm's zero-g button is pressed and relased. Future motion commands
will use the interaction parameters set in the trajectory options.
Call using:
$ rosrun intera_examples set_interaction_options.py [arguments: see below]
-s 1
--> Set interaction active to True (0 for False) in the current configuration
-k 500.0 500.0 500.0 10.0 10.0 10.0
--> Set K_impedance to [500.0 500.0 500.0 10.0 10.0 10.0] in the current configuration
-m 1 1 0 1 1 1
--> Set max_impedance to [True True False True True True] in the current configuration
-md 1 1 2 1 1 1
--> Set interaction_control_mode to [impedance, impedance, force, impedance, impedance, impedance]
in the current configuration (1: impedance, 2: force, 3: impedance w/ force limit,
4: force w/ motion limit)
-fr 0.1 0.2 0.3 1 0 0 0
--> Set the pose of the interaction_frame -- position: (0.1, 0.2, 0.3) and orientation (1, 0, 0, 0)
-ef
--> Set in_endpoint_frame to True in the current configuration (use TCP frame as reference frame)
-f 0.0 0.0 30.0 0.0 0.0 0.0
--> Set force_command to [0.0 0.0 30.0 0.0 0.0 0.0] in the current configuration
-kn 5.0 3.0 5.0 4.0 6.0 4.0 6.0
--> Set K_nullspace to [5.0 3.0 5.0 4.0 6.0 4.0 6.0] in the current configuration
-r 20
--> Set desired publish rate (Hz)
-r 0
--> The interaction command is published once and exits. The arm can remain
in interaction control after this script.
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
parser.add_argument(
"-s",
"--interaction_active",
type=int,
default=1,
choices=[0, 1],
help="Activate (1) or Deactivate (0) interaction controller")
parser.add_argument(
"-k",
"--K_impedance",
type=float,
nargs='+',
default=[1300.0, 1300.0, 1300.0, 30.0, 30.0, 30.0],
help=
"A list of desired stiffnesses, one for each of the 6 directions -- stiffness units are (N/m) for first 3 and (Nm/rad) for second 3 values"
)
parser.add_argument(
"-m",
"--max_impedance",
type=int,
nargs='+',
default=[1, 1, 1, 1, 1, 1],
choices=[0, 1],
help=
"A list of maximum stiffness behavior state, one for each of the 6 directions (a single value can be provided to apply the same value to all the directions) -- 0 for False, 1 for True"
)
parser.add_argument(
"-md",
"--interaction_control_mode",
type=int,
nargs='+',
default=[1, 1, 1, 1, 1, 1],
choices=[1, 2, 3, 4],
help=
"A list of desired interaction control mode (1: impedance, 2: force, 3: impedance with force limit, 4: force with motion limit), one for each of the 6 directions"
)
parser.add_argument(
"-fr",
"--interaction_frame",
type=float,
nargs='+',
default=[0, 0, 0, 1, 0, 0, 0],
help=
"Specify the reference frame for the interaction controller -- first 3 values are positions [m] and last 4 values are orientation in quaternion (w, x, y, z) which has to be normalized values"
)
parser.add_argument(
"-ef",
"--in_endpoint_frame",
action='store_true',
default=False,
help=
"Set the desired reference frame to endpoint frame; otherwise, it is base frame by default"
)
parser.add_argument(
"-en",
"--endpoint_name",
type=str,
default='right_hand',
help=
"Set the desired endpoint frame by its name; otherwise, it is right_hand frame by default"
)
parser.add_argument(
"-f",
"--force_command",
type=float,
nargs='+',
default=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
help=
"A list of desired force commands, one for each of the 6 directions -- in force control mode this is the vector of desired forces/torques to be regulated in (N) and (Nm), in impedance with force limit mode this vector specifies the magnitude of forces/torques (N and Nm) that the command will not exceed"
)
parser.add_argument(
"-kn",
"--K_nullspace",
type=float,
nargs='+',
default=[5.0, 10.0, 5.0, 10.0, 5.0, 10.0, 5.0],
help=
"A list of desired nullspace stiffnesses, one for each of the 7 joints (a single value can be provided to apply the same value to all the directions) -- units are in (Nm/rad)"
)
parser.add_argument("-dd",
"--disable_damping_in_force_control",
action='store_true',
default=False,
help="Disable damping in force control")
parser.add_argument(
"-dr",
"--disable_reference_resetting",
action='store_true',
default=False,
help=
"The reference signal is reset to actual position to avoid jerks/jumps when interaction parameters are changed. This option allows the user to disable this feature."
)
parser.add_argument(
"-rc",
"--rotations_for_constrained_zeroG",
action='store_true',
default=False,
help=
"Allow arbitrary rotational displacements from the current orientation for constrained zero-G (works only with a stationary reference orientation)"
)
parser.add_argument(
"-r",
"--rate",
type=int,
default=10,
help=
"A desired publish rate for updating interaction control commands (10Hz by default) -- a rate 0 publish once and exits which can cause the arm to remain in interaction control."
)
args = parser.parse_args(rospy.myargv()[1:])
rospy.init_node('set_interaction_options')
# set the interaction control options in the current configuration
interaction_options = InteractionOptions()
interaction_options.set_interaction_control_active(
int2bool(args.interaction_active))
interaction_options.set_K_impedance(args.K_impedance)
interaction_options.set_max_impedance(int2bool(args.max_impedance))
interaction_options.set_interaction_control_mode(
args.interaction_control_mode)
interaction_options.set_in_endpoint_frame(args.in_endpoint_frame)
interaction_options.set_force_command(args.force_command)
interaction_options.set_K_nullspace(args.K_nullspace)
if len(args.interaction_frame) == 7:
interaction_frame = Pose()
interaction_frame.position.x = args.interaction_frame[0]
interaction_frame.position.y = args.interaction_frame[1]
interaction_frame.position.z = args.interaction_frame[2]
interaction_frame.orientation.w = args.interaction_frame[3]
interaction_frame.orientation.x = args.interaction_frame[4]
interaction_frame.orientation.y = args.interaction_frame[5]
interaction_frame.orientation.z = args.interaction_frame[6]
interaction_options.set_interaction_frame(interaction_frame)
else:
rospy.logerr('Invalid input to interaction_frame. Must be 7 elements.')
interaction_options.set_disable_damping_in_force_control(
args.disable_damping_in_force_control)
interaction_options.set_disable_reference_resetting(
args.disable_reference_resetting)
interaction_options.set_rotations_for_constrained_zeroG(
args.rotations_for_constrained_zeroG)
# print the resultant interaction options once
rospy.loginfo(interaction_options.to_msg())
ic_pub = InteractionPublisher()
rospy.sleep(0.5)
if args.rate != 0:
rospy.on_shutdown(ic_pub.send_position_mode_cmd)
ic_pub.send_command(interaction_options, args.rate)
if args.rate == 0:
rospy.sleep(0.5)
def main():
"""RSDK Joint Position Example: Joystick Control
Use a game controller to control the angular joint positions
of Baxter's arms.
Attach a game controller to your dev machine and run this
example along with the ROS joy_node to control the position
of each joint in Baxter's arms using the joysticks. Be sure to
provide your *joystick* type to setup appropriate key mappings.
Each stick axis maps to a joint angle; which joints are currently
controlled can be incremented by using the mapped increment buttons.
Ex:
(x,y -> e0,e1) >>increment>> (x,y -> e1,e2)
"""
epilog = """
See help inside the example with the "Start" button for controller
key bindings.
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__,
epilog=epilog)
required = parser.add_argument_group('required arguments')
required.add_argument('-j',
'--joystick',
required=True,
choices=['xbox', 'logitech', 'ps3'],
help='specify the type of joystick to use')
args = parser.parse_args(rospy.myargv()[1:])
joystick = None
if args.joystick == 'xbox':
joystick = baxter_external_devices.joystick.XboxController()
elif args.joystick == 'logitech':
joystick = baxter_external_devices.joystick.LogitechController()
elif args.joystick == 'ps3':
joystick = baxter_external_devices.joystick.PS3Controller()
else:
parser.error("Unsupported joystick type '%s'" % (args.joystick))
print("Initializing node... ")
rospy.init_node("rsdk_joint_position_joystick")
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
vrepSimStart = rospy.ServiceProxy('vrep/simRosStartSimulation',
simRosStartSimulation)
vrepSimStop = rospy.ServiceProxy('vrep/simRosStopSimulation',
simRosStopSimulation)
def clean_shutdown():
print("\nExiting example.")
vrepSimStop.call()
if not init_state:
print("Disabling robot...")
rs.disable()
rospy.on_shutdown(clean_shutdown)
rospy.loginfo("Enabling robot... ")
rs.enable()
vrepSimStart.call()
# pose_pub = rospy.Publisher("/ik_baxter/d_pose", PoseStamped,queue_size=1)
# map_joystick(joystick,pose_pub)
map_joystick(joystick)
print("Done.")
p.x = pose.position.x - int_marker.pose.position.x
p.y = pose.position.y - int_marker.pose.position.y
marker.points.append(p)
p = Point()
pose = self._soma_obj_pose[points[0]]
p.x = pose.position.x - int_marker.pose.position.x
p.y = pose.position.y - int_marker.pose.position.y
marker.points.append(p)
return int_marker
if __name__ == "__main__":
# TODO: add list command
parser = argparse.ArgumentParser(prog='soma_roi.py')
parser.add_argument("map", nargs=1, help='Name of the used 2D map')
parser.add_argument("conf",
nargs=1,
help='Name of the object configuration')
parser.add_argument('-t', metavar='config-file')
args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:])
rospy.init_node("soma")
rospy.loginfo("Running SOMA (map: %s, conf: %s, types: %s)", args.map[0],
args.conf[0], args.t)
SOMAROIManager(args.map[0], args.conf[0], args.t)
#!/usr/bin/env python
import roslib
roslib.load_manifest('alpha_dashboard')
import sys
import subprocess
import rospy
## Alternate approach by using roslaunchAPIs for python
# See here: http://wiki.ros.org/roslaunch/API%20Usage
# Approach stated here: https://answers.ros.org/question/272267/is-it-possible-to-launch-non-ros-applications-with-roslaunch/
if __name__ == "__main__":
if len(sys.argv) > 2:
print("Usage: rqt_alpha_console.py file")
else:
command = "rqt --perspective-file" + (rospy.myargv(argv=sys.argv)[1])
sys.exit(subprocess.call(command))
