def update_subscribers(self): # check connections # for name in self.subscribers.keys(): # sub = self.subscribers[name] # if sub.get_num_connections() == 0: # sub.unregister() # self.subscribers.pop(name) # rospy.loginfo('unsubscribe (%s)',name) # check new publishers current_subscribers = rospy.client.get_published_topics() for topic_info in current_subscribers: name = topic_info[0] typ_tuple = tuple(topic_info[1].split('/')) typ = '%s.msg.%s'%typ_tuple if typ in self.useless_types: continue if name in self.subscribers.keys(): continue # Import and Check try: type_obj = eval(typ)() except (AttributeError, NameError), e: try: rospy.loginfo("try to load [%s]", typ_tuple[0]); roslib.load_manifest(typ_tuple[0]) exec('import ' + typ_tuple[0] + '.msg') except SyntaxError, e: rospy.loginfo('please rosmake %s', typ_tuple[0])
def readAll(self): # hbase> get "session_table", "row_key", "column_name" result = self.hbaseclient.get("session_table", "0", "timestamp:" + self.session_id) num_entries = result[0].value for i in range(1, int(num_entries) + 1): row_time = self.hbaseclient.get("session_table", "%d" % i, "timestamp:" + self.session_id) timestamp = row_time[0].value print "timestamp: " + timestamp row_topic = self.hbaseclient.get("session_table", "%d" % i, "topic:" + self.session_id) topic = row_topic[0].value print "topic: " + topic row_msg = self.hbaseclient.get("session_table", "%d" % i, "msg:" + self.session_id) msg_value = row_msg[0].value msg_type = self.topictype_dict[topic] print "msg type: " + msg_type if msg_type not in msgs: m = msg_type.split(".") roslib.load_manifest(m[0]) _tmp = __import__(m[0] + ".msg", globals(), locals(), [m[-1]]) msgs[msg_type] = getattr(_tmp, m[-1]) obj = msgs.get(msg_type)() obj.deserialize(msg_value) print "msg:\n", obj, "\n"
def setup_topic_data(self): for topic, topic_type in self.config.items(): if topic not in self.msg_parameters: self.topic_name_list.append(topic) # Only one type per topic type_key = topic_type.keys()[0] self.subscribed_list.append(type_key) # One set of data items per topic type self.data_items = [data_item for data_item in topic_type[type_key].keys()] # Add data items to the MTConnect Adapter - must be unique data items bridge_library.add_event((self.adapter, self.data_items, self.di_dict, False)) # Extract package name and topic type name tokens = topic_type.keys()[0].split('/') package = tokens[0] type_name = tokens[1] # Load package manifest if unique if package not in self.lib_manifests: roslib.load_manifest(package) self.lib_manifests.append(package) # Import module and create topic type class, # i.e. append <class 'mtconnect_msgs.msg._RobotStates.RobotStates'> rospy.loginfo('Class Instance --> ' + package + '.msg.' + type_name) type_handle = getattr(import_module(package + '.msg'), type_name) self.topic_type_list[topic] = type_handle self.msg_text[topic] = type_handle._full_text self.member_types[topic] = type_handle._slot_types self.member_names[topic] = type_handle.__slots__ return
def coverage_html(): import os.path if not os.path.isfile('.coverage-modules'): sys.stderr.write("No .coverage-modules file; nothing to do\n") return with open('.coverage-modules','r') as f: modules = [x for x in f.read().split('\n') if x.strip()] cov = coverage.coverage() cov.load() # import everything for m in modules: try: base = m.split('.')[0] roslib.load_manifest(base) __import__(m) except: sys.stderr.write("WARN: cannot import %s\n"%(base)) modlist = '\n'.join([" * %s"%m for m in modules]) sys.stdout.write("Generating for\n%s\n"%(modlist)) # load the module instances to pass to coverage so it can generate annotation html reports mods = [] # TODO: rewrite, buggy for m in modules: mods.extend([v for v in sys.modules.values() if v and v.__name__.startswith(m) and not v in mods]) # dump the output to covhtml directory cov.html_report(mods, directory="covhtml")
def setup_topic_data(self): for package, action in self.config.items(): if package not in self.msg_parameters: # Only one ROS package in config by design # Load package manifest if unique if package not in self.lib_manifests: roslib.load_manifest(package) self.lib_manifests.append(package) # Import module rospy.loginfo('Importing --> ' + package + '.msg') self.type_handle = import_module(package + '.msg') # Iterate through requests and create action result class instances and action servers for request in action: # Capture action name for action client callback reference self.action_list[request] = request # Store the ROS server name in a hash table di_conv = bridge_library.split_event(request) self.server_name[di_conv] = request # Create a dictionary of result class instances request_class = getattr(self.type_handle, request + 'Result') self._resultDict[request] = request_class() # Create instance of the action class action_class = getattr(self.type_handle, request + 'Action') # Create action server for requested action, store in a hash table self._as[request] = actionlib.SimpleActionServer(request + 'Client', action_class, self.execute_cb, False) # Start the action server self._as[request].start() return
def kinfu(self, basename, type = 'list_ext'): """ Use Kinect fusion on list of depth images, and store cloud Usage: kinfu(basename, type = 'list_ext') Input: basename - Path to file that contains depth image names. type{'list_ext'} - List file type ('list' or 'list_ext') Output: cloud, poses - Filenames to output cloud and poses from kinfu """ roslib.load_manifest('kinfu') import kinfu depth_textfile = self.getName(basename, type) # Need to read list and export using full names fullnames = list() with open(depth_textfile, 'rt') as file_: for name in file_: fullnames.append( self.getName(name.rstrip(), 'depth') ) output_prefix = os.path.join(self.getPath(type), basename) + '_' KF = kinfu.KinFuPy('file') cloud, poses = KF.kinfu_list(fullnames, output_prefix, opts='singlepass') # Store cloud and poses utils.save(self.getName(basename, 'cloud'), {'cloud': cloud}, \ zipped=True) utils.save(self.getName(basename, 'poses'), {'poses': poses}, \ zipped=True) return cloud, poses
def get_tf_listener(): import roslib roslib.load_manifest("tf") import tf return tf.TransformListener()
def __init__(self, action_name, action_type, queue_size=1): self.name = action_name action_type_module, action_type_name = tuple(action_type.split('/')) roslib.load_manifest(action_type_module) msg_module = import_module(action_type_module + '.msg') self.rostype_name = action_type self.rostype_action = getattr(msg_module, action_type_name + 'Action') self.rostype_action_goal = getattr(msg_module, action_type_name + 'ActionGoal') self.rostype_action_result = getattr(msg_module, action_type_name + 'ActionResult') self.rostype_action_feedback = getattr(msg_module, action_type_name + 'ActionFeedback') self.rostype_goal = getattr(msg_module, action_type_name + 'Goal') self.rostype_result = getattr(msg_module, action_type_name + 'Result') self.rostype_feedback = getattr(msg_module, action_type_name + 'Feedback') self.status_msg = deque([], queue_size) self.status_sub = rospy.Subscriber(self.name + '/' +self.STATUS_SUFFIX, actionlib_msgs.msg.GoalStatusArray, self.status_callback) self.result_msg = deque([], queue_size) self.result_sub = rospy.Subscriber(self.name + '/' + self.RESULT_SUFFIX, self.rostype_action_result, self.result_callback) self.feedback_msg = deque([], queue_size) self.feedback_sub = rospy.Subscriber(self.name + '/' +self.FEEDBACK_SUFFIX, self.rostype_action_feedback, self.feedback_callback) self.goal_pub = rospy.Publisher(self.name + '/' + self.GOAL_SUFFIX, self.rostype_action_goal) self.cancel_pub = rospy.Publisher(self.name + '/' +self.CANCEL_SUFFIX, actionlib_msgs.msg.GoalID)
def _loadClass(modname, subname, classname): """ Loads the manifest and imports the module that contains the specified type Returns the loaded module, or None on failure """ global loaded_manifests, loaded_modules if not (modname in loaded_manifests): try: roslib.load_manifest(modname) loaded_manifests[modname] = True; except: print "Unable to load manifest for module %s" % (modname,) return None fullname = modname + '.'+ subname + '._' + classname if not (fullname in loaded_modules): try: loaded_modules[fullname] = __import__(fullname) except: print "Unable to import %s" % (fullname,) return None # Module has been successfully imported return loaded_modules[fullname]
def hdf2bag(pargs): import roslib #@UnresolvedImport roslib.load_manifest('bootstrapping_adapter') #@UnresolvedImport parser = OptionParser(usage=usage) parser.disable_interspersed_args() (options, args) = parser.parse_args(pargs) #@UnusedVariable if len(args) != 1: msg = 'I expect only one filename.' raise Exception(msg) basedir = args[0] global_config_file = os.path.join(basedir, 'robot_info.yaml') if not os.path.exists(global_config_file): raise Exception('Configuration file %r not found.' % global_config_file) global_config = yaml.load(open(global_config_file)) print global_config.__repr__() for hdf_filename in glob(os.path.join(basedir, '*.h5')): bag_filename = os.path.splitext(hdf_filename)[0] + '.bag' id_episode = os.path.basename(os.path.splitext(hdf_filename)[0]) pg('hdf2bag_conversion', dict(hdf=hdf_filename, bag=bag_filename, id_robot=global_config['id_robot'], id_actuators=global_config['id_actuators'], id_sensors=global_config['id_sensors'], id_episode=id_episode, id_environment=global_config['id_environment'], commands_spec=global_config['commands_spec']))
def testPlanePointcloud(): import roslib; roslib.load_manifest('laser_camera_segmentation') import laser_camera_segmentation.processor as processor import laser_camera_segmentation.configuration as configuration cfg = configuration.configuration('/home/martin/robot1_data/usr/martin/laser_camera_segmentation/labeling') #sc = scanner.scanner(cfg) pc = processor.processor(cfg) #pc.load_data('2009Oct30_162400') pc.load_data('2009Nov04_141226') pc.process_raw_data() debug = False model = PlaneLeastSquaresModel(debug) data = numpy.asarray(pc.pts3d_bound).T # run RANSAC algorithm ransac_fit, ransac_data = ransac(data,model, 3, 1000, 0.02, 300, # misc. parameters debug=debug,return_all=True) print ransac_fit print ransac_data print 'len inlier',len(ransac_data['inliers']),'shape pts',np.shape(pc.pts3d_bound) pc.pts3d_bound = pc.pts3d_bound[:,ransac_data['inliers']] pc.display_3d('height')
def imu_test(): g = 9.81 from sensor_msgs.msg import Imu import roslib; roslib.load_manifest('ardrone_control') import rospy; global rospy import tf; global tf; # sensor = IMU() msg = Imu() msg.angular_velocity.z = 1.0 msg.linear_acceleration.z = -9.81 msg.linear_acceleration.x = -0.01 msg.orientation.z = 1/sqrt(2) msg.orientation.w = 1/sqrt(2) # msg.linear_acceleration.z = g # sensor.measure(msg) # for sensor in sensor.sensors.values(): # print sensor.properties sensor = IMU() #sensor = IMU_Mahoney( Ts = 0.01) #sensor = IMU_Magdwick( Ts = 0.01) sensor.measure(msg) print sensor.sensors['accelerometer'] print sensor.sensors['gyroscope'] print sensor.sensors['dummy_yaw']
def reload_msg_definition(self): # generate idl module program = ' '.join(sys.argv) base = """// This IDL is generated by '%s' #include "BasicDataType.idl" module RTMROSDataBridge {%s}; """ % (program, self.idltext) open(self.idldir+'/'+self.idlfile, 'w').write(base) genidl_bin = Popen(['rosrun','openrtm_aist','rtm-config','--idlc'], stdout=PIPE, stderr=PIPE).communicate()[0] rtmpkg = Popen(['rosrun','openrtm_aist','rtm-config','--prefix'], stdout=PIPE, stderr=PIPE).communicate()[0] genidl_bin = genidl_bin.rstrip('\n') rtmpkg = rtmpkg.rstrip('\n') call([genidl_bin, '-bpython', '-I%s/include/openrtm-1.1/rtm/idl'%rtmpkg, '-C'+self.idldir, self.idldir+'/'+self.idlfile]) time.sleep(1) # ?? # reload RTC data object global RTMROSDataBridge import RTMROSDataBridge reload(RTMROSDataBridge) reload(RTMROSDataBridge.rosbridge_idl) # register ros/rtm data object type for msg in self.generated: rtm_typ = 'RTMROSDataBridge.' + (msg.replace('/','_')) ros_typ_tuple = tuple(msg.split('/')) ros_typ = '%s.msg.%s' % ros_typ_tuple roslib.load_manifest(ros_typ_tuple[0]) exec('import ' + ros_typ_tuple[0] + '.msg') self.msg2obj[msg] = (eval(ros_typ), eval(rtm_typ)) rospy.loginfo('Data[%s] in RTC = "%s"', msg, self.get_rtmobj(msg)) return True
def update_status(self, msg): """Update the known userdata and active state set and return True if the graph needs to be redrawn.""" # Initialize the return value needs_update = False # Check if the initial states or active states have changed if set(msg.initial_states) != set(self._initial_states): self._structure_changed = True needs_update = True if set(msg.active_states) != set(self._active_states): needs_update = True # Store the initial and active states self._initial_states = msg.initial_states self._last_active_states = self._active_states self._active_states = msg.active_states # Unpack the user data while not rospy.is_shutdown(): try: self._local_data._data = pickle.loads(msg.local_data) break except ImportError as ie: # This will only happen once for each package modulename = ie.args[0][16:] packagename = modulename[0 : modulename.find(".")] roslib.load_manifest(packagename) self._local_data._data = pickle.loads(msg.local_data) # Store the info string self._info = msg.info return needs_update
def create_service_proxy(url, name, service_type, binary=None): try: service_type_module, service_type_name = tuple(service_type.split('/')) roslib.load_manifest(service_type_module) srv_module = import_module(service_type_module + '.srv') return IndividualServiceProxy(url, name, srv_module, service_type_name, binary=binary) except Exception, e: print "Unknown service type %s" % service_type return None
def load_message_class(pkg, msg): """ Given a ROS package and message name, returns the corresponding Python class object. """ roslib.load_manifest(pkg) m = __import__(pkg+'.msg') mod = getattr(m, 'msg') return getattr(mod, msg)
def startup_ros(self, spin=False): '''ROS-specific: Sets up callbacks for - recognized speech from pocketsphinx - world state updates - robot state updates and publishers for - HandsFreeCommand Returns: bool: Whether the setup succeeded. Note that this won't return until ros has shutdown if spin (== True)! ''' # Some settings Debug.printing = False # Setup default system. self.set_world() # Setup ROS. try: import roslib roslib.load_manifest('pr2_pbd_interaction') import rospy from std_msgs.msg import String from pr2_pbd_interaction.msg import ( HandsFreeCommand, WorldObjects, RobotState, Description, HandsFreeGrounding) from pr2_pbd_interaction.srv import WorldChange # TODO(mbforbes); This waits for ROS. This is annoying, but # actually may be OK for now. rospy.init_node('hfpbd_parser', anonymous=True) # We get: speech, world objects, robot state. rospy.Subscriber('recognizer/output', String, self.sphinx_cb) rospy.Subscriber( 'handsfree_robotstate', RobotState, self.robot_state_cb) # We send: parsed commands, grounding results. self.hfcmd_pub = rospy.Publisher( 'handsfree_command', HandsFreeCommand) self.hfgrounding_pub = rospy.Publisher( 'handsfree_grounding', HandsFreeGrounding) # We provide: descriptions for objects. rospy.Service( 'handsfree_worldchange', WorldChange, self.handle_world_change) # Setup complete self.ros_running = True # If no other frontend, just wait here. if spin: rospy.spin() return True except ImportError: # We don't have ROS installed! That's OK. return False
def __getattr__(self, name): try: return getattr(self.wrapped, name) except AttributeError: import roslib.packages try: roslib.load_manifest(name.split('.')[0]) except roslib.packages.InvalidROSPkgException as e: raise ImportError("Cannot import module '%s': \n%s"%(name, str(e))) return __import__(name)
def test_arm_fk(): roslib.load_manifest('tf'); import tf as rostf listener = rostf.TransformListener() rospy.sleep(.5) now = rospy.Time.now() listener.waitForTransform("base_link", "r_gripper_tool_frame", now,rospy.Duration(1)) xyz, quat = listener.lookupTransform("base_link", "r_gripper_tool_frame",now) posemat = brett.larm.get_pose_matrix("base_link", "r_gripper_tool_frame") assert np.allclose(xyz, posemat[:3,3],atol = 1e-4)
def create_topic_proxy(url, name, topic_type, pub=True, sub=True, publish_interval=None, binary=None): try: topic_type_module, topic_type_name = tuple(topic_type.split('/')) roslib.load_manifest(topic_type_module) msg_module = import_module(topic_type_module + '.msg') return IndividualTopicProxy(url, name, msg_module, topic_type_name, pub=pub, sub=sub, publish_interval=publish_interval, binary=binary) except Exception, e: print "Unknown msg type %s" % topic_type return None
def _get_message_or_service_class(type_str, message_type, reload_on_error=False): """ Utility for retrieving message/service class instances. Used by get_message_class and get_service_class. :param type_str: 'msg' or 'srv', ``str`` :param message_type: type name of message/service, ``str`` :returns: Message/Service for message/service type or None, ``class`` :raises: :exc:`ValueError` If message_type is invalidly specified """ if message_type == 'time': return Time if message_type == 'duration': return Duration ## parse package and local type name for import package, base_type = genmsg.package_resource_name(message_type) if not package: if base_type == 'Header': package = 'std_msgs' else: raise ValueError("message type is missing package name: %s"%str(message_type)) pypkg = val = None try: # import the package pypkg = __import__('%s.%s' % (package, type_str)) except ImportError: # try importing from dry package if available try: from roslib import load_manifest from rospkg import ResourceNotFound try: load_manifest(package) try: pypkg = __import__('%s.%s' % (package, type_str)) except ImportError: pass except ResourceNotFound: pass except ImportError: pass if pypkg: try: val = getattr(getattr(pypkg, type_str), base_type) except AttributeError: pass # this logic is mainly to support rosh, so that a user doesn't # have to exit a shell just because a message wasn't built yet if val is None and reload_on_error: try: if pypkg: reload(pypkg) val = getattr(getattr(pypkg, type_str), base_type) except: val = None return val
def init_ros(m): print "ROS init start" print m import roslib; roslib.load_manifest('django_crowd_server') import rospy rospy.init_node("django_crowd_server",disable_signals=True,anonymous=True) from annotation_publisher import PublishAnnotationNode m["sender"]=PublishAnnotationNode("annotation_publisher"); print "ROS init done"
def nothing(): import rospy rospy.init_node('benk',anonymous=True,disable_signals=True) import roslib roslib.load_manifest('tf') import tf.transformations as tft import numpy from brett2.PR2 import PR2 q = numpy.array([0.0037790022003,0.0621387511329,0.624889472514,0.778228364249]) p = numpy.array([0.516622185707,0.0262447148561,0.845655560493])
def __init__(self, btn_index, callback=(lambda : 0)): self.btn_index = btn_index self.callback = callback import roslib roslib.load_manifest('rospy') roslib.load_manifest('joy') from joy.msg import Joy import rospy self.rospy = rospy rospy.init_node('joy_trigger', disable_signals=True) self.sub = rospy.Subscriber('/joy', Joy, self._joy_callback) self.btn_state = -1
def initialize_ros(self): for inp in self.inputs.values(): type_split = inp.type.split('/') if len(type_split) != 2: raise Exception('invalid type') module = type_split[0] roslib.load_manifest(module) module += '.msg' message = type_split[1] mod = __import__(module, globals(), locals(), [message]) print("Subscribing to %s (%s)"%(inp.name, inp.type)) self.subscribers.append(rospy.Subscriber(inp.name, getattr(mod, message), self.handle_ros_message))
def rosload_func(self, pkg_name): """ Import roslib, load the manifest for the given package, and import rospy. """ import roslib roslib.load_manifest(pkg_name) import rospy try: __import__(pkg_name) except ImportError: print "Package does not appear to have a python module"
def run_test(): try: if len(sys.argv) > 1 and sys.argv[1].startswith("--gtest_output="): import roslib; roslib.load_manifest('multi_interface_roam') global rostest import rostest run_ros_tests() else: import unittest unittest.main() exitval.append(0) except SystemExit, v: exitval.append(v.code)
def __init__(self, service_name, service_type): self.name = service_name service_type_module, service_type_name = tuple(service_type.split('/')) roslib.load_manifest(service_type_module) srv_module = import_module(service_type_module + '.srv') self.rostype_name = service_type self.rostype = getattr(srv_module, service_type_name) self.rostype_req = getattr(srv_module, service_type_name + 'Request') self.rostype_resp = getattr(srv_module, service_type_name + 'Response') self.proxy = rospy.ServiceProxy(self.name, self.rostype)
def add_moos2ros_map(self, name, params): # Make sure the msg_type is valid split_msg_type = params['msg_type'].split('/') if len(split_msg_type) != 2: raise ValueError("Invalid msg_type given by " + "%s, should take the form ros_pkg/MsgType" % name) msg_pkg, msg_type = split_msg_type try: roslib.load_manifest(msg_pkg) eval("from %s.msgs import %s as msg_obj" % (msg_pkg, msg_type)) except (ImportError, InvalidROSPkgException) as e: raise ValueError("Cannot find msg_type given by " + "%s, should take the form ros_pkg/MsgType: %s" % (name, str(e)))
def load_pkg_module(package, directory): #check if its in the python path path = sys.path try: imp.find_module(package) except: roslib.load_manifest(package) try: m = __import__( package + '.' + directory ) except: rospy.logerr( "Cannot import package : %s"% package ) rospy.logerr( "sys.path was " + str(path) ) return None return m
#!/usr/bin/env python import roslib roslib.load_manifest('mrobot_teleop') import rospy from geometry_msgs.msg import Twist import sys, select, termios, tty msg = """ Control Your mrobot! --------------------------- Moving around: u i o j k l m , . q/z : increase/decrease max speeds by 10% w/x : increase/decrease only linear speed by 10% e/c : increase/decrease only angular speed by 10% space key, k : force stop anything else : stop smoothly CTRL-C to quit """ moveBindings = { 'i': (1, 0), 'o': (1, -1), 'j': (0, 1), 'l': (0, -1), 'u': (1, 1), ',': (-1, 0),
#!/usr/bin/env python """Test basic functionality of the Penn Pipeline over ROS.""" import roslib roslib.load_manifest('test_nlp') import sys import rospy from nlp.srv import String PIPELINE_SERVICE = "penn_nlp_pipeline_service" def parse_input(): """Request a parse for the given text.""" print "Waiting for service {}...".format(PIPELINE_SERVICE) rospy.wait_for_service(PIPELINE_SERVICE) try: pipeline = rospy.ServiceProxy(PIPELINE_SERVICE, String) while True: user_input = raw_input("> ") response = pipeline(user_input) print response except rospy.ServiceException, err: print >> sys.stderr, "Service call failed: {}".format(err) except (KeyboardInterrupt, EOFError): pass
#!/usr/bin/env python import roslib roslib.load_manifest('tuw_gazebo') import rospy import actionlib import time import tf from visualization_msgs.msg import * from std_msgs.msg import String from geometry_msgs.msg import * from gazebo_msgs.msg import * #from pudb import set_trace; set_trace() robot_name = "r1" covarianceXY = 0.1 covarianceAlpha = 0.05 class SetRobotNode(): def __init__(self): rospy.init_node('goal_test_client') rospy.on_shutdown(self.shutdown) self.pub_command_topic_name = robot_name + '/cmd_vel' self.pub_command = rospy.Publisher(self.pub_command_topic_name, Twist, queue_size=10) self.pub_model_state_topic_name = 'gazebo/set_model_state' self.pub_model_state = rospy.Publisher(self.pub_model_state_topic_name, ModelState,
#!/usr/bin/env python from __future__ import division import roslib roslib.load_manifest('experiments') import rospy import numpy as np import ExperimentLib from geometry_msgs.msg import Point, Twist from experiment_srvs.srv import Trigger, ExperimentParamsChoices, ExperimentParamsChoicesRequest from flycore.msg import MsgFrameState from galvodirector.msg import MsgGalvoCommand from ledpanels.msg import MsgPanelsCommand from patterngen.msg import MsgPattern ####################################################################################################### class Experiment(): def __init__(self): rospy.init_node('Experiment') # Fill out the data structure that defines the experiment. self.experimentparams = ExperimentParamsChoicesRequest() self.experimentparams.experiment.description = 'Navigation with food ysa' self.experimentparams.experiment.maxTrials = -1 self.experimentparams.experiment.timeout = -1 # self.experimentparams.save.filenamebase = 'foodnavi_%s_%s_%s_' % (flyspec, foodspec, gender) self.experimentparams.save.filenamebase = 'foodnavi' self.experimentparams.save.csv = True self.experimentparams.save.bag = True
#!/usr/bin/env python import roslib; roslib.load_manifest('usb2dynamixel_widomx') import rospy from dynamixel_controllers.srv import * from std_msgs.msg import Float64, Float64MultiArray import numpy as np import argparse import time INNER_LOOP_HZ = 15 current_torque = 0 target_torque = 0 steps = 0 timeout = 0 zero_torque = 1 set_torque_cb(data): zero_torque = 0 #torque not zero env start torque controller current_torque = target_torque target_torque = data if __name__ == "__main__": parser = argparse.ArgumentParser() parser.add_argument("joint_name", type = str, \ help = "name of this joint") parser.add_argument("service_name", type = str,\ #joint*_controller/set_torque_limit help = "name of the service control the torque limit") parser.add_argumemt("command_topic_name", type = str, \ #joint*_controller/command help = "name of the topic control the position of this servo")
#!/usr/bin/env python from __future__ import print_function import roslib roslib.load_manifest('robot_guidance') import rospy import cv2 from sensor_msgs.msg import Image from cv_bridge import CvBridge, CvBridgeError from std_msgs.msg import Float32, Int8 import numpy as np class dummy_robot: def __init__(self): rospy.init_node('dummy_robot', anonymous=True) self.bridge = CvBridge() self.image_pub = rospy.Publisher("image_raw", Image, queue_size=1) self.reward_pub = rospy.Publisher("reward", Float32, queue_size=1) self.action_sub = rospy.Subscriber("action", Int8, self.callback_action) self.action = 0 self.pan = 0 self.size = 0 self.reward = 0 self.reward_lr = 0 self.reward_fb = 0 self.cv_image = np.zeros((480, 640, 3), np.uint8) self.cv_image.fill(255) self.image = self.bridge.cv2_to_imgmsg(self.cv_image, encoding="bgr8") self.arrow_cv_image = np.zeros((200, 640, 3), np.uint8)
#! /usr/bin/env python import roslib roslib.load_manifest('cob_sound') import rospy # Brings in the SimpleActionClient import actionlib # Brings in the messages used by the fibonacci action, including the # goal message and the result message. from cob_sound.msg import * def say_client(): # Creates the SimpleActionClient, passing the type of the action # (FibonacciAction) to the constructor. client = actionlib.SimpleActionClient('sound_controller/say', SayAction) # Waits until the action server has started up and started # listening for goals. client.wait_for_server() # Creates a goal to send to the action server. goal = SayGoal() goal.text.data = "Hello, how are you? I am fine." # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action.
'x0var': 0, 'noisy_body_idx': np.array([]), 'noisy_body_var': np.array([]), 'pos_body_idx': np.array([]), 'pos_body_offset': np.array([]), 'smooth_noise': True, 'smooth_noise_var': 2.0, 'smooth_noise_renormalize': True, } try: import rospkg import roslib roslib.load_manifest('rl_msgs') # AgentROS AGENT_ROS = { #TODO: It might be worth putting this in JSON/yaml format so C++ # can read it. 'trial_command_topic': 'gps_controller_trial_command', 'reset_command_topic': 'gps_controller_position_command', 'relax_command_topic': 'gps_controller_relax_command', 'data_request_topic': 'gps_controller_data_request', 'sample_result_topic': 'gps_controller_report',
import roslib; roslib.load_manifest('smach_ros') import rospy import threading import traceback import copy from actionlib.simple_action_client import SimpleActionClient, GoalStatus import smach from smach.state import * __all__ = ['SimpleActionState'] class SimpleActionState(State): """Simple action client state. Use this class to represent an actionlib as a state in a state machine. """ # Meta-states for this action WAITING_FOR_SERVER = 0 INACTIVE = 1 ACTIVE = 2 PREEMPTING = 3 COMPLETED = 4 def __init__(self, # Action info action_name,
#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE #* POSSIBILITY OF SUCH DAMAGE. #* #* Author: Eitan Marder-Eppstein #*********************************************************** PKG = 'image_cb_detector' NAME = 'image_cb_detector_node' import roslib; roslib.load_manifest(PKG) import rospy import cv import math import threading import numpy import tf from sensor_msgs.msg import Image, CameraInfo from image_cb_detector.msg import ObjectInImage from geometry_msgs.msg import PoseStamped from cv_bridge import CvBridge, CvBridgeError class ImageCbDetector:
Publishes: - Actions: @author: Robotnik Automation Software License Agreement (BSD License) Copyright (c) 2015 Robotnik Automation SLL. All Rights Reserved. EDITED BY GHOSTMAN @ 2018 """ import roslib roslib.load_manifest('phantomx_reactor_arm_controller') import rospy import actionlib from control_msgs.msg import * from std_msgs.msg import * from sensor_msgs.msg import JointState class WidowxGripper: """ Class to communicate with the dinamyxel controllers of the arm """ def __init__(self): self.distance = 0.0
#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE #* POSSIBILITY OF SUCH DAMAGE. #*********************************************************** # Author: Blaise Gassend # Reads fingertip pressure data from /pressure and publishes it as a # visualization_marker import roslib roslib.load_manifest('fingertip_pressure') import rospy import threading from fingertip_pressure.msg import PressureInfo, PressureInfoElement from pr2_msgs.msg import PressureState from visualization_msgs.msg import Marker from fingertip_pressure.colormap import color from geometry_msgs.msg import Vector3 positions = [ # x, y, z, xscale, yscale, zscale (0.026, 0.007, 0.000, 0.010, 0.010, 0.015), (0.010, 0.010, -0.008, 0.030, 0.010, 0.010), (0.025, 0.010, -0.008, 0.010, 0.010, 0.010), (0.030, 0.010, -0.004, 0.010, 0.010, 0.010), (0.030, 0.010, 0.004, 0.010, 0.010, 0.010),
#!/usr/bin/env python # -*- coding: utf-8 -*- import roslib roslib.load_manifest('hadsafha') import rospy import sys from hadsafha.terrorist_detector import TerroristDetector from hadsafha.air_traffic_manager import AirTrafficManager from hadsafha.zephyr_controller import ZephyrController from hadsafha.iris_controller import IrisController from hadsafha.task_planner import TaskPlanner from hadsafha.comm_manager import CommManager from hadsafha.sensor_manager import SensorManager from hadsafha.score_manager import ScoreManager from hadsafha.battery_manager import BatteryManager from hadsafha.autonomous_intelligence import AutonomousIntelligence import hadsafha.scenario as scenario from datetime import datetime if __name__ == "__main__": rospy.init_node('uav_controller') # Alınan arguman "_" karakteri ile parçalanır ve bu sayede uav tipi ve index'i seçilmiş olur #args= sys.argv[1].split("_") #uav_index = int(args[0]) #uav_type = args[1] uav_index = int(sys.argv[1])
#!/usr/bin/env python PKG = 'depth_sensor' import roslib roslib.load_manifest(PKG) import serial import math import rospy from kraken_msgs.msg import dvlData #import numpy depth = serial.Serial() depth.name = '/dev/ttyUSB0' depth.baudrate = 9600 depth.stopbits = 1 pub = rospy.Publisher('/kraken/depthData', depthData, queue_size=10) rospy.init_node('depthdata', anonymous=True) def readResponse(): response = '' garbage = '' while (garbage != '\r'): garbage = depth.read(1) response += garbage return response
#!/usr/bin/env python from __future__ import division import roslib roslib.load_manifest('actuator_source') import rospy import threading import actionlib import actions.msg import msg_and_srv.msg class ActuatorActionServer(object): def __init__(self): self.lock = threading.Lock() self.initialized = False self.abort = False self.setpt_update_rate = rospy.get_param("actuator_update_rate", 50) self.max_value = rospy.get_param("actuator_max_value", 2000) self.min_value = rospy.get_param("actuator_min_value", 1000) self.default_value = rospy.get_param("actuator_default_value", 1500) self.rate = rospy.Rate(self.setpt_update_rate) self.dt = 1 / self.setpt_update_rate self.value_final = None # Setup action server self.feedback = actions.msg.ActuatorOutscanFeedback() self.result = actions.msg.ActuatorOutscanResult()
# # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. import roslib roslib.load_manifest('turtle_tf') import rospy import math import tf import geometry_msgs.msg import turtlesim.srv if __name__ == '__main__': rospy.init_node('tf_turtle') listener = tf.TransformListener() rospy.wait_for_service('spawn') spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn) spawner(4, 2, 0, 'turtle2')
# -*- coding: utf-8 -*- ''' ROS node in charge of interfacing with the encoders phidget. Publishes EncoderCounts messages on the "encoder_counts" topic. Parameters: - period: publish only after this period has elapsed (default value: 0.02). This limits the publishing rate. It seems the phidget publishes a count change every 8ms. - min_pub_period: if set, we want to publish messages with this period, even if encoders are not changing. Otherwise, publish only when the encoders value is changing. ''' import roslib; roslib.load_manifest('phidget_encoders') import rospy import threading from Phidgets.PhidgetException import * from Phidgets.Events.Events import * from Phidgets.Devices.Encoder import Encoder from phidget_encoders.msg import EncoderCounts import diagnostic_updater as DIAG def err(e): '''A helper function to report Phidget errors''' rospy.logerr("Phidget error %i: %s. Exiting..." % (e.code, e.details)) exit(1)
#!/usr/bin/env python import roslib roslib.load_manifest('collvoid_controller') import rospy import string import actionlib import math import random #from collvoid_local_planner.srv import InitGuess from std_msgs.msg import String from geometry_msgs.msg import PoseWithCovarianceStamped, PoseStamped from nav_msgs.msg import Odometry from socket import gethostname from collvoid_msgs.msg import PoseTwistWithCovariance from move_base_msgs.msg import * import tf THRESHOLD = 0.22 def dist(a, b): return math.sqrt( math.pow(a.position.x - b.position.x, 2) + math.pow(a.position.y - b.position.y, 2)) class ControllerRobots(): def __init__(self): self.initialize()
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. # # # Author: Advait Jain ([email protected]), Healthcare Robotics Lab, Georgia Tech import roslib roslib.load_manifest('move_arm_tutorials') import rospy import actionlib import geometric_shapes_msgs from move_arm_msgs.msg import MoveArmAction from move_arm_msgs.msg import MoveArmGoal from motion_planning_msgs.msg import SimplePoseConstraint from motion_planning_msgs.msg import PositionConstraint from motion_planning_msgs.msg import OrientationConstraint from actionlib_msgs.msg import GoalStatus
#!/usr/bin/env python import roslib roslib.load_manifest('velma_task_cs_ros_interface') import rospy import math import PyKDL from threading import Thread from velma_common import * from rcprg_planner import * from rcprg_ros_utils import MarkerPublisher, exitError from moveit_msgs.msg import AttachedCollisionObject, CollisionObject from shape_msgs.msg import SolidPrimitive from geometry_msgs.msg import Pose from visualization_msgs.msg import Marker import tf_conversions.posemath as pm class MarkerPublisherThread: def threaded_function(self, obj): pub = MarkerPublisher("attached_objects") while not self.stop_thread: pub.publishSinglePointMarker(PyKDL.Vector(), 1, r=1, g=0, b=0, a=1,
#!/usr/bin/env python from __future__ import print_function import roslib; roslib.load_manifest('teleop_twist_keyboard') import rospy import numpy as np from std_msgs.msg import Int32MultiArray from geometry_msgs.msg import Vector3 from geometry_msgs.msg import Twist import sys, select, termios, tty msg = """ -- ROBOT TELEOP KEY -- ---------------------------------- ===== robot2 general command ===== q w e a s d x w = increase linear speed x by 0.25 x = decrease linear speed x by 0.25 a = increase linear speed y by 0.25 d = decrease linear speed y by 0.25 q = increase angular speed z by 0.25 e = decrease angular speed z by 0.25 CTRL-C to quit """ #skill, position, info, goalkeeper command movement = {
#!/usr/bin/env python import roslib roslib.load_manifest('RelocSensorDriver') import tf import rospy import math import time from geometry_msgs.msg import PoseWithCovarianceStamped from tf.transformations import euler_from_quaternion pioneer1 = [-0.433, -0.224, -0.000, -0.992151563931, 0.125041090001] pioneer2 = [2.367, -1.658, -0.000, -0.992151563931, 0.125041090001] drone1 = [4.73816204071, -0.1, 0.0, -0.992151563931, 0.125041090001] #todo launch this along wih odometers and update odom frame to map def talker(): rospy.init_node('RelocNode') br = tf.TransformBroadcaster() rospy.Subscriber('/pioneer1/initialpose', PoseWithCovarianceStamped, callback1, queue_size=1) rospy.Subscriber('/pioneer2/initialpose', PoseWithCovarianceStamped, callback2, queue_size=1) rospy.Subscriber('/drone1/initialpose', PoseWithCovarianceStamped,
#!/usr/bin/env python3 import roslib roslib.load_manifest('learning_tf') import rospy import tf import turtlesim.msg def handle_turtle_pose(msg, turtlename): br = tf.TransformBroadcaster() br.sendTransform((msg.x, msg.y, 0), tf.transformations.quaternion_from_euler(0, 0, msg.theta), rospy.Time.now(), turtlename, "world") if __name__ == '__main__': rospy.init_node('turtle_tf_broadcaster') turtlename = rospy.get_param('~turtle') rospy.Subscriber('/%s/pose' % turtlename, turtlesim.msg.Pose, handle_turtle_pose, turtlename) rospy.spin()
#! /usr/bin/env python import roslib roslib.load_manifest('cotesys_ros_grasping') import rospy import actionlib from cotesys_ros_grasping.msg import AttachBoundingBoxAction, AttachBoundingBoxGoal if __name__ == '__main__': rospy.init_node('test_attach_bounding_box') attach_object_client = actionlib.SimpleActionClient( 'attach_bounding_box', AttachBoundingBoxAction) attach_object_client.wait_for_server() goal = AttachBoundingBoxGoal() goal.arm_name = "right_arm" goal.x_size = .03 goal.y_size = .03 goal.z_size = .2 attach_object_client.send_goal(goal) print 'sent goal' attach_object_client.wait_for_result() rospy.sleep(3) goal.remove = True
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Revision $Id$ import roslib roslib.load_manifest('test_roslaunch') import os, sys, unittest import time import rostest import roslaunch.pmon import roslaunch.server import roslib.xmlrpc from xmlrpclib import ServerProxy from roslaunch.server import ProcessListener class MockProcessListener(ProcessListener):
#!/usr/bin/env python import roslib roslib.load_manifest('ros_basics') import rospy import math import tf import geometry_msgs.msg import turtlesim.srv if __name__ == '__main__': rospy.init_node('turtle_tf_listener') listener = tf.TransformListener() rospy.wait_for_service('spawn') spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn) spawner(4, 2, 0, 'turtle2') turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist, queue_size=1) rate = rospy.Rate(10.0) while not rospy.is_shutdown(): try: (trans, rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue
#!/usr/bin/env python """ match_template_simple.py - Version 1.0 2012-02-27 """ import roslib roslib.load_manifest('rbx1_vision') import rospy from ros2opencv2 import ROS2OpenCV2 from sensor_msgs.msg import Image, RegionOfInterest import sys import cv2.cv as cv import cv2 import numpy as np class MatchTemplate(ROS2OpenCV2): def __init__(self, node_name): ROS2OpenCV2.__init__(self, node_name) self.node_name = node_name self.use_depth_for_detection = rospy.get_param( "~use_depth_for_detection", False) self.fov_width = rospy.get_param("~fov_width", 1.094) self.fov_height = rospy.get_param("~fov_height", 1.094) self.max_object_size = rospy.get_param("~max_object_size", 0.28) # Intialize the detection box self.detect_box = None
#! /usr/bin/python import roslib roslib.load_manifest('simple_arm_controller') import rospy import actionlib import time from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal, Pr2GripperCommandGoal, Pr2GripperCommandAction from trajectory_msgs.msg import JointTrajectoryPoint #to control the left arm: whicharm = 'l' rospy.init_node('controller_manager') joint_trajectory_action_name = whicharm + '_arm_controller/joint_trajectory_action' joint_action_client = actionlib.SimpleActionClient( joint_trajectory_action_name, JointTrajectoryAction) print "waiting for server..." joint_action_client.wait_for_server() print "got server" joint_names = [ "_shoulder_pan_joint", "_shoulder_lift_joint", "_upper_arm_roll_joint", "_elbow_flex_joint", "_forearm_roll_joint", "_wrist_flex_joint", "_wrist_roll_joint" ] joint_names1 = [whicharm + x for x in joint_names] goal = JointTrajectoryGoal() goal.trajectory.header.stamp = rospy.get_rostime() goal.trajectory.joint_names = joint_names1
import numpy as np import copy import roslib roslib.load_manifest('hrl_generic_arms') import rospy from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion from geometry_msgs.msg import Transform, TransformStamped, Vector3 import tf.transformations as tf_trans class PoseConverter: @staticmethod def _make_generic(args): if type(args[0]) == str: frame_id = args[0] header, homo_mat, rot_quat, rot_euler = PoseConverter._make_generic( args[1:]) if header is None: header = [0, rospy.Time.now(), ''] header[2] = frame_id return header, homo_mat, rot_quat, rot_euler if len(args) == 1: if type(args[0]) is Pose: homo_mat, rot_quat, rot_euler = PoseConverter._extract_pose_msg( args[0]) return None, homo_mat, rot_quat, rot_euler elif type(args[0]) is PoseStamped: homo_mat, rot_quat, rot_euler = PoseConverter._extract_pose_msg(
#!/usr/bin/python import roslib roslib.load_manifest('people_velocity_tracker') import rospy import tf import math from geometry_msgs.msg import Point, Vector3 from geometry_msgs.msg import Twist, TwistWithCovariance from people_msgs.msg import PositionMeasurementArray, Person, People from easy_markers.generator import MarkerGenerator, Marker from kalman_filter import Kalman def distance(leg1, leg2): return math.sqrt( math.pow(leg1.x - leg2.x, 2) + math.pow(leg1.y - leg2.y, 2) + math.pow(leg1.z - leg2.z, 2)) def average(leg1, leg2): return Point((leg1.x + leg2.x) / 2, (leg1.y + leg2.y) / 2, (leg1.z + leg2.z) / 2) def add(v1, v2): return Vector3(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z) def subtract(v1, v2): return Vector3(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z)