def __init__(self): rp = RosPack() pkgpath = rp.get_path('panda_bridge_ros') pkgpath += '/config/honda_civic_touring_2016_can_for_cantools.dbc' self.parser = load_dbc_file(pkgpath) self.p = Panda() self.p.set_safety_mode(self.p.SAFETY_ALLOUTPUT) self.gas_val = 0 self.brake_val = 0 self.steer_val = 0 self.cruise_modifier = 0.0 self.sub_engine = rospy.Subscriber('/joy', Joy, self.engine_cb, queue_size=1) self.sub_steer = rospy.Subscriber('/steering_amount', Int16, self.steer_cb, queue_size=1) self.sub_brake = rospy.Subscriber('/braking_amount', Int16, self.brake_cb, queue_size=1) self.ddr = DDynamicReconfigure("drive_car") self.ddr.add_variable( "rate_buttons", "rate as to which send the button presses for cruise control", 1, 1, 100) self.add_variables_to_self() self.ddr.start(self.dyn_rec_callback)
def __init__(self): self.pub = rospy.Publisher('/twist_raw', TwistStamped, queue_size=1) self.ddr = DDynamicReconfigure('twist_gains') self.ddr.add_variable('linear_x_gain', "gain for twist.linear.x", 1.0, min=-5.0, max=5.0) self.ddr.add_variable('angular_z_gain', "gain for twist.angular.z", 25.0, min=0.0, max=100.0) self.ddr.add_variable("max_angular_z", "max +- angular.z", 6.0, min=0.0, max=20.0) self.ddr.start(self.dyn_rec_callback) self.sub = rospy.Subscriber('/twist_raw_tmp', TwistStamped, self._cb, queue_size=1) rospy.loginfo("TwistStampedGains initialized.") rospy.loginfo("Listening to topic: " + str(self.sub.resolved_name)) rospy.loginfo("Publishing on topic: " + str(self.pub.resolved_name)) rospy.loginfo("Applying gains: x: " + str(self.linear_x_gain) + " y: " + str(self.angular_z_gain))
def __init__(self, config): """ Read the defaults from the config object so that we dont change the launch-defaults right away with different reconfig defaults We then just put whatever reconfig we get into the BB with the same key """ self.bb = pt.blackboard.Blackboard() # DynamicDynamicReConfig # because the .cfg way of doing this is pai1 self.ddrc = DDynamicReconfigure("smarc_bt_reconfig") # name, description, default value, min, max, edit_method self.ddrc.add_variable(bb_enums.MIN_ALTITUDE, "default:{}".format(config.MIN_ALTITUDE), config.MIN_ALTITUDE, -2, 50) self.ddrc.add_variable(bb_enums.MAX_DEPTH, "default:{}".format(config.MAX_DEPTH), config.MAX_DEPTH, 0, 50) self.ddrc.add_variable( bb_enums.MAX_ALT_FAIL_SECONDS, "default:{}".format(config.MAX_ALT_FAIL_SECONDS), config.MAX_ALT_FAIL_SECONDS, 0, 10) rospy.loginfo("Started dynamic reconfig server with keys:{}".format( self.ddrc.get_variable_names())) # this should be the last thing in this init self.ddrc.start(self.reconfig_cb)
def __init__(self): self.t0 = rospy.Time.now() self.timer = None self.config = None self.dr_client = None self.new_server = False self.pub = None self.ddr = DDynamicReconfigure("") self.ddr.add_variable("server", "dynamic reconfigure server", "") self.ddr.add_variable("param", "dr parameter name", "") self.ddr.add_variable("topic", "topic name", "signal") # The dynamic reconfigure probably won't go lower than 0.1, # 0.01 only works for topics self.ddr.add_variable("period", "update period", 0.05, 0.01, 10.0) self.num_freqs = 3 for i in range(self.num_freqs): si = str(i) fr = float(i) / float(self.num_freqs) self.ddr.add_variable("freq" + si, "frequency " + si, 1.0 - fr * fr, 0.0, 10.0) self.ddr.add_variable("amp" + si, "amplitude " + si, fr, 0.0, 2.0) self.ddr.add_variable("phase" + si, "phase " + si, 0.0, -math.pi, math.pi) self.ddr.add_variable("offset" + si, "offset " + si, 0.0, -5.0, 5.0) # TODO(lucasw) sine, sawtooth, ramp, square self.ddr.start(self.config_callback)
def __init__(self): # Create a D(ynamic)DynamicReconfigure self.ddr = DDynamicReconfigure("class_example") # Add variables (name, description, default value, min, max, edit_method) self.ddr.add_variable("decimal", "float/double variable", 0.0, -1.0, 1.0) self.ddr.add_variable("integer", "integer variable", 0, -1, 1) self.ddr.add_variable("bool", "bool variable", True) self.ddr.add_variable("string", "string variable", "string dynamic variable") enum_method = self.ddr.enum([ self.ddr.const("Small", "int", 0, "A small constant"), self.ddr.const("Medium", "int", 1, "A medium constant"), self.ddr.const("Large", "int", 2, "A large constant"), self.ddr.const("ExtraLarge", "int", 3, "An extra large constant") ], "An enum example") self.ddr.add_variable("enumerate", "enumerate variable", 1, 0, 3, edit_method=enum_method) self.add_variables_to_self() self.ddr.start(self.dyn_rec_callback)
def __init__(self): # The Null behavior will automatically process a 0-velocity Twist at 10hz self.null_behavior = Behavior(self.process_command, 'zero', freq=10) self.behaviors = {'zero': self.null_behavior} self.active_behavior_name = 'zero' self.set_active_behavior('zero') self.tf = TransformListener() self.ddynrec = DDynamicReconfigure("example_dyn_rec") # Transformers are functions capable of processing incoming data in a variety of formats. # They are functions that take input of whatever type the topic is, and produce a transformers.Command # object. alt_pid = transformers.PIDAltController(self.tf, self.ddynrec) pos_pid = transformers.PIDPosController(self.tf, self.ddynrec, alt_pid) pos_cam_pid = transformers.PIDPosCamController(self.tf, self.ddynrec, pos_pid) self.transformers = { 'cmd_vel': (Twist, transformers.cmd_vel), 'cmd_takeoff': (Empty, transformers.cmd_takeoff), 'cmd_land': (Empty, transformers.cmd_land), 'cmd_pos': (PoseStamped, pos_pid.cmd_pos), 'cmd_vel_alt': (VelAlt, alt_pid.cmd_vel_alt), 'cmd_cam_pos': (PosCam, pos_cam_pid.cmd_pos_cam), 'cmd_rel_pos': (PoseStamped, pos_pid.cmd_pos) } """:type : dict[str, (str, (Any) -> transformers.Command)]""" # Subscribe to the behaviors passed as ROS parameters starting_behaviors = rospy.get_param('~behaviors', []) for b in starting_behaviors: behavior = Behavior(self.process_command, b, freq=20) behavior.subscribe(self.transformers) self.behaviors[b] = behavior # Filters constrain final output before it goes to the drone. # Potential examples include last-minute obstacle avoidance, speed limiters, or arena boundary constraints. self.filters = [filters.make_speed_filter(0.1, 0.5, 0.5)] self.vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=0) self.takeoff_pub = rospy.Publisher('/takeoff', Empty, queue_size=0) self.land_pub = rospy.Publisher('/land', Empty, queue_size=0) self.debug_pub = rospy.Publisher('/arbiter/debug', String, queue_size=10) self.active_pub = rospy.Publisher('/arbiter/active_behavior', String, queue_size=10) self.ddynrec.add_variable("midbrain_freq", "Frequency of the main control loop for PIDs", rospy.get_param('~midbrain_freq', 100), 1, 1000) self.start_ddynrec() self.null_behavior.handle_message('cmd_vel', Twist()) rospy.sleep(0.5) rospy.Subscriber('/arbiter/register', RegisterBehavior, self.handle_register) rospy.Subscriber('/arbiter/activate_behavior', String, self.handle_activate)
def __init__(self): rospy.loginfo("Initializing GripperGrasper...") # This node Dynamic params self.ddr = DDynamicReconfigure("grasper") self.max_position_error = self.ddr.add_variable( "max_position_error", "Max absolute value of controller state of any joint to stop closing", 0.002, 0.00001, 0.045) self.timeout = self.ddr.add_variable("timeout", "timeout for the closing action", 5.0, 0.0, 30.0) self.closing_time = self.ddr.add_variable("closing_time", "Time for the closing goal", 2.0, 0.01, 30.0) self.rate = self.ddr.add_variable( "rate", "Rate Hz at which the node closing will do stuff", 5, 1, 50) self.ddr.start(self.ddr_cb) rospy.loginfo("Initialized dynamic reconfigure on: " + str(rospy.get_name())) # Subscriber to the gripper state self.last_state = None self.controller_name = rospy.get_param("~controller_name", None) if not self.controller_name: rospy.logerr("No controller name found in param: ~controller_name") exit(1) self.real_joint_names = rospy.get_param("~real_joint_names", None) if not self.real_joint_names: rospy.logerr( "No real joint names given in param: ~real_joint_names") exit(1) self.state_sub = rospy.Subscriber('/' + self.controller_name + '_controller/state', JointTrajectoryControllerState, self.state_cb, queue_size=1) rospy.loginfo("Subscribed to topic: " + str(self.state_sub.resolved_name)) # Publisher on the gripper command topic self.cmd_pub = rospy.Publisher('/' + self.controller_name + '_controller/command', JointTrajectory, queue_size=1) rospy.loginfo("Publishing to topic: " + str(self.cmd_pub.resolved_name)) # Grasping service to offer self.grasp_srv = rospy.Service( '/' + self.controller_name + '_controller/grasp', Empty, self.grasp_cb) rospy.loginfo("Offering grasp service on: " + str(self.grasp_srv.resolved_name)) rospy.loginfo("Done initializing GripperGrasper!")
def __init__(self): self.n_measurements_consider_valid = 4 # Publishes the area where the pedestrian pass is self.pub_pedestrian_area = rospy.Publisher("/pedestrian_area_reference", PolygonStamped, queue_size=1, latch=True) self.pedestrian_center = [42.3,78.9, -0.48] self.pedestrian_wide = [6.5,4.0] self.publish_pedestrian_area() self.pedestrian_area = DDynamicReconfigure("pedestrian_area") self.pedestrian_area.add_variable('x_pose','x_pose', self.pedestrian_center[0], min= self.pedestrian_center[0] - 1, max = self.pedestrian_center[0] + 1) self.pedestrian_area.add_variable('y_pose','y_pose', self.pedestrian_center[1], min= self.pedestrian_center[1] - 1, max = self.pedestrian_center[1] + 1) self.pedestrian_area.add_variable('x_size','x_size', self.pedestrian_wide[0], min= self.pedestrian_wide[0] - 1, max = self.pedestrian_wide[0] + 1) self.pedestrian_area.add_variable('y_size','y_size', self.pedestrian_wide[1], min= self.pedestrian_wide[1] - 1, max = self.pedestrian_wide[1] + 1) self.pub_pedestrian_area_attention = rospy.Publisher("/pedestrian_area_attention", PolygonStamped, queue_size=1, latch=True) self.pedestrian_recog_area_c = [self.pedestrian_center[0] + self.pedestrian_wide[0]/2.0, self.pedestrian_center[1] + self.pedestrian_wide[1]/2.0] self.pedestrian_recog_area_wide_x = [-2.4, 3.3] self.pedestrian_recog_area_wide_y = self.pedestrian_wide[1]/2 + 0.05 self.publish_pedestrian_area_attention() # Uses as a reference the direction on which the car comes self.pedestrian_area.add_variable('x_pedest_neg','x_pedestrian_neg', self.pedestrian_recog_area_wide_x[0], min= -5.0, max = 0.0) self.pedestrian_area.add_variable('x_pedest_pos','x_pedestrian_pos', self.pedestrian_recog_area_wide_x[1], min= 0.0, max = 5.0) self.pedestrian_area.start(self.pedestrian_changes_cb) self.pointcloud_sub = rospy.Subscriber('/velodyne_downsampled_xyz', PointCloud2, self.velodyne_cb, queue_size=1) self.pub_person_found = rospy.Publisher("/pedestrian_detection", String, queue_size=1) self.pub_person_found_area = rospy.Publisher("/pedestrian_area_detection", Marker, queue_size=1, latch=True) self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
def __init__(self): self.tf = TransformListener() self.ddynrec = DDynamicReconfigure("arbiterlite_configuration") # Transformers are functions capable of processing incoming data in a variety of formats. # They are functions that take input of whatever type the topic is, and produce a transformers.Command # object. alt_pid = transformers.PIDAltController(self.tf, self.ddynrec) pos_pid = transformers.PIDPosController(self.tf, self.ddynrec, alt_pid) pos_cam_pid = transformers.PIDPosCamController(self.tf, self.ddynrec, pos_pid) # topic_name : (type, transformer_func, publish_immediately) self.transformers = { 'high_level/cmd_vel': (Twist, transformers.cmd_vel, False), 'high_level/cmd_takeoff': (Empty, transformers.cmd_takeoff, True), 'high_level/cmd_land': (Empty, transformers.cmd_land, True), 'high_level/cmd_pos': (PoseStamped, pos_pid.cmd_pos, False), 'high_level/cmd_rel_pos': (PoseStamped, pos_pid.cmd_pos, False), 'high_level/cmd_vel_alt': (VelAlt, alt_pid.cmd_vel_alt, False), 'high_level/cmd_cam_pos': (PosCam, pos_cam_pid.cmd_pos_cam, False), } self.last_topic = None self.last_msg = None self.last_timestamp = None for topic, (msgtype, _, immediate) in self.transformers.items(): def handler(msg, topic=topic, immediate=immediate): self.last_topic = topic self.last_msg = msg self.last_timestamp = rospy.Time.now() if immediate: self.update() rospy.Subscriber(topic, msgtype, handler) # Filters constrain final output before it goes to the drone. # Potential examples include last-minute obstacle avoidance, speed limiters, or arena boundary constraints. self.filters = [filters.make_speed_filter(0.2, 0.2, 0.2)] self.vel_pub = rospy.Publisher('low_level/cmd_vel', Twist, queue_size=0) self.takeoff_pub = rospy.Publisher('low_level/takeoff', Empty, queue_size=0) self.land_pub = rospy.Publisher('low_level/land', Empty, queue_size=0) self.rate = rospy.get_param('~rate', 20) # How long to republish an old message before erroring out (seconds) self.timeout = rospy.get_param('~timeout', 1.0) self.start_ddynrec()
def __init__(self): self.config = DDynamicReconfigure() self.config.add_variable("rate", "Frequency to republish updates, in hertz", rospy.get_param('~rate', 10.0), 0.2, 200.0) self.config.add_variable("height", "Height to fly above base frame", rospy.get_param('~height', 2.0), 0.0, 5.0) self.pub = rospy.Publisher('cmd_pos', PoseStamped, queue_size=10) self.sub = rospy.Subscriber('/move_base_simple/goal', PoseStamped, self.onmsg) self.last_msg = PoseStamped() self.has_msg = False self.start_ddynrec()
def _setup_ddr(self): self.ddr = DDynamicReconfigure(rospy.get_name()) rest_params = [ p for p in self._get_rest_parameters() if p['name'] not in self.ignored_parameters ] def enum_method_from_param(p): if p['type'] != 'string': return "" enum_matches = re.findall(r'.*\[(?P<enum>.+)\].*', p['description']) if not enum_matches: return "" enum_names = [str(e.strip()) for e in enum_matches[0].split(',')] enum_list = [self.ddr.const(n, 'str', n, n) for n in enum_names] return self.ddr.enum(enum_list, p['name'] + '_enum') for p in rest_params: level = 0 edit_method = enum_method_from_param(p) if p['type'] == 'int32': self.ddr.add(p['name'], 'int', level, p['description'], p['default'], p['min'], p['max']) elif p['type'] == 'float64': self.ddr.add(p['name'], 'double', level, p['description'], p['default'], p['min'], p['max']) elif p['type'] == 'string': self.ddr.add(p['name'], 'str', level, p['description'], str(p['default']), edit_method=edit_method) elif p['type'] == 'bool': self.ddr.add(p['name'], 'bool', level, p['description'], p['default']) else: rospy.logwarn("Unsupported parameter type: %s", p['type']) self.ddr.start(self._dyn_rec_callback)
class TwistStampedGains(object): def __init__(self): self.pub = rospy.Publisher('/twist_raw', TwistStamped, queue_size=1) self.ddr = DDynamicReconfigure('twist_gains') self.ddr.add_variable('linear_x_gain', "gain for twist.linear.x", 1.0, min=-5.0, max=5.0) self.ddr.add_variable('angular_z_gain', "gain for twist.angular.z", 25.0, min=0.0, max=100.0) self.ddr.add_variable("max_angular_z", "max +- angular.z", 6.0, min=0.0, max=20.0) self.ddr.start(self.dyn_rec_callback) self.sub = rospy.Subscriber('/twist_raw_tmp', TwistStamped, self._cb, queue_size=1) rospy.loginfo("TwistStampedGains initialized.") rospy.loginfo("Listening to topic: " + str(self.sub.resolved_name)) rospy.loginfo("Publishing on topic: " + str(self.pub.resolved_name)) rospy.loginfo("Applying gains: x: " + str(self.linear_x_gain) + " y: " + str(self.angular_z_gain)) def dyn_rec_callback(self, config, level): self.linear_x_gain = config['linear_x_gain'] self.angular_z_gain = config['angular_z_gain'] self.max_z = config['max_angular_z'] rospy.loginfo("TwistStampedGains got a reconfigure callback: " + str(config)) return config def _cb(self, msg): # rospy.loginfo("in z: " + str(msg.twist.angular.z)) msg.twist.linear.x *= self.linear_x_gain msg.twist.angular.z *= self.angular_z_gain if abs(msg.twist.angular.z) > self.max_z: if msg.twist.angular.z >= 0.0: msg.twist.angular.z = self.max_z else: msg.twist.angular.z = -self.max_z # rospy.loginfo("out z: " + str(msg.twist.angular.z)) self.pub.publish(msg)
def __init__(self, name, x_offset, y_offset, x_size, y_size): # self.n_measurements_consider_valid = 4 self.pub_avoidance_area = rospy.Publisher("/avoidance_area_" + name, PolygonStamped, queue_size=1, latch=True) self.attention_wide = [x_offset, y_offset] #[1.5, 10.0] self.attention_center = [x_size, y_size, 0.0] #[-48.9, 120.0, 0.0] self.attention_area = DDynamicReconfigure("avoidance_aerea" + name) self.attention_area.add_variable('x_offset', 'x_offset', self.attention_center[0], min=self.attention_center[0] - 25.0, max=self.attention_center[0] + 25.0) self.attention_area.add_variable('y_offset', 'y_offset', self.attention_center[1], min=self.attention_center[1] - 25.0, max=self.attention_center[1] + 25.0) self.attention_area.add_variable('x_size', 'x_size', self.attention_wide[0], min=0.0, max=self.attention_wide[0] + 20.0) self.attention_area.add_variable('y_size', 'y_size', self.attention_wide[1], min=0.0, max=self.attention_wide[1] + 5.0) self.publish_attention_area() self.attention_area.start(self.attention_changes_cb) self.points_found = None
def __init__(self): rospy.loginfo("Initializing GripperGrasper...") # This node Dynamic params self.ddr = DDynamicReconfigure("grasper") self.max_position_error = self.ddr.add_variable("max_position_error", "Max absolute value of controller state of any joint to stop closing", 0.002, 0.00001, 0.045) self.timeout = self.ddr.add_variable("timeout", "timeout for the closing action", 5.0, 0.0, 30.0) self.closing_time = self.ddr.add_variable("closing_time", "Time for the closing goal", 2.0, 0.01, 30.0) self.rate = self.ddr.add_variable("rate", "Rate Hz at which the node closing will do stuff", 5, 1, 50) self.ddr.start(self.ddr_cb) rospy.loginfo("Initialized dynamic reconfigure on: " + str(rospy.get_name())) # Subscriber to the gripper state self.last_state = None self.state_sub = rospy.Subscriber('/gripper_controller/state', JointTrajectoryControllerState, self.state_cb, queue_size=1) rospy.loginfo("Subscribed to topic: " + str(self.state_sub.resolved_name)) # Publisher on the gripper command topic self.cmd_pub = rospy.Publisher('/gripper_controller/command', JointTrajectory, queue_size=1) rospy.loginfo("Publishing to topic: " + str(self.cmd_pub.resolved_name)) # Grasping service to offer self.grasp_srv = rospy.Service('/gripper_controller/grasp', Empty, self.grasp_cb) rospy.loginfo("Offering grasp service on: " + str(self.grasp_srv.resolved_name)) rospy.loginfo("Done initializing GripperGrasper!")
def __init__(self): if not rospy.core.is_initialized(): rospy.init_node('laser_test') rospy.loginfo("Initialised rospy node: laser_test") self.tl = TransformListener() self.lp = LaserProjection() # Publishers self.all_laser_pub = rospy.Publisher('/pepper/laser_2', LaserScan, queue_size=1) self.pc_pub = rospy.Publisher('/cloud', PointCloud2, queue_size=1) self.pcl_pub = rospy.Publisher('/cloudl', PointCloud2, queue_size=1) self.pcr_pub = rospy.Publisher('/cloudr', PointCloud2, queue_size=1) self.pc_redone_pub = rospy.Publisher('/cloud_redone', PointCloud2, queue_size=1) self.pc_rereprojected_pub = rospy.Publisher('/cloud_rereprojected', PointCloud2, queue_size=1) # Subscribers left_sub = Subscriber('/pepper/scan_left', LaserScan) front_sub = Subscriber('/pepper/scan_front', LaserScan) right_sub = Subscriber('/pepper/scan_right', LaserScan) self.ts = TimeSynchronizer([left_sub, front_sub, right_sub], 10) rospy.loginfo("Finished intialising") self.ddr = DDR('increment') default_increment = radians(120.0 * 2.0) / 61.0 self.ddr.add_variable('angle_increment', '', default_increment, 0.05, 0.08) # 130.665 self.ddr.add_variable('half_max_angle', '', 120., 115., 145.0) self.ddr.start(self.dyn_rec_callback) self.ts.registerCallback(self.scan_cb) rospy.loginfo("Ready to go.")
#!/usr/bin/env python import rospy from ddynamic_reconfigure_python.ddynamic_reconfigure import DDynamicReconfigure def callback(config, level): #rospy.loginfo("Received reconf call: " + str(config)) return config if __name__ == '__main__': rospy.loginfo("Marathon Parameters - Running") rospy.init_node('marathon_params') # Create a D(ynamic)DynamicReconfigure player = DDynamicReconfigure("player") # Add variables (name, description, default value, min, max, edit_method) # ddynrec.add_variable("decimal", "float/double variable", 0.0, -1.0, 1.0) player.add_variable("Pan_Kp", "Pan Kp", 0.0, 0, 1) player.add_variable("Pan_Ki", "Pan Ki", 0.0, 0, 1) player.add_variable("Pan_Kd", "Pan Kd", 0.0, 0, 0.000010) player.add_variable("Tilt_Kp", "Tilt Kp", 0.0, 0, 1) player.add_variable("Tilt_Ki", "Tilt Ki", 0.0, 0, 1) player.add_variable("Tilt_Kd", "Tilt Kd", 0.0, 0, 0.000010) player.add_variable("Pan_Step", "Pan Step", 0.0, 0, 0.2) player.add_variable("Tilt_Step", "Tilt Step", 0.0, 0, 0.5) player.add_variable("Tilt_Angle", "Tilt Angle", 0.0, -2, 0) player.add_variable("Scan_Rate", "Scan Rate", 0, 0, 10) player.add_variable("Body_Kp", "Body Kp", 0.0, 0, 5)
class ArbiterLite(object): """ The ArbiterLite transforms high-level commands into low-level commands by using utilities from the filters.py file. It subscribes to various topics of the form high_level/cmd_**, then republishes as cmd_vel, takeoff, and land The publishing of this node will run at the configured frequency, but one-time commands like takeoff and land will always be handled immediately. One copy of this node is intended to be created for each physical drone being controlled. """ def __init__(self): self.tf = TransformListener() self.ddynrec = DDynamicReconfigure("arbiterlite_configuration") # Transformers are functions capable of processing incoming data in a variety of formats. # They are functions that take input of whatever type the topic is, and produce a transformers.Command # object. alt_pid = transformers.PIDAltController(self.tf, self.ddynrec) pos_pid = transformers.PIDPosController(self.tf, self.ddynrec, alt_pid) pos_cam_pid = transformers.PIDPosCamController(self.tf, self.ddynrec, pos_pid) # topic_name : (type, transformer_func, publish_immediately) self.transformers = { 'high_level/cmd_vel': (Twist, transformers.cmd_vel, False), 'high_level/cmd_takeoff': (Empty, transformers.cmd_takeoff, True), 'high_level/cmd_land': (Empty, transformers.cmd_land, True), 'high_level/cmd_pos': (PoseStamped, pos_pid.cmd_pos, False), 'high_level/cmd_rel_pos': (PoseStamped, pos_pid.cmd_pos, False), 'high_level/cmd_vel_alt': (VelAlt, alt_pid.cmd_vel_alt, False), 'high_level/cmd_cam_pos': (PosCam, pos_cam_pid.cmd_pos_cam, False), } self.last_topic = None self.last_msg = None self.last_timestamp = None for topic, (msgtype, _, immediate) in self.transformers.items(): def handler(msg, topic=topic, immediate=immediate): self.last_topic = topic self.last_msg = msg self.last_timestamp = rospy.Time.now() if immediate: self.update() rospy.Subscriber(topic, msgtype, handler) # Filters constrain final output before it goes to the drone. # Potential examples include last-minute obstacle avoidance, speed limiters, or arena boundary constraints. self.filters = [filters.make_speed_filter(0.2, 0.2, 0.2)] self.vel_pub = rospy.Publisher('low_level/cmd_vel', Twist, queue_size=0) self.takeoff_pub = rospy.Publisher('low_level/takeoff', Empty, queue_size=0) self.land_pub = rospy.Publisher('low_level/land', Empty, queue_size=0) self.rate = rospy.get_param('~rate', 20) # How long to republish an old message before erroring out (seconds) self.timeout = rospy.get_param('~timeout', 1.0) self.start_ddynrec() def update(self): """ Computes any PID loops and publishes the low-level commands """ if self.last_topic is None: rospy.logwarn_throttle( 5.0, "No messages recieved, unable to publish") return if rospy.Time.now() - self.last_timestamp > rospy.Duration.from_sec(self.timeout): rospy.logwarn_throttle( 5.0, "Last message to {} was stale, refusing to publish".format(self.last_topic)) # TODO(eric) Consider publishing a "stop" command here, rather than relying on the timeout in the drone's driver return _, func, _ = self.transformers[self.last_topic] # Convert to a transformers.Command cmd = func(self.last_msg) # Limit the drone speed, and apply any other filters for func in self.filters: cmd = func(cmd) # Publish the result to the ROS network if cmd.takeoff: self.takeoff_pub.publish(Empty()) vel = Twist() vel.linear.z = 0.5 self.vel_pub.publish(vel) elif cmd.land: self.land_pub.publish(Empty()) vel = Twist() vel.linear.z = -0.5 self.vel_pub.publish(vel) else: self.vel_pub.publish(cmd.vel) def start_ddynrec(self): """ Helper function to start the ddynamic reconfigure server with a callback function that updates the self.ddynrec attribute. Needed to tune PID loops in realtime """ def callback(config, level): """ A callback function used to as the parameter in the ddynrec.start() function. This custom callback function updates the state of self.ddynrec so we can refer to its variables whenever we have access to it. """ rospy.loginfo("Received reconf call: " + str(config)) # Update all variables var_names = self.ddynrec.get_variable_names() for var_name in var_names: self.ddynrec.__dict__[var_name] = config[var_name] return config self.ddynrec.start(callback) def run(self): r = rospy.Rate(self.rate) while not rospy.is_shutdown(): if self.last_topic is not None: _, _, immediate = self.transformers[self.last_topic] if not immediate: # For any topic that is not supposed to be handled immediately upon reception, # calculate the PID loop here at a defined frequency. self.update() try: r.sleep() except rospy.ROSTimeMovedBackwardsException: r = rospy.Rate(self.rate)
class AllPublisher(object): def __init__(self): rp = RosPack() pkgpath = rp.get_path('panda_bridge_ros') pkgpath += '/config/honda_civic_touring_2016_can_for_cantools.dbc' self.parser = load_dbc_file(pkgpath) self.p = Panda() self.p.set_safety_mode(self.p.SAFETY_ALLOUTPUT) self.gas_val = 0 self.brake_val = 0 self.steer_val = 0 self.cruise_modifier = 0.0 self.sub_engine = rospy.Subscriber('/joy', Joy, self.engine_cb, queue_size=1) self.sub_steer = rospy.Subscriber('/steering_amount', Int16, self.steer_cb, queue_size=1) self.sub_brake = rospy.Subscriber('/braking_amount', Int16, self.brake_cb, queue_size=1) self.ddr = DDynamicReconfigure("drive_car") self.ddr.add_variable( "rate_buttons", "rate as to which send the button presses for cruise control", 1, 1, 100) self.add_variables_to_self() self.ddr.start(self.dyn_rec_callback) def add_variables_to_self(self): var_names = self.ddr.get_variable_names() for var_name in var_names: self.__setattr__(var_name, None) def dyn_rec_callback(self, config, level): rospy.loginfo("Received reconf call: " + str(config)) # Update all variables var_names = self.ddr.get_variable_names() for var_name in var_names: self.__dict__[var_name] = config[var_name] return config def engine_cb(self, data): self.cruise_modifier = data.axes[1] def steer_cb(self, data): # print("steer_cb: " + str(data)) if data.data > 3840: self.steer_val = 3840 elif data.data < -3840: self.steer_val = -3840 self.steer_val = data.data def brake_cb(self, data): # print("brake_cb: " + str(data)) if data.data > 1023: self.brake_val = 1023 elif data.data < 0: self.brake_val = 0 self.brake_val = data.data def run(self): print("Publishing...") idx_counter = 0 total_cmds_sent = 0 iterations = 0 last_rate = time.time() while not rospy.is_shutdown(): #print("rate: " + str(self.rate_buttons)) if iterations % (100 / self.rate_buttons) == 0: if self.cruise_modifier != 0.0: if self.cruise_modifier > 0.0: cruise_command = 'accel_res' elif self.cruise_modifier < 0.0: cruise_command = 'decel_set' settings_command = 'none' print("Sending cruise command: " + cruise_command) # (xmission_speed, engine_rpm=2000, odometer=3, idx=0): cmd = create_buttons_command(cruise_command, settings_command, idx_counter) print("command is: " + str(cmd)) print("Sending: " + str(cmd) + " (#" + str(total_cmds_sent) + ") engine modifier val: " + str(self.cruise_modifier)) self.p.can_send(cmd[0], cmd[2], 1) cmd = create_steering_control(self.steer_val, idx_counter)[0] # print("Sending: " + str(cmd) + # " (#" + str(total_cmds_sent) + # ") steer val: " + str(self.steer_val)) self.p.can_send(cmd[0], cmd[2], 1) # (apply_brake, pcm_override, pcm_cancel_cmd, chime, idx): cmd = create_brake_command(self.brake_val, 1, 0, 0, idx_counter) # print("Sending: " + str(cmd) + # " (#" + str(total_cmds_sent) + # ") brake val: " + str(self.brake_val)) self.p.can_send(cmd[0], cmd[2], 1) idx_counter += 1 idx_counter %= 4 # its wrong, but who cares total_cmds_sent += 1 time.sleep(0.01) iterations += 1
Author: Sammy Pfeiffer """ import rospy from ddynamic_reconfigure_python.ddynamic_reconfigure import DDynamicReconfigure def dyn_rec_callback(config, level): rospy.loginfo("Received reconf call: " + str(config)) return config if __name__ == '__main__': rospy.init_node('test_ddynrec') # Create a D(ynamic)DynamicReconfigure ddynrec = DDynamicReconfigure("example_dyn_rec") # Add variables (name, description, default value, min, max, edit_method) ddynrec.add_variable("decimal", "float/double variable", 0.0, -1.0, 1.0) ddynrec.add_variable("integer", "integer variable", 0, -1, 1) ddynrec.add_variable("bool", "bool variable", True) ddynrec.add_variable("string", "string variable", "string dynamic variable") enum_method = ddynrec.enum([ ddynrec.const("Small", "int", 0, "A small constant"), ddynrec.const("Medium", "int", 1, "A medium constant"), ddynrec.const("Large", "int", 2, "A large constant"), ddynrec.const("ExtraLarge", "int", 3, "An extra large constant")], "An enum example") ddynrec.add_variable("enumerate", "enumerate variable", 1, 0, 3, edit_method=enum_method) # Start the server ddynrec.start(dyn_rec_callback)
class Arbiter: """ The Arbiter is a mutiplexer that reads cmd_* topics from several namespaces, converts them into standard cmd_vel form, and forwards one of them into the global namespace. It receives information about which behavior (namespace) should be selected from String messages on the /arbiter/activate_behavior topic. This information comes from the planning stack. In the future, a voting system could replace this mechanism. The special value "zero" represents an internal behavior that stops the vehicle. It also publishes the name of the active behavior to arbiter/active_behavior. """ def __init__(self): # The Null behavior will aafutomatically process a 0-velocity Twist at 10hz self.null_behavior = Behavior(self.process_command, 'zero', freq=10) self.behaviors = {'zero': self.null_behavior} self.active_behavior_name = 'zero' self.set_active_behavior('zero') self.tf = TransformListener() self.ddynrec = DDynamicReconfigure("example_dyn_rec") # Transformers are functions capable of processing incoming data in a variety of formats. # They are functions that take input of whatever type the topic is, and produce a transformers.Command # object. alt_pid = transformers.PIDAltController(self.tf, self.ddynrec) pos_pid = transformers.PIDPosController(self.tf, self.ddynrec, alt_pid) print pos_pid.cmd_pos self.transformers = { 'cmd_vel': (Twist, transformers.cmd_vel), 'cmd_takeoff': (Empty, transformers.cmd_takeoff), 'cmd_land': (Empty, transformers.cmd_land), 'cmd_pos': (PoseStamped, pos_pid.cmd_pos), 'cmd_vel_alt': (VelAlt, alt_pid.cmd_vel_alt) } """:type : dict[str, (str, (Any) -> transformers.Command)]""" # Subscribe to the behaviors passed as ROS parameters starting_behaviors = rospy.get_param('~behaviors', []) for b in starting_behaviors: behavior = Behavior(self.process_command, b, freq=20) behavior.subscribe(self.transformers) self.behaviors[b] = behavior # Secondary behaviors are filters that are always active on the Command before it is published. # Examples include last-minute obstacle avoidance, speed limiters, or arena boundary constraints. self.secondaries = [] self.vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=0) self.takeoff_pub = rospy.Publisher('/takeoff', Empty, queue_size=0) self.land_pub = rospy.Publisher('/land', Empty, queue_size=0) self.debug_pub = rospy.Publisher('/arbiter/debug', String, queue_size=10) self.active_pub = rospy.Publisher('/arbiter/active_behavior', String, queue_size=10) self.ddynrec.add_variable( "midbrain_freq", "Frequency of the main control loop for PIDs", rospy.get_param('~midbrain_freq', 100), 1, 1000) self.start_ddynrec() self.null_behavior.handle_message('cmd_vel', Twist()) rospy.sleep(0.5) rospy.Subscriber('/arbiter/register', RegisterBehavior, self.handle_register) rospy.Subscriber('/arbiter/activate_behavior', String, self.handle_activate) def start_ddynrec(self): """ Helper function to start the ddynamic reconfigure server with a callback function that updates the self.ddynrec attribute. """ def callback(config, level): """ A callback function used to as the parameter in the ddynrec.start() function. This custom callback function updates the state of self.ddynrec so we can refer to its variables whenever we have access to it. """ rospy.loginfo("Received reconf call: " + str(config)) # Update all variables var_names = self.ddynrec.get_variable_names() for var_name in var_names: self.ddynrec.__dict__[var_name] = config[var_name] return config self.ddynrec.start(callback) def handle_activate(self, msg): """ ROS subscriber for the activate_behavior topic :type msg: String """ self.set_active_behavior(msg.data) def set_active_behavior(self, name): """ Sets the active behavior, if the provided name is one of the known behaviors. :type name: str """ if name not in self.behaviors: # TODO: consider automatically registering new behaviors rospy.logerr('{} does not exist as a behavior!'.format(name)) self.set_active_behavior('zero') if name != self.active_behavior_name: self.active_behavior_name = name # Stop the vehicle self.process_command(name, 'cmd_vel', Twist()) rospy.loginfo_throttle(1.0, '{} selected as active behavior'.format(name)) def handle_register(self, req): """ ROS subscriber for adding a new behavior to the system. It is recommended to publish a single latched message to this behavior when a behavior node starts. :type req: RegisterBehavior """ if req.name in self.behaviors: rospy.logwarn("Behavior {} already exists".format(req.name)) old = self.behaviors[req.name] old.stop() if not req.name: rospy.logerr("Behavior cannot be created with empty name") return behavior = Behavior(self.process_command, req.name, freq=self.ddynrec.midbrain_freq if req.fast else 0) behavior.subscribe(self.transformers) self.behaviors[req.name] = behavior rospy.loginfo("Created behavior {}".format(behavior)) def process_command(self, behavior, topic, raw_cmd): """ process_command gets called after a message gets received from the currently active behavior. :param str behavior: The name of the behavior initiating the command :param str topic: The topic (without namespace) to which the command was sent :type raw_cmd: ROS message :return: success """ if behavior != self.active_behavior_name: # Only messages from the active behavior are handled return False _, transformer = self.transformers[topic] # Convert to a transformers.Command cmd = transformer(raw_cmd) # type: transformers.Command # Apply secondary behaviors for func in self.secondaries: cmd = func(cmd) # Publish the result to the ROS network if cmd.takeoff: self.takeoff_pub.publish(Empty()) vel = Twist() vel.linear.z = 0.5 self.vel_pub.publish(vel) elif cmd.land: self.land_pub.publish(Empty()) vel = Twist() vel.linear.z = -0.5 self.vel_pub.publish(vel) else: self.vel_pub.publish(cmd.vel) # Log it self.active_pub.publish(String(behavior)) rospy.loginfo_throttle( 1, "Command published by {}/{}".format(behavior, topic)) return True def publish_debug(self): """ Publishes debug information to the ROS network :return: None """ self.debug_pub.publish(String(str(self.behaviors))) def run(self): """ Main method, publishes debug information and :return: """ r = rospy.Rate(20) while not rospy.is_shutdown(): self.publish_debug() try: r.sleep() except rospy.ROSTimeMovedBackwardsException: r = rospy.Rate(20)
class SignalGenerator(object): def __init__(self): self.t0 = rospy.Time.now() self.timer = None self.config = None self.dr_client = None self.new_server = False self.pub = None self.ddr = DDynamicReconfigure("") self.ddr.add_variable("server", "dynamic reconfigure server", "") self.ddr.add_variable("param", "dr parameter name", "") self.ddr.add_variable("topic", "topic name", "signal") # The dynamic reconfigure probably won't go lower than 0.1, # 0.01 only works for topics self.ddr.add_variable("period", "update period", 0.05, 0.01, 10.0) self.num_freqs = 3 for i in range(self.num_freqs): si = str(i) fr = float(i) / float(self.num_freqs) self.ddr.add_variable("freq" + si, "frequency " + si, 1.0 - fr * fr, 0.0, 10.0) self.ddr.add_variable("amp" + si, "amplitude " + si, fr, 0.0, 2.0) self.ddr.add_variable("phase" + si, "phase " + si, 0.0, -math.pi, math.pi) self.ddr.add_variable("offset" + si, "offset " + si, 0.0, -5.0, 5.0) # TODO(lucasw) sine, sawtooth, ramp, square self.ddr.start(self.config_callback) def is_changed(self, config, param): if self.config is None: return True return config[param] != self.config[param] def config_callback(self, config, level): if self.is_changed(config, 'period'): if self.timer: self.timer.shutdown() self.timer = rospy.Timer(rospy.Duration(config.period), self.update) if self.is_changed(config, 'server'): self.new_server = True if self.is_changed(config, 'topic'): self.pub = rospy.Publisher(config.topic, Float32, queue_size=2) self.config = config return config def server_dr_callback(self, config): rospy.logdebug(config) def safe_update_config(self, values): if self.dr_client is None: return False update_timeout = 1.0 # TODO(lucasw) could follow # https://stackoverflow.com/questions/2829329/catch-a-threads-exception-in-the-caller-thread-in-python # and pass a message back if the update configuration fails try: th1 = threading.Thread(target=self.dr_client.update_configuration, args=[values]) th1.start() t1 = rospy.Time.now() while ((rospy.Time.now() - t1).to_sec() < update_timeout): if th1.isAlive(): rospy.sleep(0.05) else: break if th1.isAlive(): # TODO(lucasw) how to kill t1- or does it matter? raise RuntimeError("timeout") except RuntimeError as ex: # self.dr_client = None rospy.logerr(ex + " " + str(self.config.server) + " " + self.config.param) return False return True def connect_server(self): if self.config.server == '': self.dr_client = None self.new_server = False return True try: self.dr_client = Client(self.config.server, timeout=0.05, config_callback=self.server_dr_callback) self.new_server = False rospy.loginfo("connected to new server '" + self.config.server + "'") except Exception as ex: rospy.logerr_throttle( 5.0, "no server available '" + self.config.server + "'") return False return True def update(self, event): if self.config is None: return dt = (event.current_real - self.t0).to_sec() val = 0.0 for i in range(self.num_freqs): si = str(i) # TODO(lucasw) want to smoothly transition when # frequencies change, to do that need to offset phase in a way # that puts the old frequency and the new one at the same value. theta = 2.0 * math.pi * self.config[ "freq" + si] * dt + self.config["phase" + si] val += self.config["amp" + si] * math.sin(theta) + self.config[ "offset" + si] if self.pub is not None: self.pub.publish(Float32(val)) if self.new_server: self.connect_server() self.safe_update_config({self.config.param: val})
def setup_ddrec_server(): params_file = c["param_path"] def dyn_rec_callback(config, level): param_names = [ # Core Ball "H_Low_Core", "H_High_Core", "S_Low_Core", "S_High_Core", "V_Low_Core", "V_High_Core", "H_Low_Core", # Edge Ball "H_Low_Edge", "H_High_Edge", "S_Low_Edge", "S_High_Edge", "V_Low_Edge", "V_High_Edge", # Angle "angle_offset" ] try: if get_dy("SaveToJson") == 1: new_params = {param: get_dy(param) for param in param_names} with open(params_file, 'w') as f: json.dump(new_params, f) rospy.loginfo("Parameters saved to json") except KeyError: rospy.loginfo("Param not found, is parameter server running?") return config dd = DDynamicReconfigure("cv_dyn_rec") with open(params_file, 'r') as f: params = json.load(f) # In Range for central ball dd.add_variable("H_Low_Core", "Lower bound hue for central ball", params["H_Low_Core"], 0, 180) dd.add_variable("H_High_Core", "Higher bound hue for central ball", params["H_High_Core"], 0, 180) dd.add_variable("S_Low_Core", "Lower bound saturation for central ball", params["S_Low_Core"], 0, 255) dd.add_variable("S_High_Core", "Higher bound saturation for central ball", params["S_High_Core"], 0, 255) dd.add_variable("V_Low_Core", "Lower bound value for central ball", params["V_Low_Core"], 0, 255) dd.add_variable("V_High_Core", "Higher bound value for central ball", params["V_High_Core"], 0, 255) # In Range for edge ball dd.add_variable("H_Low_Edge", "Lower bound hue for edge ball", params["H_Low_Edge"], 0, 180) dd.add_variable("H_High_Edge", "Higher bound hue for edge ball", params["H_High_Edge"], 0, 180) dd.add_variable("S_Low_Edge", "Lower bound saturation for edge ball", params["S_Low_Edge"], 0, 255) dd.add_variable("S_High_Edge", "Higher bound saturation for edge ball", params["S_High_Edge"], 0, 255) dd.add_variable("V_Low_Edge", "Lower bound value for edge ball", params["V_Low_Edge"], 0, 255) dd.add_variable("V_High_Edge", "Higher bound value for edge ball", params["V_High_Edge"], 0, 255) # Angle offset dd.add_variable("angle_offset", "Offset [rad] to added to angle such that hanging " "pendulum yields an angle of zero", params["angle_offset"], -3.142, 3.142) dd.add_variable("SaveToJson", "Evoke callback", 0, 0, 1) dd.start(dyn_rec_callback) return dd
class Example(object): def __init__(self): # Create a D(ynamic)DynamicReconfigure self.ddr = DDynamicReconfigure("class_example") # Add variables (name, description, default value, min, max, edit_method) self.ddr.add_variable("decimal", "float/double variable", 0.0, -1.0, 1.0) self.ddr.add_variable("integer", "integer variable", 0, -1, 1) self.ddr.add_variable("bool", "bool variable", True) self.ddr.add_variable("string", "string variable", "string dynamic variable") enum_method = self.ddr.enum([ self.ddr.const("Small", "int", 0, "A small constant"), self.ddr.const("Medium", "int", 1, "A medium constant"), self.ddr.const("Large", "int", 2, "A large constant"), self.ddr.const("ExtraLarge", "int", 3, "An extra large constant") ], "An enum example") self.ddr.add_variable("enumerate", "enumerate variable", 1, 0, 3, edit_method=enum_method) self.add_variables_to_self() self.ddr.start(self.dyn_rec_callback) def add_variables_to_self(self): var_names = self.ddr.get_variable_names() for var_name in var_names: self.__setattr__(var_name, None) def dyn_rec_callback(self, config, level): rospy.loginfo("Received reconf call: " + str(config)) # Update all variables var_names = self.ddr.get_variable_names() for var_name in var_names: self.__dict__[var_name] = config[var_name] return config
class AreaDetector(object): """ Obstacle detector, capable of checking if there is an object in a specified area """ def __init__(self, name, x_offset, y_offset, x_size, y_size): # self.n_measurements_consider_valid = 4 self.pub_avoidance_area = rospy.Publisher("/avoidance_area_" + name, PolygonStamped, queue_size=1, latch=True) self.attention_wide = [x_offset, y_offset] #[1.5, 10.0] self.attention_center = [x_size, y_size, 0.0] #[-48.9, 120.0, 0.0] self.attention_area = DDynamicReconfigure("avoidance_aerea" + name) self.attention_area.add_variable('x_offset', 'x_offset', self.attention_center[0], min=self.attention_center[0] - 25.0, max=self.attention_center[0] + 25.0) self.attention_area.add_variable('y_offset', 'y_offset', self.attention_center[1], min=self.attention_center[1] - 25.0, max=self.attention_center[1] + 25.0) self.attention_area.add_variable('x_size', 'x_size', self.attention_wide[0], min=0.0, max=self.attention_wide[0] + 20.0) self.attention_area.add_variable('y_size', 'y_size', self.attention_wide[1], min=0.0, max=self.attention_wide[1] + 5.0) self.publish_attention_area() self.attention_area.start(self.attention_changes_cb) self.points_found = None def attention_changes_cb(self, config, level): self.attention_center[0] = config['x_offset'] self.attention_center[1] = config['y_offset'] self.attention_wide[0] = config['x_size'] self.attention_wide[1] = config['y_size'] self.publish_attention_area() return config def publish_attention_area(self): msg = PolygonStamped() msg.header.frame_id = "/map" p1 = Point() p1.x = self.attention_center[0] + self.attention_wide[0] p1.y = self.attention_center[1] + self.attention_wide[1] p1.z = self.attention_center[2] msg.polygon.points.append(p1) p2 = Point() p2.x = self.attention_center[0] + self.attention_wide[0] p2.y = self.attention_center[1] - self.attention_wide[1] p2.z = self.attention_center[2] msg.polygon.points.append(p2) p3 = Point() p3.x = self.attention_center[0] - self.attention_wide[0] p3.y = self.attention_center[1] - self.attention_wide[1] p3.z = self.attention_center[2] msg.polygon.points.append(p3) p4 = Point() p4.x = self.attention_center[0] - self.attention_wide[0] p4.y = self.attention_center[1] + self.attention_wide[1] p4.z = self.attention_center[2] msg.polygon.points.append(p4) self.pub_avoidance_area.publish(msg) return def check_point(self, x, y, z): # Checks if a point is on the area px = x - self.attention_center[0] py = y - self.attention_center[1] if (-self.attention_wide[0] < px and px < self.attention_wide[0]): if (-self.attention_wide[1] < py and py < self.attention_wide[1]): if self.points_found == None: self.points_found = [] # The point is inside the box self.points_found.append([x, y, z]) return def get_filtered_position(self): # Centeres the position of the measurements if self.points_found == None: return px = 0 py = 0 pz = 0 for p in self.points_found: px += p[0] py += p[1] pz += p[2] px /= len(self.points_found) py /= len(self.points_found) pz /= len(self.points_found) return [px, py, pz] def get_n_points(self): if self.points_found == None: return 0 return len(self.points_found)
#!/usr/bin/env python import rospy from ddynamic_reconfigure_python.ddynamic_reconfigure import DDynamicReconfigure def callback(config, level): #rospy.loginfo("Received reconf call: " + str(config)) return config if __name__ == '__main__': rospy.loginfo("Marathon Parameters - Running") rospy.init_node('marathon_params') # Create a D(ynamic)DynamicReconfigure player = DDynamicReconfigure("player") # Add variables (name, description, default value, min, max, edit_method) # ddynrec.add_variable("decimal", "float/double variable", 0.0, -1.0, 1.0) player.add_variable("Pan_KP", "Pan KP", 0.0, 0, 1) player.add_variable("Pan_KI", "Pan KI", 0.0, 0, 1) player.add_variable("Pan_KD", "Pan KD", 0.0, 0, 0.000010) player.add_variable("Tilt_KP", "Tilt KP", 0.0, 0, 1) player.add_variable("Tilt_KI", "Tilt KI", 0.0, 0, 1) player.add_variable("Tilt_KD", "Tilt KD", 0.0, 0, 0.000010) player.add_variable("Pan_Step", "Pan Step", 0.0, 0, 0.2) player.add_variable("Tilt_Step", "Tilt Step", 0.0, 0, 0.5) player.add_variable("Tilt_Angle", "Tilt Angle", 0.0, -2, 0) player.add_variable("Scan_Rate", "Scan Rate", 0, 0, 10) player.add_variable("Body_KP", "Body KP", 0.0, 0, 1)
# exit() print services for service_name in services: # service_name = ''.join(["/", service_name]) index = service_name.rfind("/") param_ns = service_name[:index] param_name = service_name[index + 1:] # param_name = service_name.replace("/", "___") if param_ns not in ddyns: ddyns[param_ns] = DDynamicReconfigure(param_ns) ddynrec = ddyns[param_ns] ddynrec.add_variable(param_name, service_name, 0.0, 0.0, 10.0) # fs = {} for param_ns in ddyns: def _dyn_rec_callback(config, level, _param_ns=param_ns): return dyn_rec_callback(config, level, _param_ns) # fs[param_ns] = _dyn_rec_callback ddynrec = ddyns[param_ns]
class GripperGrasper(object): def __init__(self): rospy.loginfo("Initializing GripperGrasper...") # This node Dynamic params self.ddr = DDynamicReconfigure("grasper") self.max_position_error = self.ddr.add_variable("max_position_error", "Max absolute value of controller state of any joint to stop closing", 0.002, 0.00001, 0.045) self.timeout = self.ddr.add_variable("timeout", "timeout for the closing action", 5.0, 0.0, 30.0) self.closing_time = self.ddr.add_variable("closing_time", "Time for the closing goal", 2.0, 0.01, 30.0) self.rate = self.ddr.add_variable("rate", "Rate Hz at which the node closing will do stuff", 5, 1, 50) self.ddr.start(self.ddr_cb) rospy.loginfo("Initialized dynamic reconfigure on: " + str(rospy.get_name())) # Subscriber to the gripper state self.last_state = None self.state_sub = rospy.Subscriber('/gripper_controller/state', JointTrajectoryControllerState, self.state_cb, queue_size=1) rospy.loginfo("Subscribed to topic: " + str(self.state_sub.resolved_name)) # Publisher on the gripper command topic self.cmd_pub = rospy.Publisher('/gripper_controller/command', JointTrajectory, queue_size=1) rospy.loginfo("Publishing to topic: " + str(self.cmd_pub.resolved_name)) # Grasping service to offer self.grasp_srv = rospy.Service('/gripper_controller/grasp', Empty, self.grasp_cb) rospy.loginfo("Offering grasp service on: " + str(self.grasp_srv.resolved_name)) rospy.loginfo("Done initializing GripperGrasper!") def ddr_cb(self, config, level): self.max_position_error = config['max_position_error'] self.timeout = config['timeout'] self.closing_time = config['closing_time'] self.rate = config['rate'] return config def state_cb(self, data): self.last_state = data def grasp_cb(self, req): rospy.logdebug("Received grasp request!") # From wherever we are close gripper # Keep closing until the error of the state reaches # max_position_error on any of the gripper joints # or we reach timeout initial_time = rospy.Time.now() closing_amount = [0.0, 0.0] # Initial command, wait for it to do something self.send_close(closing_amount) rospy.sleep(self.closing_time) r = rospy.Rate(self.rate) on_optimal_close = False while not rospy.is_shutdown() and (rospy.Time.now() - initial_time) < rospy.Duration(self.timeout) and not on_optimal_close: if -self.last_state.error.positions[0] > self.max_position_error: rospy.logdebug("Over error joint 0...") closing_amount = self.get_optimal_close() on_optimal_close = True elif -self.last_state.error.positions[1] > self.max_position_error: rospy.logdebug("Over error joint 1...") closing_amount = self.get_optimal_close() on_optimal_close = True self.send_close(closing_amount) r.sleep() return EmptyResponse() def get_optimal_close(self): optimal_0 = self.last_state.actual.positions[0] - self.max_position_error optimal_1 = self.last_state.actual.positions[1] - self.max_position_error return [optimal_0, optimal_1] def send_close(self, closing_amount): rospy.loginfo("Closing: " + str(closing_amount)) jt = JointTrajectory() jt.joint_names = ['gripper_right_finger_joint', 'gripper_left_finger_joint'] p = JointTrajectoryPoint() p.positions = closing_amount p.time_from_start = rospy.Duration(self.closing_time) jt.points.append(p) self.cmd_pub.publish(jt)
class ClickFollower(object): def __init__(self): self.config = DDynamicReconfigure() self.config.add_variable("rate", "Frequency to republish updates, in hertz", rospy.get_param('~rate', 10.0), 0.2, 200.0) self.config.add_variable("height", "Height to fly above base frame", rospy.get_param('~height', 2.0), 0.0, 5.0) self.pub = rospy.Publisher('cmd_pos', PoseStamped, queue_size=10) self.sub = rospy.Subscriber('/move_base_simple/goal', PoseStamped, self.onmsg) self.last_msg = PoseStamped() self.has_msg = False self.start_ddynrec() def onmsg(self, msg): """ :type msg: PoseStamped """ self.last_msg = msg self.has_msg = True msg.pose.position.z = self.config.height msg.header.stamp = rospy.Time(0) self.pub.publish(msg) def run(self): last_rate = None r = rospy.Rate( 1) # This is dynamically overridden to be the rate dynamic config while not rospy.is_shutdown(): if self.config.rate != last_rate: last_rate = self.config.rate r = rospy.Rate(last_rate) print "Rate changed to {}".format(last_rate) if self.has_msg: # Republish the last received message self.onmsg(self.last_msg) r.sleep() def start_ddynrec(self): """ Helper function to start the ddynamic reconfigure server with a callback function that updates the self.ddynrec attribute. """ def callback(config, level): """ A callback function used to as the parameter in the ddynrec.start() function. This custom callback function updates the state of self.ddynrec so we can refer to its variables whenever we have access to it. """ rospy.loginfo("Received reconfigure request: " + str(config)) # Update all variables var_names = self.config.get_variable_names() for var_name in var_names: self.config.__dict__[var_name] = config[var_name] return config self.config.start(callback)
class RestClient(object): def __init__(self, rest_name, ignored_parameters=[]): self.rest_name = rest_name self.ignored_parameters = ignored_parameters self.rest_services = [] self.ddr = None rospy.init_node(rest_name + '_client', log_level=rospy.INFO) self.host = rospy.get_param('~host', '') if not self.host: rospy.logerr('host is not set') sys.exit(1) self._setup_ddr() def _get_rest_parameters(self): try: url = 'http://{}/api/v1/nodes/{}/parameters'.format( self.host, self.rest_name) res = requests_retry_session().get(url) if res.status_code != 200: rospy.logerr("Getting parameters failed with status code: %d", res.status_code) return [] return res.json() except Exception as e: rospy.logerr(str(e)) return [] def _set_rest_parameters(self, parameters): try: url = 'http://{}/api/v1/nodes/{}/parameters'.format( self.host, self.rest_name) res = requests_retry_session().put(url, json=parameters) j = res.json() rospy.logdebug("set parameters response: %s", json.dumps(j, indent=2)) if 'return_code' in j and j['return_code']['value'] != 0: rospy.logwarn("Setting parameter failed: %s", j['return_code']['message']) return [] if res.status_code != 200: rospy.logerr("Setting parameters failed with status code: %d", res.status_code) return [] return j except Exception as e: rospy.logerr(str(e)) return [] def _setup_ddr(self): self.ddr = DDynamicReconfigure(rospy.get_name()) rest_params = [ p for p in self._get_rest_parameters() if p['name'] not in self.ignored_parameters ] def enum_method_from_param(p): if p['type'] != 'string': return "" enum_matches = re.findall(r'.*\[(?P<enum>.+)\].*', p['description']) if not enum_matches: return "" enum_names = [str(e.strip()) for e in enum_matches[0].split(',')] enum_list = [self.ddr.const(n, 'str', n, n) for n in enum_names] return self.ddr.enum(enum_list, p['name'] + '_enum') for p in rest_params: level = 0 edit_method = enum_method_from_param(p) if p['type'] == 'int32': self.ddr.add(p['name'], 'int', level, p['description'], p['default'], p['min'], p['max']) elif p['type'] == 'float64': self.ddr.add(p['name'], 'double', level, p['description'], p['default'], p['min'], p['max']) elif p['type'] == 'string': self.ddr.add(p['name'], 'str', level, p['description'], str(p['default']), edit_method=edit_method) elif p['type'] == 'bool': self.ddr.add(p['name'], 'bool', level, p['description'], p['default']) else: rospy.logwarn("Unsupported parameter type: %s", p['type']) self.ddr.start(self._dyn_rec_callback) def _dyn_rec_callback(self, config, level): rospy.logdebug("Received reconf call: " + str(config)) new_rest_params = [{ 'name': n, 'value': config[n] } for n in self.ddr.get_variable_names() if n in config] if new_rest_params: returned_params = self._set_rest_parameters(new_rest_params) for p in returned_params: if p['name'] not in config: rospy.logerr("param %s not in config", p['name']) continue config[p['name']] = p['value'] return config def call_rest_service(self, name, srv_type=None, request=None): try: args = {} if request is not None: # convert ROS request to JSON (with custom API mappings) args = convert_ros_message_to_dictionary(request) rospy.logdebug('calling {} with args: {}'.format(name, args)) url = 'http://{}/api/v1/nodes/{}/services/{}'.format( self.host, self.rest_name, name) res = requests_retry_session().put(url, json={'args': args}) j = res.json() rospy.logdebug("{} rest response: {}".format( name, json.dumps(j, indent=2))) rc = j['response'].get('return_code') if rc is not None and rc['value'] < 0: rospy.logwarn("service {} returned an error: [{}] {}".format( name, rc['value'], rc['message'])) # convert to ROS response if srv_type is not None: response = convert_dictionary_to_ros_message( srv_type._response_class(), j['response'], strict_mode=False) else: response = j['response'] except Exception as e: rospy.logerr(str(e)) if srv_type is not None: response = srv_type._response_class() if hasattr(response, 'return_code'): response.return_code.value = -1000 response.return_code.message = str(e) return response def add_rest_service(self, srv_type, srv_name, callback): """create a service and inject the REST-API service name""" srv = rospy.Service(rospy.get_name() + "/" + srv_name, srv_type, partial(callback, srv_name, srv_type)) self.rest_services.append(srv)
#!/usr/bin/env python import rospy from ddynamic_reconfigure_python.ddynamic_reconfigure import DDynamicReconfigure def callback(config, level): #rospy.loginfo("Received reconf call: " + str(config)) return config if __name__ == '__main__': rospy.loginfo("Sprint Parameters - Running") rospy.init_node('sprint_params') # Create a D(ynamic)DynamicReconfigure player = DDynamicReconfigure("player") # Add variables (name, description, default value, min, max, edit_method) # ddynrec.add_variable("decimal", "float/double variable", 0.0, -1.0, 1.0) player.add_variable("Pan_KP", "Pan KP", 0.0, 0, 1) player.add_variable("Pan_KI", "Pan KI", 0.0, 0, 1) player.add_variable("Pan_KD", "Pan KD", 0.0, 0, 0.000010) player.add_variable("Tilt_KP", "Tilt KP", 0.0, 0, 1) player.add_variable("Tilt_KI", "Tilt KI", 0.0, 0, 1) player.add_variable("Tilt_KD", "Tilt KD", 0.0, 0, 0.000010) player.add_variable("Pan_Step", "Pan Step", 0.0, 0, 0.2) player.add_variable("Tilt_Step", "Tilt Step", 0.0, 0, 0.5) player.add_variable("Scan_Rate", "Scan Rate", 0, 0, 10) player.add_variable("Body_Forward_KP", "Body Forward KP", 0.0, 0, 1) player.add_variable("Body_Backward_KP", "Body Backward KP", 0.0, 0, 1)
class GripperGraspService(object): def __init__(self): rospy.loginfo("Initializing Gripper Grasper...") # Get the params from param server self.last_state = None self.controller_name = rospy.get_param("~controller_name", None) if not self.controller_name: rospy.logerr("No controller name found in param: ~controller_name") exit(1) self.real_joint_names = rospy.get_param("~real_joint_names", None) if not self.real_joint_names: rospy.logerr( "No real joint names given in param: ~real_joint_names") exit(1) self.close_configuration = rospy.get_param("~close_configuration", None) if not self.close_configuration: rospy.logerr( "No close configuration values given in param: ~close_configuration" ) exit(1) if len(self.close_configuration) != len(self.real_joint_names): rospy.logerr( "The close configuration values defined in param: ~close_configuration" ", should be of same length as that of joints in param: ~real_joint_names" ) exit(1) # This node Dynamic params self.ddr = DDynamicReconfigure(self.controller_name + "_grasp_service") self.max_position_error = self.ddr.add_variable( "max_position_error", "Max absolute value of controller " "state of any joint to stop closing", 0.002, 0.00001, 0.045) self.timeout = self.ddr.add_variable("timeout", "timeout for the closing action", 5.0, 0.0, 30.0) self.closing_time = self.ddr.add_variable("closing_time", "Time for the closing goal", 2.0, 0.01, 30.0) self.rate = self.ddr.add_variable( "rate", "Rate Hz at which the node closing will do stuff", 5, 1, 50) self.ddr.start(self.ddr_cb) rospy.loginfo("Initialized dynamic reconfigure on: " + str(rospy.get_name())) # Subscriber to the gripper state self.state_sub = rospy.Subscriber('/' + self.controller_name + '_controller/state', JointTrajectoryControllerState, self.state_cb, queue_size=1) rospy.loginfo("Subscribed to topic: " + str(self.state_sub.resolved_name)) # Publisher on the gripper command topic self.cmd_pub = rospy.Publisher('/' + self.controller_name + '_controller/command', JointTrajectory, queue_size=1) rospy.loginfo("Publishing to topic: " + str(self.cmd_pub.resolved_name)) # Grasping service to offer self.grasp_srv = rospy.Service( '/' + self.controller_name + '_controller/grasp', Empty, self.grasp_cb) rospy.loginfo("Offering grasp service on: " + str(self.grasp_srv.resolved_name)) rospy.loginfo("Done initializing Gripper Grasp Service!") def ddr_cb(self, config, level): self.max_position_error = config['max_position_error'] self.timeout = config['timeout'] self.closing_time = config['closing_time'] self.rate = config['rate'] return config def state_cb(self, data): self.last_state = data def grasp_cb(self, req): rospy.logdebug("Received grasp request!") # From wherever we are close gripper # Keep closing until the error of the state reaches # max_position_error on any of the gripper joints # or we reach timeout initial_time = rospy.Time.now() closing_amount = self.close_configuration # Initial command, wait for it to do something self.send_close(closing_amount) rospy.sleep(self.closing_time) r = rospy.Rate(self.rate) on_optimal_close = False while not rospy.is_shutdown() and \ (rospy.Time.now() - initial_time) < rospy.Duration(self.timeout) and \ not on_optimal_close: for index in range(len(self.last_state.error.positions)): if -self.last_state.error.positions[ index] > self.max_position_error: rospy.logdebug("Over error joint {}...".format(index)) closing_amount = self.get_optimal_close() on_optimal_close = True self.send_close(closing_amount) r.sleep() return EmptyResponse() def get_optimal_close(self): optimal_state = [] for pos in self.last_state.actual.positions: optimal_state.append(pos - self.max_position_error) return optimal_state def send_close(self, closing_amount): rospy.loginfo("Closing: " + str(closing_amount)) jt = JointTrajectory() jt.joint_names = self.real_joint_names p = JointTrajectoryPoint() p.positions = closing_amount p.time_from_start = rospy.Duration(self.closing_time) jt.points.append(p) self.cmd_pub.publish(jt)
class PedestrianDetector(object): """ Pedestrian detector, capable of checking if there is one person un a specific area """ def __init__(self): self.n_measurements_consider_valid = 4 # Publishes the area where the pedestrian pass is self.pub_pedestrian_area = rospy.Publisher("/pedestrian_area_reference", PolygonStamped, queue_size=1, latch=True) self.pedestrian_center = [42.3,78.9, -0.48] self.pedestrian_wide = [6.5,4.0] self.publish_pedestrian_area() self.pedestrian_area = DDynamicReconfigure("pedestrian_area") self.pedestrian_area.add_variable('x_pose','x_pose', self.pedestrian_center[0], min= self.pedestrian_center[0] - 1, max = self.pedestrian_center[0] + 1) self.pedestrian_area.add_variable('y_pose','y_pose', self.pedestrian_center[1], min= self.pedestrian_center[1] - 1, max = self.pedestrian_center[1] + 1) self.pedestrian_area.add_variable('x_size','x_size', self.pedestrian_wide[0], min= self.pedestrian_wide[0] - 1, max = self.pedestrian_wide[0] + 1) self.pedestrian_area.add_variable('y_size','y_size', self.pedestrian_wide[1], min= self.pedestrian_wide[1] - 1, max = self.pedestrian_wide[1] + 1) self.pub_pedestrian_area_attention = rospy.Publisher("/pedestrian_area_attention", PolygonStamped, queue_size=1, latch=True) self.pedestrian_recog_area_c = [self.pedestrian_center[0] + self.pedestrian_wide[0]/2.0, self.pedestrian_center[1] + self.pedestrian_wide[1]/2.0] self.pedestrian_recog_area_wide_x = [-2.4, 3.3] self.pedestrian_recog_area_wide_y = self.pedestrian_wide[1]/2 + 0.05 self.publish_pedestrian_area_attention() # Uses as a reference the direction on which the car comes self.pedestrian_area.add_variable('x_pedest_neg','x_pedestrian_neg', self.pedestrian_recog_area_wide_x[0], min= -5.0, max = 0.0) self.pedestrian_area.add_variable('x_pedest_pos','x_pedestrian_pos', self.pedestrian_recog_area_wide_x[1], min= 0.0, max = 5.0) self.pedestrian_area.start(self.pedestrian_changes_cb) self.pointcloud_sub = rospy.Subscriber('/velodyne_downsampled_xyz', PointCloud2, self.velodyne_cb, queue_size=1) self.pub_person_found = rospy.Publisher("/pedestrian_detection", String, queue_size=1) self.pub_person_found_area = rospy.Publisher("/pedestrian_area_detection", Marker, queue_size=1, latch=True) self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) def publish_pedestrian_area(self): msg = PolygonStamped() msg.header.frame_id = "/map" p1 = Point() p1.x = self.pedestrian_center[0] p1.y = self.pedestrian_center[1] p1.z = self.pedestrian_center[2] msg.polygon.points.append(p1) p2 = Point() p2.x = self.pedestrian_center[0] + self.pedestrian_wide[0] p2.y = self.pedestrian_center[1] p2.z = self.pedestrian_center[2] msg.polygon.points.append(p2) p3 = Point() p3.x = self.pedestrian_center[0] + self.pedestrian_wide[0] p3.y = self.pedestrian_center[1] + self.pedestrian_wide[1] p3.z = self.pedestrian_center[2] msg.polygon.points.append(p3) p4 = Point() p4.x = self.pedestrian_center[0] p4.y = self.pedestrian_center[1] + self.pedestrian_wide[1] p4.z = self.pedestrian_center[2] msg.polygon.points.append(p4) self.pub_pedestrian_area.publish(msg) return def publish_pedestrian_area_attention(self): msg = PolygonStamped() msg.header.frame_id = "/map" p1 = Point() # Farer away point to th2 factory p1.x = self.pedestrian_recog_area_c[0] + self.pedestrian_recog_area_wide_x[0] p1.y = self.pedestrian_recog_area_c[1] + self.pedestrian_recog_area_wide_y p1.z = self.pedestrian_center[2] msg.polygon.points.append(p1) p2 = Point() p2.x = self.pedestrian_recog_area_c[0] + self.pedestrian_recog_area_wide_x[0] p2.y = self.pedestrian_recog_area_c[1] - self.pedestrian_recog_area_wide_y p2.z = self.pedestrian_center[2] msg.polygon.points.append(p2) p3 = Point() # Closest point to the factory p3.x = self.pedestrian_recog_area_c[0] + self.pedestrian_recog_area_wide_x[1] p3.y = self.pedestrian_recog_area_c[1] - self.pedestrian_recog_area_wide_y p3.z = self.pedestrian_center[2] msg.polygon.points.append(p3) p4 = Point() # The point that is at the left of the car p4.x = self.pedestrian_recog_area_c[0] + self.pedestrian_recog_area_wide_x[1] p4.y = self.pedestrian_recog_area_c[1] + self.pedestrian_recog_area_wide_y p4.z = self.pedestrian_center[2] msg.polygon.points.append(p4) self.pub_pedestrian_area_attention.publish(msg) return def pedestrian_changes_cb(self, config, level): self.pedestrian_center[0] = config['x_pose'] self.pedestrian_center[1] = config['y_pose'] self.pedestrian_wide[0] = config['x_size'] self.pedestrian_wide[1] = config['y_size'] self.pedestrian_recog_area_wide_x[0] = config['x_pedest_neg'] self.pedestrian_recog_area_wide_x[1] = config['x_pedest_pos'] self.publish_pedestrian_area() self.publish_pedestrian_area_attention() return config def velodyne_cb(self, pointcloud): # Transforming velodyne to map transform = self.tf_buffer.lookup_transform("map", pointcloud.header.frame_id, pointcloud.header.stamp, rospy.Duration(3.0)) pointcloud = do_transform_cloud(pointcloud, transform) #pointcloud_map cloud_points = list(read_points( pointcloud, skip_nans=True, field_names=("x", "y", "z"))) person_found = [] for p in cloud_points: px = p[0] - self.pedestrian_recog_area_c[0] py = p[1] - self.pedestrian_recog_area_c[1] if (abs(py) < self.pedestrian_recog_area_wide_y): # Taking points at the right of the pedestrian if (self.pedestrian_recog_area_wide_x[0] < px and px < self.pedestrian_recog_area_wide_x[1]): person_found.append([p[0], p[1], p[2]]) if len(person_found) > self.n_measurements_consider_valid: self.publish_person_found('CROSSING') self.publish_person_found_area(person_found) return self.publish_person_found('CLEAR') return def publish_person_found(self, status): rospy.loginfo('Publishing: ' + status) self.pub_person_found.publish(status) def publish_person_found_area(self, position): # Publishes a circle around the area where the person is placed msg = PolygonStamped() msg.header.frame_id = "/map" px = 0 py = 0 pz = 0 for person in position: px += person[0] py += person[1] pz += person[2] px /= len(position) py /= len(position) pz /= len(position) r = 0.4 marker = Marker( type=Marker.CUBE, id=0, lifetime=rospy.Duration(0.3), pose=Pose(Point(px,py,pz), Quaternion(0, 0, 0, 1)), scale=Vector3(r,r,2.0), header=Header(frame_id='map'), color=ColorRGBA(0.0, 1.0, 0.0, 0.8), text="text") self.pub_person_found_area.publish(marker) # p.append(Point()) # p4.x = self.pedestrian_center[0] # p4.y = self.pedestrian_center[1] + self.pedestrian_wide[1] # p4.z = self.pedestrian_center[2] # msg.polygon.points.append(p4) def run(self): rate = rospy.Rate(10) # Detect at 10hz while not rospy.is_shutdown(): rate.sleep()