def listener(self): # setup call back for lgging print "Starting listener..." rospy.init_node('log_listener', anonymous=True) print "Looking for ros params..." print rospy.search_param('logging_location') if rospy.has_param("~logging_location"): print "logging to the following location..." self.logLocation = rospy.get_param('~logging_location') print self.logLocation else: print "Using default logging location" if rospy.has_param("~onboard"): print "logging instance is onboard?" self.onboard = rospy.get_param("~onboard") print self.onboard print "Subscribe to logging messages ..." rospy.Subscriber('/vigir_logging', OCSLogging, self.callback) rospy.Subscriber('/vigir_logging_query', String, self.state) print "Publish topic ..." self.pub = rospy.Publisher('/vigir_logging_responce', String, queue_size=1) self.query = "desktop_" print "Now spin ..." rospy.spin() print "Shutdown the desktop logger!"
def listener(self): # setup call back for lgging print "Starting listener..." rospy.init_node('log_listener', anonymous=True) print "Looking for ros params..." print rospy.search_param('logging_location') print rospy.search_param('to_log') if rospy.has_param('~logger_name'): print("This logger is named") self.logger_name = rospy.get_param('~logger_name') print self.logger_name if rospy.has_param('~to_log'): print "logging the following topics..." self.toLog = rospy.get_param('~to_log') print self.toLog else: print "Failed to find topics to log." if rospy.has_param('~enable_Log_grabbing'): self.enableBDILogging = rospy.get_param('~enable_Log_grabbing') else: print "Not setting up log grabbing" if rospy.has_param("~logging_location"): print "logging to the following location..." self.logLocation = rospy.get_param('~logging_location') print self.logLocation else: print "Using default logging location" rospy.Subscriber('/vigir_logging', OCSLogging, self.callback) rospy.Subscriber('/vigir_logging_query', String, self.state) self.pub = rospy.Publisher('/vigir_logging_responce', String, queue_size=1) rospy.spin()
def gazebo_world_state_handler(self, data): # Retrieve the static world & robot descriptions once on first run if self.gazebo_world == None: world_param = rospy.search_param('gazebo_world_description') world_description = rospy.get_param(world_param, '') robot_param = rospy.search_param('robot_description') robot_description = str(rospy.get_param(robot_param, '')) self.gazebo_world = GazeboDescriptionParser(world_description, robot_description) # Begin parsing icarus world state icarus_state = '(list' regex = re.compile('(env|obj)_(.*?)_(.*?)_(.*?)_body') for i in range(len(data.name)): pos = data.pose[i].position ori = data.pose[i].orientation lin = data.twist[i].linear ang = data.twist[i].angular frc = data.wrench[i].force trq = data.wrench[i].torque # Only publish recognizable objects from the world & robot desscriptions if self.gazebo_world != None and self.gazebo_world.has_object(data.name[i].strip()): match = re.search(regex, data.name[i]) if match: if match.group(1) == 'env': icarus_state += ' \'(environment %s' % (data.name[i]) else: icarus_state += ' \'(object %s' % (data.name[i]) icarus_state += ' type %s' % (match.group(2)) icarus_state += ' color %s' % (match.group(4)) else: icarus_state += ' \'(object %s' % (data.name[i]) if data.name[i].strip() == 'base_link': icarus_state += ' type self' else: icarus_state += ' type empty' icarus_state += ' color empty' icarus_state += ' role empty' icarus_state += ' status empty' icarus_state += ' xpos %s ypos %s zpos %s' % (pos.x, pos.y, pos.z) icarus_state += ' xori %s yori %s zori %s wori %s' % (ori.x, ori.y, ori.z, ori.w) icarus_state += ' xlin %s ylin %s zlin %s' % (lin.x, lin.y, lin.z) icarus_state += ' xang %s yang %s zang %s' % (ang.x, ang.y, ang.z) icarus_state += ' xtrq %s ytrq %s ztrq %s' % (trq.x, trq.y, trq.z) obj = self.gazebo_world.get_object(data.name[i].strip()) icarus_state += ' xsiz %s ysiz %s zsiz %s srad %s)' % (obj.size.x, obj.size.y, obj.size.z, obj.s_radius) icarus_state += ')' self.icarus_world_state = icarus_state self.icarus_world_state_pub.publish(icarus_state)
def __init__(self): '''Initialize ros publisher, ros subscriber''' # topic where we publish self.bridge = CvBridge() out_image = rospy.get_param(rospy.search_param('out')) image = rospy.get_param(rospy.search_param('image')) # subscribed Topic self.image_pub = rospy.Publisher(out_image, Image) self.subscriber = rospy.Subscriber(image, CompressedImage, self.callback, queue_size = 1000)
def main(): rospy.init_node("robospect_laser_assembler_node") _name = rospy.get_name().replace('/','') arg_defaults = { 'topic_state': 'state', 'desired_freq': DEFAULT_FREQ, 'assembly_scans_service_name': '/assemble_scans2', 'point_cloud_publisher_topic_name': '/assembled_cloud', 'time_point_cloud_back': DEFAULT_TIME_PCLOUD_BACK, } args = {} for name in arg_defaults: try: if rospy.search_param(name): args[name] = rospy.get_param('~%s'%(name)) # Adding the name of the node, because the para has the namespace of the node else: args[name] = arg_defaults[name] #print name except rospy.ROSException, e: rospy.logerr('%s: %s'%(e, _name))
def load_config_from_param(self): # first, make sure parameter server is even loaded while not rospy.search_param("/joints"): rospy.loginfo("waiting for parameter server to load with joint definitions") rospy.sleep(1) rospy.sleep(1) joints = rospy.get_param('/joints') for name in joints: rospy.loginfo( "found: " + name ) s = Servo() key = '/joints/' + name + '/' s.bus = int(rospy.get_param(key + 'bus')) s.servo = int(rospy.get_param(key + 'servo')) s.flip = int(rospy.get_param(key + 'flip')) s.servopin = int(rospy.get_param(key + 'servopin')) s.sensorpin = int(rospy.get_param(key + 'sensorpin')) s.minpulse = int(rospy.get_param(key + 'minpulse')) s.maxpulse = int(rospy.get_param(key + 'maxpulse')) s.minangle = float(rospy.get_param(key + 'minangle')) s.maxangle = float(rospy.get_param(key + 'maxangle')) s.minsensor = int(rospy.get_param(key + 'minsensor')) s.maxsensor = int(rospy.get_param(key + 'maxsensor')) s.maxspeed = float(rospy.get_param(key + 'maxspeed')) s.smoothing = int(rospy.get_param(key + 'smoothing')) self.servos[name] = s
def initialize(self): self.status_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/status", GoalStatusArray, latch=True); self.result_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/result", self.ActionResult); self.feedback_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/feedback", self.ActionFeedback); self.goal_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/goal", self.ActionGoal,self.internal_goal_callback); self.cancel_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/cancel", GoalID,self.internal_cancel_callback); #read the frequency with which to publish status from the parameter server #if not specified locally explicitly, use search param to find actionlib_status_frequency resolved_status_frequency_name = rospy.remap_name(self.ns)+"/status_frequency" if rospy.has_param(resolved_status_frequency_name): self.status_frequency = rospy.get_param(resolved_status_frequency_name, 5.0); rospy.logwarn("You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency.") else: search_status_frequency_name = rospy.search_param("actionlib_status_frequency") if search_status_frequency_name is None: self.status_frequency = 5.0 else: self.status_frequency = rospy.get_param(search_status_frequency_name, 5.0) status_list_timeout = rospy.get_param(rospy.remap_name(self.ns)+"/status_list_timeout", 5.0); self.status_list_timeout = rospy.Duration(status_list_timeout); self.status_timer = threading.Thread( None, ros_timer, None, (self.publish_status_async,self.status_frequency) ); self.status_timer.start();
def load_config_from_param(): # first, make sure parameter server is even loaded while not rospy.search_param("/joints"): rospy.loginfo("waiting for parameter server to load with joint definitions") rospy.sleep(1) rospy.sleep(1) joints = rospy.get_param('/joints') for name in joints: rospy.loginfo( "found: " + name ) s = Servo() key = '/joints/' + name + '/' s.bus = rospy.get_param(key + 'bus') s.servoPin = rospy.get_param(key + 'servoPin') s.minPulse = rospy.get_param(key + 'minPulse') s.maxPulse = rospy.get_param(key + 'maxPulse') s.minGoal = rospy.get_param(key + 'minGoal') s.maxGoal = rospy.get_param(key + 'maxGoal') s.rest = rospy.get_param(key + 'rest') s.maxSpeed = rospy.get_param(key + 'maxSpeed') s.smoothing = rospy.get_param(key + 'smoothing') s.sensorpin = rospy.get_param(key + 'sensorPin') s.minSensor = rospy.get_param(key + 'minSensor') s.maxSensor = rospy.get_param(key + 'maxSensor') servos[name] = s return servos
def __init__(self): NaoNode.__init__(self, 'nao_sensors') self.connectNaoQi() # default sensor rate: 25 Hz (50 is max, stresses Nao's CPU) self.sensorRate = rospy.Rate(rospy.get_param('~sensor_rate', 25.0)) self.dataNamesList = ["DCM/Time", "Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value","Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value", "Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value", "Device/SubDeviceList/InertialSensor/GyroscopeX/Sensor/Value", "Device/SubDeviceList/InertialSensor/GyroscopeY/Sensor/Value", "Device/SubDeviceList/InertialSensor/GyroscopeZ/Sensor/Value", "Device/SubDeviceList/InertialSensor/AccelerometerX/Sensor/Value", "Device/SubDeviceList/InertialSensor/AccelerometerY/Sensor/Value", "Device/SubDeviceList/InertialSensor/AccelerometerZ/Sensor/Value"] tf_prefix_param_name = rospy.search_param('tf_prefix') if tf_prefix_param_name: self.tf_prefix = rospy.get_param(tf_prefix_param_name) else: self.tf_prefix = "" # To stop odometry tf being broadcast self.broadcast_odometry = rospy.get_param('~broadcast_odometry', True) self.base_frameID = rospy.get_param('~base_frame_id', "base_link") if not(self.base_frameID[0] == '/'): self.base_frameID = self.tf_prefix + '/' + self.base_frameID # use sensor values or commanded (open-loop) values for joint angles self.useJointSensors = rospy.get_param('~use_joint_sensors', True) # (set to False in simulation!) # init. messages: self.torsoOdom = Odometry() self.torsoOdom.header.frame_id = rospy.get_param('~odom_frame_id', "odom") if not(self.torsoOdom.header.frame_id[0] == '/'): self.torsoOdom.header.frame_id = self.tf_prefix + '/' + self.torsoOdom.header.frame_id self.torsoIMU = Imu() self.torsoIMU.header.frame_id = self.base_frameID self.jointState = JointState() self.jointState.name = self.motionProxy.getJointNames('Body') # simluated model misses some joints, we need to fill: if (len(self.jointState.name) == 22): self.jointState.name.insert(6,"LWristYaw") self.jointState.name.insert(7,"LHand") self.jointState.name.append("RWristYaw") self.jointState.name.append("RHand") msg = "Nao joints found: "+ str(self.jointState.name) rospy.logdebug(msg) self.torsoOdomPub = rospy.Publisher("odom", Odometry, queue_size=10) self.torsoIMUPub = rospy.Publisher("imu", Imu, queue_size=10) self.jointStatePub = rospy.Publisher("joint_states", JointState, queue_size=10) self.tf_br = tf.TransformBroadcaster() rospy.loginfo("nao_sensors initialized")
def __get_parameter(pname): param_full_name = rospy.search_param(pname) if not param_full_name: raise ParamNotFoundError("'{}'".format(pname)) p = Param(param_full_name, rospy.get_param(param_full_name)) rospy.loginfo("Param '{}'. Value: '{}'".format(p.name, p.value)) return p
def main(): rospy.init_node("test_contact_marker") _name = rospy.get_name().replace('/','') arg_defaults = { 'topic_state': 'state', 'desired_freq': DEFAULT_FREQ, 'marker_frames': ['sw1', 'sw2', 'sw3', 'sw4'], 'marker_names': ['sw1', 'sw2', 'sw3', 'sw4'], } args = {} for name in arg_defaults: try: if rospy.search_param(name): args[name] = rospy.get_param('~%s'%(name)) # Adding the name of the node, because the para has the namespace of the node else: args[name] = arg_defaults[name] #print name except rospy.ROSException, e: rospy.logerr('%s: %s'%(e, _name))
def getFixedTypeProperty(self, propertyName, valueType): """Main property getter receiving the property name and the value type and returning the current value from the low level driver.""" propertyData = self.translator.interpret(propertyName) if propertyData == None: return None #else: if propertyData[PPTY_KIND] == "dynParam": paramName = propertyTranslator.getBasename(propertyData[PPTY_REF]) resp1 = self.driverMgr.getParameter(paramName, self.translator.getDynServerPath(propertyName)) elif propertyData[PPTY_KIND] == "publishedTopic": resp1 = self.driverMgr.getTopic(propertyData[PPTY_REF], valueType) elif propertyData[PPTY_KIND] == "readOnlyParam": paramName = propertyTranslator.getBasename(propertyData[PPTY_REF]) location = rospy.search_param(param_name) resp1 = rospy.get_param(location) elif propertyData[PPTY_KIND] == "topic": ##MICHI: 14Feb2012 if valueType == str: resp1 = str(propertyData[PPTY_REF]) else: ## Special case for reading disparity images valueType2 = DisparityImage if propertyData[PPTY_TYPE].find("Disparity") < 0: valueType2 = Image resp1 = self.driverMgr.getTopic(propertyData[PPTY_REF], valueType2) else: rospy.logerr("Error: unable to get property %s of kind %s"%(propertyName, propertyData[PPTY_TYPE])) resp1 = None return resp1
def main(): rospy.init_node("axis_twist") arg_defaults = { 'hostname': '192.168.0.90', 'username': '', 'password': '', 'flip': False, # things get weird if flip=true 'speed_control': False } args = {} # go through all arguments for name in arg_defaults: full_param_name = rospy.search_param(name) # make sure argument was found (https://github.com/ros/ros_comm/issues/253) if full_param_name == None: args[name] = arg_defaults[name] else: args[name] = rospy.get_param(full_param_name, arg_defaults[name]) # create new PTZ object and start dynamic_reconfigure server my_ptz = AxisPTZ(**args) srv = Server(PTZConfig, my_ptz.dynamicReconfigCallback) rospy.spin()
def search_param(name, params_glob): if params_glob and not any(fnmatch.fnmatch(str(v), glob) for glob in params_glob): # If the glob list is not empty and there are no glob matches, # stop the attempt to find the parameter. return None # If the glob list is empty (i.e. false) or the parameter matches # one of the glob strings, continue to find the parameter. return rospy.search_param(name)
def start_exploration(self): '''Starts FlexBe behavior according to param "mission_behavior".''' msg = BehaviorExecutionActionGoal() #msg.goal.behavior_name = 'Search Victims' mission_full_param_name = rospy.search_param('mission_behavior') msg.goal.behavior_name = rospy.get_param(mission_full_param_name) self._behavior_publisher.publish(msg) rospy.loginfo("FlexBe exploration behavior "+ rospy.get_param(mission_full_param_name) +" activated.")
def addTFPrefix(frame_id): prefix = "" prefix_param = rospy.search_param("tf_prefix") if prefix_param: prefix = rospy.get_param(prefix_param) if prefix[0] != "/": prefix = "/%s" % prefix return "%s/%s" % (prefix, frame_id)
def param_talker(): rospy.init_node('param_talker') # Fetch values from the Parameter Server. In this example, we fetch # parameters from three different namespaces: # # 1) global (/global_example) # 2) parent (/foo/utterance) # 3) private (/foo/param_talker/topic_name) # fetch a /global parameter global_example = rospy.get_param("/global_example") rospy.loginfo("%s is %s", rospy.resolve_name('/global_example'), global_example) # fetch the utterance parameter from our parent namespace utterance = rospy.get_param('utterance') rospy.loginfo("%s is %s", rospy.resolve_name('utterance'), utterance) # fetch topic_name from the ~private namespace topic_name = rospy.get_param('~topic_name') rospy.loginfo("%s is %s", rospy.resolve_name('~topic_name'), topic_name) # fetch a parameter, using 'default_value' if it doesn't exist default_param = rospy.get_param('default_param', 'default_value') rospy.loginfo('%s is %s', rospy.resolve_name('default_param'), default_param) # fetch a group (dictionary) of parameters gains = rospy.get_param('gains') p, i, d = gains['P'], gains['I'], gains['D'] rospy.loginfo("gains are %s, %s, %s", p, i, d) # set some parameters rospy.loginfo('setting parameters...') rospy.set_param('list_of_floats', [1., 2., 3., 4.]) rospy.set_param('bool_True', True) rospy.set_param('~private_bar', 1+2) rospy.set_param('to_delete', 'baz') rospy.loginfo('...parameters have been set') # delete a parameter if rospy.has_param('to_delete'): rospy.delete_param('to_delete') rospy.loginfo("deleted %s parameter"%rospy.resolve_name('to_delete')) else: rospy.loginfo('parameter %s was already deleted'%rospy.resolve_name('to_delete')) # search for a parameter param_name = rospy.search_param('global_example') rospy.loginfo('found global_example parameter under key: %s'%param_name) # publish the value of utterance repeatedly pub = rospy.Publisher(topic_name, String) while not rospy.is_shutdown(): pub.publish(utterance) rospy.loginfo(utterance) rospy.sleep(1)
def get_frame_id(): frame_id = rospy.get_param('~frame_id', 'gps') """Add the TF prefix""" prefix = "" prefix_param = rospy.search_param('tf_prefix') if prefix_param: prefix = rospy.get_param(prefix_param) return "%s/%s" % (prefix, frame_id) else: return frame_id
def main(): rospy.init_node("axis") arg_defaults = {"hostname": "192.168.0.90", "username": "", "password": ""} args = {} for name in arg_defaults: args[name] = rospy.get_param(rospy.search_param(name), arg_defaults[name]) AxisPTZ(**args) rospy.spin()
def __init__(self): param_name = rospy.search_param('restamped_topics') print "Param name" print param_name self.topic_names = rospy.get_param(param_name) self.subscribers = {} self.publishers = {} self.setup_topics()
def main(): rospy.init_node("axis_driver") arg_defaults = { 'hostname': '192.168.0.90', # default IP address 'username': '******', # default login name 'password': '', 'fps': 0, 'width': 640, 'height': 480, 'delay': 0.0, 'compression': 10, 'frame_id': 'axis_camera', 'camera_info_url': ''} # Look up parameters starting in the driver's private parameter # space, but also searching outer namespaces. Defining them in a # higher namespace allows the axis_ptz.py script to share parameters # with the driver. args = {} for name, val in arg_defaults.iteritems(): full_name = rospy.search_param(name) if full_name is None: args[name] = val else: args[name] = rospy.get_param(full_name, val) # resolve frame_id with tf_prefix (unless already absolute) if args['frame_id'][0] != '/': # not absolute? tf_prefix = rospy.search_param('tf_prefix') prefix_val = '' if tf_prefix is not None: # prefix defined? prefix_val = rospy.get_param(tf_prefix) if prefix_val[0] != '/': # prefix not absolute? prefix_val = '/' + prefix_val args['frame_id'] = prefix_val + '/' + args['frame_id'] rospy.sleep(args['delay']) rospy.loginfo("Starting AXIS video acquisition") Axis(**args) rospy.spin()
def read_args_with_defaults(arg_defaults): """Look up parameters starting in the driver's private parameter space, but also searching outer namespaces. Defining them in a higher namespace allows the axis_ptz.py script to share parameters with the driver.""" args = {} for name, val in arg_defaults.iteritems(): full_name = rospy.search_param(name) if full_name is None: args[name] = val else: args[name] = rospy.get_param(full_name, val) # resolve frame_id with tf_prefix (unless already absolute) if args['frame_id'][0] != '/': # not absolute? tf_prefix = rospy.search_param('tf_prefix') prefix_val = '' if tf_prefix is not None: # prefix defined? prefix_val = rospy.get_param(tf_prefix) if prefix_val[0] != '/': # prefix not absolute? prefix_val = '/' + prefix_val args['frame_id'] = prefix_val + '/' + args['frame_id'] return args
def updateArgs(arg_defaults): '''Look up parameters starting in the driver's private parameter space, but also searching outer namespaces. ''' args = {} for name, val in arg_defaults.iteritems(): full_name = rospy.search_param(name) if full_name is None: args[name] = val else: args[name] = rospy.get_param(full_name, val) return(args)
def test_search_param(self): try: orig_caller_id = rospy.names.get_caller_id() rospy.names._set_caller_id('/global_node') self.assertEquals(None, rospy.search_param('search_param')) rospy.set_param('/search_param', 1) self.assertEquals('/search_param', rospy.search_param('search_param')) rospy.names._set_caller_id('/level1/level2/relative_node') self.assertEquals('/search_param', rospy.search_param('search_param')) rospy.set_param('/level1/search_param', 2) self.assertEquals('/level1/search_param', rospy.search_param('search_param')) rospy.set_param('~search_param', 3) # make sure that search starts in our private namespace first self.assertEquals('/level1/level2/relative_node/search_param', rospy.search_param('search_param')) finally: rospy.names._set_caller_id(orig_caller_id)
def __init__(self): rospy.init_node('ichthus_controller_python_core') wxPython = ichthus_2.MyApp() wxPython.MainLoop() self.MAX_COMMAND_COUNT = 0 self.num_command = 0 self.regex_list = [] self.command_regex = "" if rospy.search_param("MAX_COMMAND_COUNT") is None: rospy.logerr("check yaml file") exit() self.regex_init()
def updateArgs(arg_defaults): # Look up parameters starting in the node's private parameter space, but also search outer namespaces. args = {} for name, val in arg_defaults.items(): full_name = rospy.search_param(name) rospy.loginfo("name %s, %s" % (name, full_name)) if full_name is None: args[name] = val else: args[name] = rospy.get_param(full_name, val) rospy.loginfo("We have args %s value %s" % (val, args[name])) return (args)
def updateArgs(arg_defaults): '''Look up parameters starting in the driver's private parameter space, but also searching outer namespaces. Defining them in a higher namespace allows the axis_ptz.py script to share parameters with the driver.''' args = {} for name, val in arg_defaults.iteritems(): full_name = rospy.search_param(name) if full_name is None: args[name] = val else: args[name] = rospy.get_param(full_name, val) # resolve frame_id with tf_prefix (unless already absolute) if args['frame_id'][0] != '/': # not absolute? tf_prefix = rospy.search_param('tf_prefix') prefix_val = '' if tf_prefix is not None: # prefix defined? prefix_val = rospy.get_param(tf_prefix) if prefix_val[0] != '/': # prefix not absolute? prefix_val = '/' + prefix_val args['frame_id'] = prefix_val + '/' + args['frame_id'] return (args)
def __init__(self): """Initialise node.""" rospy.init_node("range_merger") self._merged_topic = rospy.Publisher("range_merger/scan", LaserScan, queue_size=10) robot_root = rospy.get_param("~robot_root", rospy.get_namespace()) tf_prefix_key = rospy.search_param("tf_prefix") if tf_prefix_key: tf_prefix = rospy.get_param(tf_prefix_key, None) else: tf_prefix = None if tf_prefix is not None and not tf_prefix.endswith("/"): tf_prefix += "/" self._tf_reference_frame = str( rospy.get_param("~reference_frame", REFERENCE_FRAME_ID)) if tf_prefix: self._tf_reference_frame = tf_prefix + self._tf_reference_frame self._rate = rospy.Rate(rospy.get_param('~rate', 1)) self._sensor_last_values = {key: INF for key in RANGE_SENSORS} self._latest_message = rospy.Time.now() self._scan_start = float(rospy.get_param("~scan_start", SCAN_START)) self._scan_end = float(rospy.get_param("~scan_end", SCAN_END)) self._scan_step = float(rospy.get_param("~scan_step", SCAN_STEP)) self._scan_steps = int( (self._scan_end - self._scan_start) / self._scan_step) self._scan_max_influence = float( rospy.get_param("~scan_max_influence", RELEVANCE_DISTANCE)) self._scan_influence_smoothing = float( rospy.get_param("~scan_influence_smoothing", EFFECT_SMOOTHING)) self._scan_max_range = float( rospy.get_param("~scan_max_range", MAX_RANGE)) self._scan_min_range = float( rospy.get_param("~scan_min_range", MIN_RANGE)) self._scan_robot_radius = float( rospy.get_param("~scan_robot_radius", ROBOT_RADIUS)) for sensor in RANGE_SENSORS: rospy.Subscriber(robot_root + SENSOR_PREFIX + sensor, Range, partial(self.range_handler, sensor=sensor)) self._running = True
def __init__(self): # Publish binary, thresholded image (for debugging)? self.binary_publish = True # Pull ROS parameters from launch file: param = rospy.search_param("bounding_box_topic") self.bounding_boxes_top = rospy.get_param(param) param = rospy.search_param("image_topic") self.img_top = rospy.get_param(param) param = rospy.search_param("binary_image_topic") self.binary_img_top = rospy.get_param(param) param = rospy.search_param("pose_difference_topic") self.pose_diff_top = rospy.get_param(param) param = rospy.search_param("camera_frame") self.cam_frame = rospy.get_param(param) param = rospy.search_param("odometry_frame") self.odom_frame = rospy.get_param(param) param = rospy.search_param("map_frame") self.map_frame = rospy.get_param(param) param = rospy.search_param("true_stop_frame") self.true_stop_frame = rospy.get_param(param) # From '/home/robot/dd2419_ws/src/course_packages/dd2419_launch/calibration/camera.yaml' or topic: /cf1/camera/camera_info # [ fx , s , x0 , 0 , fy , y0 , 0 , 0 , 1 ] self.cam_mat = [231.250001, 0.000000, 320.519378, 0.000000, 231.065552, 240.631482, 0.000000, 0.000000, 1.000000] # From '/home/robot/dd2419_ws/src/course_packages/dd2419_resources/worlds_json/______.world.json' self.sign_dim = [0.20, 0.20] # Initialize subscriber to bounding box rospy.Subscriber(self.bounding_boxes_top, BoundingBoxes, self.boxes_cb) # Initialize subscriber to image rospy.Subscriber(self.img_top, Image, self.img_cb) # Initialize CvBridge self.bridge = CvBridge() # Initialize publisher for binary, thresholded sign (helps for debugging) if self.binary_publish: self.image_pub = rospy.Publisher(self.binary_img_top, Image, queue_size=1) # Initialize tf broadcaster self.br = tf2_ros.TransformBroadcaster() # Listen to other tf transforms self.tfBuffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tfBuffer) # Initialize publisher for differecne between detected and true sign pose self.pose_diff_pub = rospy.Publisher(self.binary_img_top, PoseStamped, queue_size=2)
def test_param_api(self): # test get_param_names param_names = rospy.get_param_names() for n in ['/param1', 'param1', '~param1', 'param_int', 'param_float']: self.assert_(rospy.resolve_name(n) in param_names) # test has_param self.assert_(rospy.has_param('/run_id')) self.assert_(rospy.has_param('/param1')) self.assert_(rospy.has_param('param1')) self.assert_(rospy.has_param('~param1')) # test search_param self.assertEquals(None, rospy.search_param('not_param1')) self.assertEquals(rospy.resolve_name('~param1'), rospy.search_param('param1')) self.assertEquals(rospy.resolve_name('parent_param'), rospy.search_param('parent_param')) self.assertEquals("/global_param", rospy.search_param('global_param')) # test get_param on params set in rostest file self.assertEquals('global_param1', rospy.get_param('/param1')) self.assertEquals('parent_param1', rospy.get_param('param1')) self.assertEquals('private_param1', rospy.get_param('~param1')) # - type tests self.assertEquals(1, rospy.get_param('param_int')) self.assertAlmostEquals(2., rospy.get_param('param_float')) self.assertEquals(True, rospy.get_param('param_bool')) self.assertEquals("hello world", rospy.get_param('param_str')) # test default behavior get_param try: rospy.get_param('not_param1') self.fail("should have raised KeyError") except KeyError: pass self.assertEquals('parent_param1', rospy.get_param('param1', 'foo')) self.assertEquals('private_param1', rospy.get_param('~param1', 'foo')) self.assertEquals('myval', rospy.get_param('not_param1', 'myval')) self.assertEquals('myval', rospy.get_param('~not_param1', 'myval')) self.assertEquals(None, rospy.get_param('not_param1', None)) self.assertEquals(None, rospy.get_param('~not_param1', None)) # test set/get roundtrips vals = [ '1', 1, 1., [1, 2, 3, 4], {'a': 1, 'b': 2}] for v in vals: self.failIf(rospy.has_param("cp_param")) try: rospy.get_param('cp_param1') self.fail("should have thrown KeyError") except KeyError: pass self.assertEquals(None, rospy.get_param('cp_param', None)) self.assertEquals("default", rospy.get_param('cp_param', "default")) rospy.set_param("cp_param", v) self.assert_(rospy.has_param("cp_param")) self.assertEquals(v, rospy.get_param("cp_param")) self.assertEquals(rospy.resolve_name('cp_param'), rospy.search_param('cp_param')) # erase the param and recheck state rospy.delete_param('cp_param') self.failIf(rospy.has_param("cp_param")) self.assertEquals(None, rospy.get_param('cp_param', None)) self.assertEquals("default", rospy.get_param('cp_param', "default")) self.assertEquals(None, rospy.search_param('cp_param'))
def init(): """ Initialization handles use with just python or in a launch file :return: start_pt Starting point (x,y) where the car needs to start :rtype: list :return: use_rviz Flag to enable/disable rviz visualization :rtype: bool :return: use_matplotlib Flag to enable/disable pure pursuit animation :rtype: bool :return: cx List of x coordinates :rtype: list :return: cy List of y coordinates :rtype: list :return cyaw List of yaw values :rtype: list :return ck List of curvature values :rtype: list """ # grab parameters from launch-file start_pt_param = rospy.search_param('start_pt') use_rviz_param = rospy.search_param('use_rviz') use_matplotlib_param = rospy.search_param('use_matplotlib') start_pt = rospy.get_param(start_pt_param, init_pt) if type(start_pt) == type(''): start_pt = start_pt.split(',') start_pt = [float(curr) for curr in start_pt] coords_param = rospy.search_param('coords_param') coords = rospy.get_param(coords_param) # print(coords_param) coords = json.loads(coords) # print(coords) # cx, cy = create_smooth_path(coords) cx, cy, cyaw, ck = create_smooth_path(coords) use_rviz = rospy.get_param(use_rviz_param, False) use_matplotlib = rospy.get_param(use_matplotlib_param, visualize_plot) return start_pt, use_rviz, use_matplotlib, cx, cy, cyaw, ck
def run(): rospy.init_node('p5a') full_param_name = rospy.search_param('location') loc = rospy.get_param(full_param_name) full_param_name = rospy.search_param('turtlename') name = rospy.get_param(full_param_name) rospy.wait_for_service('spawn') spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn) spawner(2 + loc, 9.1 - loc, 0, name) if loc == 0: rospy.wait_for_service('kill') killer = rospy.ServiceProxy('kill', turtlesim.srv.Kill) killer('turtle1') rospy.Subscriber(name + '/pose', Pose, callback) pub = rospy.Publisher(name + '/cmd_vel', Twist, queue_size=10) rate = rospy.Rate(0.5) message = Twist() i = 0 while i < 10: if i % 2 == 0: message.linear.x = 2 message.linear.y = 0 message.linear.z = 0 message.angular.x = 0 message.angular.y = 0 message.angular.z = 0 else: message.linear.x = 0 message.linear.y = 0 message.linear.z = 0 message.angular.x = 0 message.angular.y = 0 message.angular.z = math.pi / 2 i = i + 1 pub.publish(message) rate.sleep()
def get_frame_id(): frame_id = rospy.get_param("~frame_id", "gps") if frame_id[0] != "/": """Add the TF prefix""" prefix = "" prefix_param = rospy.search_param("tf_prefix") if prefix_param: prefix = rospy.get_param(prefix_param) if prefix[0] != "/": prefix = "/%s" % prefix return "%s/%s" % (prefix, frame_id) else: return frame_id
def start(self): """ Start the necessary messages to operate the Kobuki. """ if self.started: rospy.logwarn("Warn[MCCKobuki.start]: Already started.") return #rospy.sleep(15) self.subModelUpdate = rospy.Subscriber(self.subModelUpdateTopic, ModelUpdate, self.sub_model_update) subKobukiOdomTopic = rospy.get_param( rospy.search_param('sub_kobuki_odom')) self.subKobukiOdom = rospy.Subscriber(subKobukiOdomTopic, Odometry, self.sub_kobuki_odom) subKobukiBumpTopic = rospy.get_param( rospy.search_param('sub_kobuki_bump')) self.subKobukiBump = rospy.Subscriber(subKobukiBumpTopic, BumperEvent, self.sub_kobuki_bump) pubKobukiVelTopic = rospy.get_param( rospy.search_param('pub_kobuki_vel')) self.pubKobukiVel = rospy.Publisher(pubKobukiVelTopic, Twist, queue_size=32) pubKobukiResetOdomTopic = rospy.get_param( rospy.search_param('pub_kobuki_reset_odom')) self.pubKobukiResetOdom = rospy.Publisher(pubKobukiResetOdomTopic, Empty, queue_size=32) pubPathTopic = rospy.get_param(rospy.search_param('pub_path')) self.pubPath = rospy.Publisher(pubPathTopic, Path, queue_size=32) self.started = True
def __init__(self): full_param_name = rospy.search_param('mpc_dir') mpc_dir = rospy.get_param(full_param_name) mpc_dir = mpc_dir['test_performance']['mpc_dir'] mpc_dir = os.path.join(mpc_dir, 'parameters/default.yaml') self.mpc_params = os.path.abspath(mpc_dir) print(self.mpc_params) self.test_mode = 'mpc' # 'pid' or 'mpc' self.sampling_mode = 'continuous' # 'grid' or 'continuous' self.handtuned_params = True self.params = {} self.idx_array = None self.n_combinations = 1 # will be overwritten in case of grid sampling
def get_frame_id(): frame_id = rospy.get_param('~frame_id', 'gps') if frame_id[0] != "/": """Add the TF prefix""" prefix = "" prefix_param = rospy.search_param('tf_prefix') if prefix_param: prefix = rospy.get_param(prefix_param) if prefix[0] != "/": prefix = "/%s" % prefix return "%s/%s" % (prefix, frame_id) else: return frame_id
def __init__(self): # Pull ROS parameters from launch file: param = rospy.search_param("bounding_box_topic") self.bounding_boxes_top = rospy.get_param(param) param = rospy.search_param("image_topic") self.img_top = rospy.get_param(param) # param = rospy.search_param("pose_difference_topic") # self.pose_diff_top = rospy.get_param(param) param = rospy.search_param("camera_frame") self.cam_frame = rospy.get_param(param) param = rospy.search_param("odometry_frame") self.odom_frame = rospy.get_param(param) param = rospy.search_param("map_frame") self.map_frame = rospy.get_param(param) # From '/home/robot/dd2419_ws/src/course_packages/dd2419_launch/calibration/camera.yaml' or topic: /cf1/camera/camera_info # [ fx , s , x0 , 0 , fy , y0 , 0 , 0 , 1 ] self.cam_mat = [231.250001, 0.000000, 320.519378, 0.000000, 231.065552, 240.631482, 0.000000, 0.000000, 1.000000] # From '/home/robot/dd2419_ws/src/course_packages/dd2419_resources/worlds_json/______.world.json' self.sign_dim = [0.20, 0.20] self.classes = ['narrows_from_left', 'no_bicycle', 'residential', 'roundabout'] # From '/home/robot/dd2419_ws/src/darknet_ros/darknet_ros/config/yolo-lite-cf9-4classes.yaml' # Initialize subscriber to bounding box rospy.Subscriber(self.bounding_boxes_top, BoundingBoxes, self.boxes_cb) # Initialize subscriber to image rospy.Subscriber(self.img_top, Image, self.img_cb) # Initialize CvBridge self.bridge = CvBridge() # Initialize tf broadcaster self.br = tf2_ros.TransformBroadcaster() # Listen to other tf transforms self.tfBuffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tfBuffer)
def main(): rospy.init_node('point_planner', anonymous=True) name = rospy.get_namespace().split('/')[1] rospy.loginfo("waiting for params to become available") while not rospy.has_param( '/planners/update_freq') and not rospy.is_shutdown( ): # wait for the param server to load pass rospy.loginfo("params found") param_name = rospy.search_param('planners/update_freq') update_period = 1 / int(rospy.get_param(param_name)) p = Planner(name, update_period) rospy.spin()
def updateArgs(arg_defaults): '''Look up parameters starting in the driver's private parameter space, but also searching outer namespaces. ''' args = {} print("processing args") for name, val in arg_defaults.iteritems(): full_name = rospy.search_param(name) if full_name is None: args[name] = val else: args[name] = rospy.get_param(full_name, val) print("We have args " + val + " value " + args[name]) return (args)
def init(): """ Grabs parameters from launch file, parses them and returns them. :return: speed_limit The desired target speed for the car. :rtype: float :return: path_radius Radius for the circular path :rtype: float :return: geofence radius Radius for the geofence. :rtype: float :return: gear The gear at which we want the car to drive. 0 or 1. :rtype: int :return: target_laps How many laps the car is supposed to drive. :rtype: int """ # grab parameters from launch-file speed_limit_param = rospy.search_param('speed_limit') speed_limit = rospy.get_param(speed_limit_param) speed_limit = float(speed_limit) path_radius_param = rospy.search_param('path_radius') path_radius = rospy.get_param(path_radius_param) path_radius = float(path_radius) geofence_radius_param = rospy.search_param('geofence_radius') geofence_radius = rospy.get_param(geofence_radius_param) geofence_radius = float(geofence_radius) gear_param = rospy.search_param('gear') gear = rospy.get_param(gear_param) gear = int(gear) look_ahead_param = rospy.search_param('look_ahead') look_ahead = rospy.get_param(look_ahead_param) look_ahead = float(look_ahead) pure_pursuit.Lfc = look_ahead # look-ahead distance target_laps_param = rospy.search_param('target_laps') target_laps = rospy.get_param(target_laps_param) target_laps = float(target_laps) front_differentials_param = rospy.search_param('front_differentials') front_differentials = rospy.get_param(front_differentials_param) front_differentials = int(front_differentials) rear_differentials_param = rospy.search_param('rear_differentials') rear_differentials = rospy.get_param(rear_differentials_param) rear_differentials = int(rear_differentials) return speed_limit, path_radius, geofence_radius, gear, target_laps
def param_talker(): rospy.init_node('param_talker') # fetch a /global parameter global_example = rospy.get_param("/global_example") # getting from launch def rospy.loginfo("%s is %s", rospy.resolve_name('/global_example'), global_example) # fetch the utterance parameter from our parent namespace utterance = rospy.get_param('utterance') rospy.loginfo("%s is %s", rospy.resolve_name('utterance'), utterance) # fetch topic_name from the ~private namespace topic_name = rospy.get_param('~topic_name') rospy.loginfo("%s is %s", rospy.resolve_name('~topic_name'), topic_name) # fetch a parameter, using 'default_value' if it doesn't exist default_param = rospy.get_param('default_param', 'default_value') rospy.loginfo('%s is %s', rospy.resolve_name('default_param'), default_param) # fetch a group (dictionary) of parameters gains = rospy.get_param('gains') #getting gain from launch p, i, d = gains['P'], gains['I'], gains['D'] rospy.loginfo("gains are %s, %s, %s", p, i, d) # set some parameters rospy.loginfo('setting parameters...') rospy.set_param('list_of_floats', [1., 2., 3., 4.]) rospy.set_param('bool_True', True) rospy.set_param('~private_bar', 1+2) rospy.set_param('to_delete', 'baz') rospy.loginfo('...parameters have been set') # delete a parameter if rospy.has_param('to_delete'): rospy.delete_param('to_delete') rospy.loginfo("deleted %s parameter"%rospy.resolve_name('to_delete')) else: rospy.loginfo('parameter %s was already deleted'%rospy.resolve_name('to_delete')) # search for a parameter param_name = rospy.search_param('global_example') rospy.loginfo('found global_example parameter under key: %s'%param_name) # publish the value of utterance repeatedly pub = rospy.Publisher(topic_name, String, queue_size=10) while not rospy.is_shutdown(): pub.publish(utterance) rospy.loginfo(utterance) #print utterance #print p,i,d rospy.sleep(1)
def init_regex_list(self): num_command = 0 if rospy.search_param("MAX_COMMAND_COUNT") is None: rospy.logerr("check yaml file") exit() while True: str1 = "command" + str(num_command) if rospy.has_param(str1) == False: break command_regex = rospy.get_param(str1) self.regex_list.append(command_regex) num_command += 1 for i in range(0, len(self.regex_list)): print(self.regex_list[i])
def initialize(self): self.status_pub = rospy.Publisher(rospy.remap_name(self.ns) + "/status", GoalStatusArray, latch=True, queue_size=50) self.result_pub = rospy.Publisher(rospy.remap_name(self.ns) + "/result", self.ActionResult, queue_size=50) self.feedback_pub = rospy.Publisher(rospy.remap_name(self.ns) + "/feedback", self.ActionFeedback, queue_size=50) self.goal_sub = rospy.Subscriber( rospy.remap_name(self.ns) + "/goal", self.ActionGoal, self.internal_goal_callback) self.cancel_sub = rospy.Subscriber( rospy.remap_name(self.ns) + "/cancel", GoalID, self.internal_cancel_callback) # read the frequency with which to publish status from the parameter server # if not specified locally explicitly, use search param to find actionlib_status_frequency resolved_status_frequency_name = rospy.remap_name( self.ns) + "/status_frequency" if rospy.has_param(resolved_status_frequency_name): self.status_frequency = rospy.get_param( resolved_status_frequency_name, 5.0) rospy.logwarn( "You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency." ) else: search_status_frequency_name = rospy.search_param( "actionlib_status_frequency") if search_status_frequency_name is None: self.status_frequency = 5.0 else: self.status_frequency = rospy.get_param( search_status_frequency_name, 5.0) status_list_timeout = rospy.get_param( rospy.remap_name(self.ns) + "/status_list_timeout", 5.0) self.status_list_timeout = rospy.Duration(status_list_timeout) self.status_timer = threading.Thread( None, ros_timer, None, (self.publish_status_async, self.status_frequency)) self.status_timer.start()
def miroline(): global tetha, start_x, start_y param_name = rospy.search_param('virtual_x') VX = rospy.get_param(param_name) param_name = rospy.search_param('virtual_y') VY = rospy.get_param(param_name) start_x = VX #i take this information for fix the miro position is move out of the straight line start_y = VY ax = float(x) #actual position of miro ay = float(y) #actual position of miro #rospy.loginfo('ax :{} , ay:{}'.format(ax, ay)) px = ax - float( (ay * (VX - ax) / (VY - ay)) ) #this point corrispont to the size of the side of the triangole that is along x py = ay - float((ax * (VY - ay) / (VX - ax))) #rospy.loginfo('ax :{} , ay:{}'.format(px, py)) sq = px**2 + py**2 py = math.sqrt(sq) # py became the other side of the triangle if px != 0: #& py!=0 : tetha = math.acos(px / py)
def main(): rospy.init_node('auv_pf', anonymous=True) rospy.loginfo("Successful initilization of node") param = rospy.search_param("measurement_period") T_meas = float(rospy.get_param(param)) pf = auv_pf() rospy.loginfo("Particle filter class successfully created") meas_rate = rospy.Rate(1 / T_meas) while not rospy.is_shutdown(): pf.measurement() meas_rate.sleep()
def __init__(self, feedback, uuid=None, priority=0, topic=None, frequency=common.HEARTBEAT_HZ, lock=None): """ Constructor. """ if lock is None: lock = threading.RLock() self.lock = lock """ .. _Big_Requester_Lock: Big Requester Lock. This recursive Python threading lock serializes access to requester data. The requester *feedback* function is always invoked holding it. All :class:`.Requester` methods acquire it automatically, whenever needed. In any other thread, acquire it when updating shared request set objects. Never hold it when sleeping or waiting for I/O. """ if uuid is None: uuid = unique_id.fromRandom() self.requester_id = uuid """ :class:`uuid.UUID` of this requester. """ self.rset = RequestSet([], self.requester_id) """ :class:`.RequestSet` containing the current status of every :class:`.ResourceRequest` made by this requester. All requester operations are done using this object and its contents. """ self.priority = priority """ Default for new requests' priorities if none specified. """ self.feedback = feedback # requester feedback if topic is None: topic_param = rospy.search_param('topic_name') if topic_param is None: topic = common.SCHEDULER_TOPIC else: topic = rospy.get_param(topic_param) self.pub_topic = topic self.sub_topic = common.feedback_topic(uuid, topic) rospy.loginfo('ROCON requester feedback topic: ' + self.sub_topic) self.sub = rospy.Subscriber(self.sub_topic, SchedulerRequests, self._feedback, queue_size=1, tcp_nodelay=True) self.pub = rospy.Publisher(self.pub_topic, SchedulerRequests, latch=True, queue_size=1) self.time_delay = rospy.Duration(1.0 / frequency) self._set_timer()
def main(): rospy.init_node("axis_driver") arg_defaults = { 'hostname': '192.168.0.90', # default IP address 'username': '******', # default login name 'password': '', 'width': 640, 'height': 480, 'frame_id': 'axis_camera', 'camera_info_url': '' } # Look up parameters starting in the driver's private parameter # space, but also searching outer namespaces. Defining them in a # higher namespace allows the axis_ptz.py script to share parameters # with the driver. args = {} for name, val in arg_defaults.iteritems(): full_name = rospy.search_param(name) if full_name is None: args[name] = val else: args[name] = rospy.get_param(full_name, val) # resolve frame_id with tf_prefix (unless already absolute) if args['frame_id'][0] != '/': # not absolute? tf_prefix = rospy.search_param('tf_prefix') prefix_val = '' if tf_prefix is not None: # prefix defined? prefix_val = rospy.get_param(tf_prefix) if prefix_val[0] != '/': # prefix not absolute? prefix_val = '/' + prefix_val args['frame_id'] = prefix_val + '/' + args['frame_id'] Axis(**args) rospy.spin()
def __init__(self): kQueueSize = 15 topics = {} try: node_name = rospy.get_name() topics['image_sub'] = rospy.get_param( rospy.search_param("camera_image_topic")) topics['mask_sub'] = rospy.get_param( rospy.search_param("detector_out_topic")) topics['track_sub'] = rospy.get_param( rospy.search_param("tracker_out_topic")) topics['add_entry_pub'] = rospy.get_param( rospy.search_param("add_entity_topic")) topics['detect_pub'] = rospy.get_param( rospy.search_param("detector_input_topic")) topics['track_pub'] = rospy.get_param( rospy.search_param("tracker_input_topic")) topics['out_pub'] = rospy.get_param( rospy.search_param("image_out_topic")) except rospy.ROSException: print("could not get param name") self.image_sub = rospy.Subscriber(topics['image_sub'], CompressedImage, self._callback_image, queue_size=kQueueSize) self.mask_sub = rospy.Subscriber(topics['mask_sub'], MaskFrame, self._callback_mask, queue_size=kQueueSize) self.track_sub = rospy.Subscriber(topics['track_sub'], EntitiesFrameMsg, self._callback_tracker, queue_size=kQueueSize) self.add_entry_pub = rospy.Publisher(topics['add_entry_pub'], AddEntityRequestMsg, queue_size=kQueueSize) self.detect_pub = rospy.Publisher(topics['detect_pub'], CompressedImage, queue_size=kQueueSize) self.track_pub = rospy.Publisher(topics['track_pub'], CompressedImage, queue_size=kQueueSize) self.out_pub = rospy.Publisher(topics['out_pub'], CompressedImage, queue_size=kQueueSize) self.shouldInitialize = True self.isInitialized = False self.key_frame_msg = None self.counter = 1
def main(): rospy.init_node('jointstate_publisher') rospy.loginfo("Joint State Publisher Node Started") Wheels = JointState() Wheels.name = ["left_wheel_to_motors", "right_wheel_to_motors"] Wheels.position = [0.0, 0.0] # Create wheel joint state publisher wheels = rospy.Publisher('Wheels', JointState, queue_size=10) # Look for robot Parameters robotParams = rospy.search_param('/Robot name') if robotParams: name = rospy.get_param('/Robot name') rospy.loginfo("Robot Parameters loaded for : %s", name) # Retrieve Parameters for physical properties of the robot. _leftTPR = rospy.get_param('/leftTicksPerRotation') _rightTPR = rospy.get_param('/rightTicksPerRotation') else: rospy.logwarn("Robot Parameters not loaded") _lstate = 0 _rstate = 0 def lstateCB(lstat): global _lstate _lstate = lstat.data def rstateCB(rstat): global _rstate _rstate = rstat.data lstateSub = rospy.Subscriber('lstate', Float64, lstateCB) rstateSub = rospy.Subscriber('rstate', Float64, rstateCB) def moveWheels(): left_wheel_pos = (2 * pi / _leftTPR) * _lstate right_wheel_pos = (2 * pi / _rightTPR) * _rstate if left_wheel_pos > pi: left_wheel_pos = -pi if left_wheel_pos < -pi: left_wheel_pos = pi if right_wheel_pos > pi: right_wheel_pos = -pi if right_wheel_pos < -pi: right_wheel_pos = pi Wheels.position[0] = left_wheel_pos Wheels.position[1] = right_wheel_pos wheels.publish(Wheels)
def main(): rospy.init_node("axis") arg_defaults = { 'hostname': '192.168.0.64', 'username': '******', 'password': '******', 'flip': True, } args = {} for name in arg_defaults: args[name] = rospy.get_param(rospy.search_param(name), arg_defaults[name]) AxisPTZ(**args) rospy.spin()
def initialize_origin(): global _origin rospy.init_node('initialize_origin', anonymous=True) origin_pub = rospy.Publisher('/local_xy_origin', PoseStamped, latch=True, queue_size=2) diagnostic_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=2) origin_name = rospy.get_param('~local_xy_origin', 'auto') rospy.loginfo('Local XY origin is "' + origin_name + '"') origin_frame_id = rospy.get_param(rospy.search_param('local_xy_frame'), 'map') origin_frame_identity = rospy.get_param('~local_xy_frame_identity', origin_frame_id + "__identity") rospy.loginfo('Local XY frame ID is "' + origin_frame_id + '"') if len(origin_frame_id): tf_broadcaster = tf.TransformBroadcaster() else: tf_broadcaster = None if origin_name != "auto": origin_list = rospy.get_param('~local_xy_origins', []) _origin = make_origin_from_list(origin_frame_id, origin_name, origin_list) if _origin is not None: origin_pub.publish(_origin) else: origin_name = "auto" if origin_name == "auto": sub = rospy.Subscriber("gps", NavSatFix) sub.impl.add_callback(gps_callback, (origin_frame_id, origin_pub, sub)) rospy.loginfo('Subscribed to NavSat on ' + sub.resolved_name) while not rospy.is_shutdown(): if tf_broadcaster: # Publish transform involving map (to an anonymous unused # frame) so that TransformManager can support /tf<->/wgs84 # conversions without requiring additional nodes. tf_broadcaster.sendTransform((0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), origin_frame_identity, origin_frame_id) diagnostic_pub.publish( make_diagnostic(_origin, (origin_name != "auto"))) rospy.sleep(1.0)
def obstacles(self): """Grabs simulated obstacles simulated Lidar is using""" if self._obstacles is not None: return self._obstacles obstacles_param = rospy.search_param('obstacles') obstacles_from_param = rospy.get_param(obstacles_param, None) if type(obstacles_from_param) == type([]): self._obstacles = obstacles_from_param for obstacle_edges in self._obstacles: # add last segment to close polygon obstacle_edges += [obstacle_edges[0]] else: self._obstacles = [] return self._obstacles
def main(): rospy.init_node("axis") arg_defaults = { 'hostname': '192.168.0.65:554', 'username': '******', 'password': '******', 'width': 640, 'height': 480 } args = {} for name in arg_defaults: args[name] = rospy.get_param(rospy.search_param(name), arg_defaults[name]) Axis(**args) rospy.spin()
def init(): """ Grabs parameters from launch file, parses them and returns them. :return: speed_limit The desired target speed for the car. :rtype: float :return: path_radius Radius for the circular path :rtype: float :return: geofence radius Radius for the geofence. :rtype: float :return: gear The gear at which we want the car to drive. 0 or 1. :rtype: int :return: target_laps How many laps the car is supposed to drive. :rtype: int :return: front_differentials Front differentials for the car. Locked or unlocked. :rtype: int :return: rear_differentials Rear differentials for the car. Locked or unlocked. :rtype: int """ speed_limit_param = rospy.search_param('speed_limit') speed_limit = rospy.get_param(speed_limit_param) print('speed limit') print(speed_limit) speed_limit = float(speed_limit) path_radius_param = rospy.search_param('path_radius') path_radius = rospy.get_param(path_radius_param) path_radius = float(path_radius) geofence_radius = rospy.search_param('geofence_radius') geofence_radius = rospy.get_param(geofence_radius) geofence_radius = float(geofence_radius) gear_param = rospy.search_param('gear') gear = rospy.get_param(gear_param) gear = int(gear) target_laps_param = rospy.search_param('target_laps') target_laps = rospy.get_param(target_laps_param) target_laps = float(target_laps) front_differentials_param = rospy.search_param('front_differentials') front_differentials = rospy.get_param(front_differentials_param) front_differentials = int(front_differentials) rear_differentials_param = rospy.search_param('rear_differentials') rear_differentials = rospy.get_param(rear_differentials_param) rear_differentials = int(rear_differentials) return speed_limit, path_radius, geofence_radius, gear, target_laps, front_differentials, rear_differentials
def __init__(self): self.listener = tf.TransformListener() self.br = tf.TransformBroadcaster() self.dict_pub = rospy.Publisher('/robomuse/marker_dictionary_map', idarray, queue_size=10) self.id_dictionary = np.array([]) self.id_visible_flags = np.array([]) param_name = rospy.search_param('folder_name') v = rospy.get_param(param_name) self.nst = v print self.nst self.posearray = [] self.goalarray = [] self.goalpub = [] self.flag1 = 0
def main(): rospy.init_node("get_traj_params_test") args = {} groups = None try: if rospy.search_param('groups'): groups = rospy.get_param('groups') else: rospy.logerr('Param groups not found!') #print name except rospy.ROSException, e: rospy.logerror('%s: %s' % (e, _name))
def main(): rospy.init_node("axis_twist") arg_defaults = { 'hostname': '192.168.0.90', 'username': '', 'password': '', 'flip': True, } args = {} for name in arg_defaults: args[name] = rospy.get_param(rospy.search_param(name), arg_defaults[name]) AxisPTZ(**args) rospy.spin()
def main(): rospy.init_node("axis") arg_defaults = { 'hostname': '192.168.0.90', 'username': '', 'password': '', 'delay': 0.0, 'max_command_rate': 0.0, 'flip': True, } args = {} for name in arg_defaults: args[name] = rospy.get_param(rospy.search_param(name), arg_defaults[name]) rospy.sleep(args['delay']) AxisPTZ(**args) rospy.spin()