Python源码示例：rospy.get_name()
示例1
def __init__(self):
#############################################################
rospy.init_node("twist_to_motors")
nodename = rospy.get_name()
rospy.loginfo("%s started" % nodename)
self.w = rospy.get_param("~base_width", 0.2)
self.pub_lmotor = rospy.Publisher('lwheel_vtarget', Float32, queue_size=10)
self.pub_rmotor = rospy.Publisher('rwheel_vtarget', Float32, queue_size=10)
rospy.Subscriber('twist', Twist, self.twistCallback)
self.rate = rospy.get_param("~rate", 50)
self.timeout_ticks = rospy.get_param("~timeout_ticks", 2)
self.left = 0
self.right = 0
#############################################################
示例2
def __init__(self):
###############################################
rospy.init_node("wheel_loopback");
self.nodename = rospy.get_name()
rospy.loginfo("%s started" % self.nodename)
self.rate = rospy.get_param("~rate", 200)
self.timeout_secs = rospy.get_param("~timeout_secs", 0.5)
self.ticks_meter = float(rospy.get_param('~ticks_meter', 50))
self.velocity_scale = float(rospy.get_param('~velocity_scale', 255))
self.latest_motor = 0
self.wheel = 0
rospy.Subscriber('motor', Int16, self.motor_callback)
self.pub_wheel = rospy.Publisher('wheel', Int16, queue_size=10)
###############################################
示例3
def __init__(self):
self._ns = "/robot/gripper_action_server"
if demo_constants.is_real_robot():
self._gripper = PSGGripper() # intera_interface.Gripper()
else:
self._gripper = intera_interface.Gripper()
# Action Server
self._server = actionlib.SimpleActionServer(
self._ns,
GripperCommandAction,
execute_cb=self._on_gripper_action,
auto_start=False)
self._action_name = rospy.get_name()
self._server.start()
self._gripper.set_dead_zone("0.021")
# Action Feedback/Result
self.feedback_msg = GripperCommandFeedback()
self.result_msg = GripperCommandResult()
# Initialize Parameters
self._timeout = 5.0
示例4
def __init__(self):
#############################################################
rospy.init_node("twist_to_motors")
nodename = rospy.get_name()
rospy.loginfo("%s started" % nodename)
self.w = rospy.get_param("~base_width", 0.2)
self.pub_lmotor = rospy.Publisher('lwheel_vtarget', Float32,queue_size=10)
self.pub_rmotor = rospy.Publisher('rwheel_vtarget', Float32,queue_size=10)
rospy.Subscriber('cmd_vel_mux/input/teleop', Twist, self.twistCallback)
self.rate = rospy.get_param("~rate", 50)
self.timeout_ticks = rospy.get_param("~timeout_ticks", 2)
self.left = 0
self.right = 0
#############################################################
示例5
def __init__(self):
###############################################
rospy.init_node("wheel_loopback");
self.nodename = rospy.get_name()
rospy.loginfo("%s started" % self.nodename)
self.rate = rospy.get_param("~rate", 200)
self.timeout_secs = rospy.get_param("~timeout_secs", 0.5)
self.ticks_meter = float(rospy.get_param('~ticks_meter', 50))
self.velocity_scale = float(rospy.get_param('~velocity_scale', 255))
self.latest_motor = 0
self.wheel = 0
rospy.Subscriber('motor', Int16, self.motor_callback)
self.pub_wheel = rospy.Publisher('wheel', Int16,queue_size=10)
###############################################
示例6
def __init__(self):
#############################################################
rospy.init_node("twist_to_motors")
nodename = rospy.get_name()
rospy.loginfo("%s started" % nodename)
self.w = rospy.get_param("~base_width", 0.2)
self.pub_lmotor = rospy.Publisher('lwheel_vtarget', Float32,queue_size=10)
self.pub_rmotor = rospy.Publisher('rwheel_vtarget', Float32,queue_size=10)
rospy.Subscriber('cmd_vel_mux/input/teleop', Twist, self.twistCallback)
self.rate = rospy.get_param("~rate", 50)
self.timeout_ticks = rospy.get_param("~timeout_ticks", 2)
self.left = 0
self.right = 0
#############################################################
示例7
def __init__(self):
#############################################################
rospy.init_node("twist_to_motors")
nodename = rospy.get_name()
rospy.loginfo("%s started" % nodename)
self.w = rospy.get_param("~base_width", 0.2)
self.pub_lmotor = rospy.Publisher('lwheel_vtarget', Float32,queue_size=10)
self.pub_rmotor = rospy.Publisher('rwheel_vtarget', Float32,queue_size=10)
rospy.Subscriber('cmd_vel_mux/input/teleop', Twist, self.twistCallback)
self.rate = rospy.get_param("~rate", 50)
self.timeout_ticks = rospy.get_param("~timeout_ticks", 2)
self.left = 0
self.right = 0
#############################################################
示例8
def __init__(self):
###############################################
rospy.init_node("wheel_loopback");
self.nodename = rospy.get_name()
rospy.loginfo("%s started" % self.nodename)
self.rate = rospy.get_param("~rate", 200)
self.timeout_secs = rospy.get_param("~timeout_secs", 0.5)
self.ticks_meter = float(rospy.get_param('~ticks_meter', 50))
self.velocity_scale = float(rospy.get_param('~velocity_scale', 255))
self.latest_motor = 0
self.wheel = 0
rospy.Subscriber('motor', Int16, self.motor_callback)
self.pub_wheel = rospy.Publisher('wheel', Int16,queue_size=10)
###############################################
示例9
def __init__(self, name):
"""
:param name: The name of the node
"""
rospy.logdebug( "[" + rospy.get_name() + "]: " + "Starting " )
self._lib = ProbRepLib()
self.services = {}
for namespace, services in ServiceManager.available_services.items():
# Automatically creating a service for all the entries in 'qsrrep_lib.rep_io.available_services'
# Passing the key of the dict entry to the service to identify the right function to call
for i, service in enumerate(services):
# The outer lambda function creates a new scope for the inner lambda function
# This is necessary to preserve the value of k, otherwise it will have the same value for all services
# x will be substituded by the service request
rospy.logdebug( "[" + rospy.get_name() + "]: " + "Creating service: ["+namespace+"]["+service+"]" )
self.services[service] = rospy.Service("~"+namespace+"/"+service, QSRProbRep, (lambda a,b: lambda x: self.callback(x, a, b))(namespace,service))
rospy.logdebug( "[" + rospy.get_name() + "]: " + "Created" )
rospy.logdebug( "[" + rospy.get_name() + "]: " + "Done" )
示例10
def __init__(self):
#############################################################
rospy.init_node("twist_to_motors")
nodename = rospy.get_name()
rospy.loginfo("%s started" % nodename)
self.w = rospy.get_param("~base_width", 0.2)
self.pub_lmotor = rospy.Publisher('lwheel_vtarget', Float32,queue_size=10)
self.pub_rmotor = rospy.Publisher('rwheel_vtarget', Float32,queue_size=10)
rospy.Subscriber('cmd_vel', Twist, self.twistCallback)
self.rate = rospy.get_param("~rate", 50)
self.timeout_ticks = rospy.get_param("~timeout_ticks", 2)
self.left = 0
self.right = 0
#############################################################
示例11
def __init__(self):
#############################################################
rospy.init_node("twist_to_motors")
nodename = rospy.get_name()
rospy.loginfo("%s started" % nodename)
self.w = rospy.get_param("~base_width", 0.2)
self.pub_lmotor = rospy.Publisher('lwheel_vtarget', Float32,queue_size=10)
self.pub_rmotor = rospy.Publisher('rwheel_vtarget', Float32,queue_size=10)
rospy.Subscriber('cmd_vel', Twist, self.twistCallback)
self.rate = rospy.get_param("~rate", 50)
self.timeout_ticks = rospy.get_param("~timeout_ticks", 2)
self.left = 0
self.right = 0
#############################################################
示例12
def start(self):
"""Start the sensor.
"""
if rospy.get_name() == '/unnamed':
raise ValueError('PhoXi sensor must be run inside a ros node!')
# Connect to the cameras
if not self._connect_to_sensor():
self._running = False
return False
# Set up subscribers for camera data
self._color_im_sub = rospy.Subscriber('/phoxi_camera/texture', ImageMessage, self._color_im_callback)
self._depth_im_sub = rospy.Subscriber('/phoxi_camera/depth_map', ImageMessage, self._depth_im_callback)
self._normal_map_sub = rospy.Subscriber('/phoxi_camera/normal_map', ImageMessage, self._normal_map_callback)
self._running = True
return True
示例13
def __init__(self, service_node_name="qsr_lib"):
"""Constructor.
:param service_node_name: Node name of the service.
:type service_node_name: str
"""
self.service_topic_names = {"request": service_node_name+"/request"}
"""dict: Topic names of the services."""
rospy.logdebug( "[" + rospy.get_name() + "]: " + "Waiting for service '" + self.service_topic_names["request"] + "' to come up")
rospy.wait_for_service(self.service_topic_names["request"])
rospy.logdebug( "[" + rospy.get_name() + "]: " + "done")
示例14
def start(self):
"""Start the sensor.
"""
if rospy.get_name() == '/unnamed':
raise ValueError('Weight sensor must be run inside a ros node!')
self._weight_subscriber = rospy.Subscriber('weight_sensor/weights', Float32MultiArray, self._weights_callback)
self._running = True
示例15
def __init__(self):
#############################################################################
rospy.init_node("diff_tf")
self.nodename = rospy.get_name()
rospy.loginfo("-I- %s started" % self.nodename)
#### parameters #######
self.rate = rospy.get_param('~rate',10.0) # the rate at which to publish the transform
self.ticks_meter = float(rospy.get_param('ticks_meter', 50)) # The number of wheel encoder ticks per meter of travel
self.base_width = float(rospy.get_param('~base_width', 0.245)) # The wheel base width in meters
self.base_frame_id = rospy.get_param('~base_frame_id','base_link') # the name of the base frame of the robot
self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame
self.encoder_min = rospy.get_param('encoder_min', -32768)
self.encoder_max = rospy.get_param('encoder_max', 32768)
self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
self.t_delta = rospy.Duration(1.0/self.rate)
self.t_next = rospy.Time.now() + self.t_delta
# internal data
self.enc_left = None # wheel encoder readings
self.enc_right = None
self.left = 0 # actual values coming back from robot
self.right = 0
self.lmult = 0
self.rmult = 0
self.prev_lencoder = 0
self.prev_rencoder = 0
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
self.dx = 0 # speeds in x/rotation
self.dr = 0
self.then = rospy.Time.now()
# subscriptions
rospy.Subscriber("lwheel", Int16, self.lwheelCallback)
rospy.Subscriber("rwheel", Int16, self.rwheelCallback)
self.odomPub = rospy.Publisher("odom", Odometry, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
#############################################################################
示例16
def __init__(self):
#####################################################
rospy.init_node("pid_velocity")
self.nodename = rospy.get_name()
rospy.loginfo("%s started" % self.nodename)
### initialize variables
self.target = 0
self.motor = 0
self.vel = 0
self.integral = 0
self.error = 0
self.derivative = 0
self.previous_error = 0
self.wheel_prev = 0
self.wheel_latest = 0
self.then = rospy.Time.now()
self.wheel_mult = 0
self.prev_encoder = 0
### get parameters ####
self.Kp = rospy.get_param('~Kp',10)
self.Ki = rospy.get_param('~Ki',10)
self.Kd = rospy.get_param('~Kd',0.001)
self.out_min = rospy.get_param('~out_min',-255)
self.out_max = rospy.get_param('~out_max',255)
self.rate = rospy.get_param('~rate',30)
self.rolling_pts = rospy.get_param('~rolling_pts',2)
self.timeout_ticks = rospy.get_param('~timeout_ticks',4)
self.ticks_per_meter = rospy.get_param('ticks_meter', 20)
self.vel_threshold = rospy.get_param('~vel_threshold', 0.001)
self.encoder_min = rospy.get_param('encoder_min', -32768)
self.encoder_max = rospy.get_param('encoder_max', 32768)
self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
self.prev_vel = [0.0] * self.rolling_pts
self.wheel_latest = 0.0
self.prev_pid_time = rospy.Time.now()
rospy.logdebug("%s got Kp:%0.3f Ki:%0.3f Kd:%0.3f tpm:%0.3f" % (self.nodename, self.Kp, self.Ki, self.Kd, self.ticks_per_meter))
#### subscribers/publishers
rospy.Subscriber("wheel", Int16, self.wheelCallback)
rospy.Subscriber("wheel_vtarget", Float32, self.targetCallback)
self.pub_motor = rospy.Publisher('motor_cmd',Float32, queue_size=10)
self.pub_vel = rospy.Publisher('wheel_vel', Float32, queue_size=10)
#####################################################
示例17
def __init__(self):
#############################################################################
rospy.init_node("diff_tf")
self.nodename = rospy.get_name()
rospy.loginfo("-I- %s started" % self.nodename)
#### parameters #######
self.rate = rospy.get_param('~rate',10.0) # the rate at which to publish the transform
self.ticks_meter = float(rospy.get_param('ticks_meter', 50)) # The number of wheel encoder ticks per meter of travel
self.base_width = float(rospy.get_param('~base_width', 0.3)) # The wheel base width in meters
self.base_frame_id = rospy.get_param('~base_frame_id','base_footprint') # the name of the base frame of the robot
self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame
self.encoder_min = rospy.get_param('encoder_min', -2147483648)
self.encoder_max = rospy.get_param('encoder_max', 2147483648)
self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
self.t_delta = rospy.Duration(1.0/self.rate)
self.t_next = rospy.Time.now() + self.t_delta
# internal data
self.enc_left = None # wheel encoder readings
self.enc_right = None
self.left = 0 # actual values coming back from robot
self.right = 0
self.lmult = 0
self.rmult = 0
self.prev_lencoder = 0
self.prev_rencoder = 0
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
self.dx = 0 # speeds in x/rotation
self.dr = 0
self.then = rospy.Time.now()
# subscriptions
rospy.Subscriber("lwheel", Int64, self.lwheelCallback)
rospy.Subscriber("rwheel", Int64, self.rwheelCallback)
self.odomPub = rospy.Publisher("odom", Odometry,queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
#############################################################################
示例18
def __init__(self):
#####################################################
rospy.init_node("lpid_velocity")
self.nodename = rospy.get_name()
rospy.loginfo("%s started" % self.nodename)
### initialize variables
self.target = 0
self.motor = 0
self.vel = 0
self.integral = 0
self.error = 0
self.derivative = 0
self.previous_error = 0
self.wheel_prev = 0
self.wheel_latest = 0
self.then = rospy.Time.now()
self.wheel_mult = 0
self.prev_encoder = 0
### get parameters ####
self.Kp = rospy.get_param('~Kp',10)
self.Ki = rospy.get_param('~Ki',10)
self.Kd = rospy.get_param('~Kd',0.001)
self.out_min = rospy.get_param('~out_min',-255)
self.out_max = rospy.get_param('~out_max',255)
self.rate = rospy.get_param('~rate',30)
self.rolling_pts = rospy.get_param('~rolling_pts',2)
self.timeout_ticks = rospy.get_param('~timeout_ticks',4)
self.ticks_per_meter = rospy.get_param('ticks_meter', 20)
self.vel_threshold = rospy.get_param('~vel_threshold', 0.001)
self.encoder_min = rospy.get_param('encoder_min', -32768)
self.encoder_max = rospy.get_param('encoder_max', 32768)
self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
self.prev_vel = [0.0] * self.rolling_pts
self.wheel_latest = 0.0
self.prev_pid_time = rospy.Time.now()
rospy.logdebug("%s got Kp:%0.3f Ki:%0.3f Kd:%0.3f tpm:%0.3f" % (self.nodename, self.Kp, self.Ki, self.Kd, self.ticks_per_meter))
#### subscribers/publishers
rospy.Subscriber("wheel", Int16, self.wheelCallback)
rospy.Subscriber("wheel_vtarget", Float32, self.targetCallback)
self.pub_motor = rospy.Publisher('motor_cmd',Float32,queue_size=10)
self.pub_vel = rospy.Publisher('wheel_vel', Float32,queue_size=10)
#####################################################
示例19
def __init__(self):
#####################################################
rospy.init_node("rpid_velocity")
self.nodename = rospy.get_name()
rospy.loginfo("%s started" % self.nodename)
### initialize variables
self.target = 0
self.motor = 0
self.vel = 0
self.integral = 0
self.error = 0
self.derivative = 0
self.previous_error = 0
self.wheel_prev = 0
self.wheel_latest = 0
self.then = rospy.Time.now()
self.wheel_mult = 0
self.prev_encoder = 0
### get parameters ####
self.Kp = rospy.get_param('~Kp',10)
self.Ki = rospy.get_param('~Ki',10)
self.Kd = rospy.get_param('~Kd',0.001)
self.out_min = rospy.get_param('~out_min',-255)
self.out_max = rospy.get_param('~out_max',255)
self.rate = rospy.get_param('~rate',30)
self.rolling_pts = rospy.get_param('~rolling_pts',2)
self.timeout_ticks = rospy.get_param('~timeout_ticks',4)
self.ticks_per_meter = rospy.get_param('ticks_meter', 20)
self.vel_threshold = rospy.get_param('~vel_threshold', 0.001)
self.encoder_min = rospy.get_param('encoder_min', -32768)
self.encoder_max = rospy.get_param('encoder_max', 32768)
self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
self.prev_vel = [0.0] * self.rolling_pts
self.wheel_latest = 0.0
self.prev_pid_time = rospy.Time.now()
rospy.logdebug("%s got Kp:%0.3f Ki:%0.3f Kd:%0.3f tpm:%0.3f" % (self.nodename, self.Kp, self.Ki, self.Kd, self.ticks_per_meter))
#### subscribers/publishers
rospy.Subscriber("wheel", Int16, self.wheelCallback)
rospy.Subscriber("wheel_vtarget", Float32, self.targetCallback)
self.pub_motor = rospy.Publisher('motor_cmd',Float32,queue_size=10)
self.pub_vel = rospy.Publisher('wheel_vel', Float32,queue_size=10)
#####################################################
示例20
def __init__(self):
#####################################################
rospy.init_node("pid_velocity")
self.nodename = rospy.get_name()
rospy.loginfo("%s started" % self.nodename)
### initialize variables
self.target = 0
self.motor = 0
self.vel = 0
self.integral = 0
self.error = 0
self.derivative = 0
self.previous_error = 0
self.wheel_prev = 0
self.wheel_latest = 0
self.then = rospy.Time.now()
self.wheel_mult = 0
self.prev_encoder = 0
### get parameters ####
self.Kp = rospy.get_param('~Kp',10)
self.Ki = rospy.get_param('~Ki',10)
self.Kd = rospy.get_param('~Kd',0.001)
self.out_min = rospy.get_param('~out_min',-255)
self.out_max = rospy.get_param('~out_max',255)
self.rate = rospy.get_param('~rate',30)
self.rolling_pts = rospy.get_param('~rolling_pts',2)
self.timeout_ticks = rospy.get_param('~timeout_ticks',4)
self.ticks_per_meter = rospy.get_param('ticks_meter', 14860)
self.vel_threshold = rospy.get_param('~vel_threshold', 0.001)
self.encoder_min = rospy.get_param('encoder_min', -2147483648)
self.encoder_max = rospy.get_param('encoder_max', 2147483648)
self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
self.prev_vel = [0.0] * self.rolling_pts
self.wheel_latest = 0.0
self.prev_pid_time = rospy.Time.now()
rospy.logdebug("%s got Kp:%0.3f Ki:%0.3f Kd:%0.3f tpm:%0.3f" % (self.nodename, self.Kp, self.Ki, self.Kd, self.ticks_per_meter))
#### subscribers/publishers
rospy.Subscriber("wheel", Int64, self.wheelCallback)
rospy.Subscriber("wheel_vtarget", Float32, self.targetCallback)
self.pub_motor = rospy.Publisher('motor_cmd',Float32,queue_size=10)
self.pub_vel = rospy.Publisher('wheel_vel', Float32,queue_size=10)
#####################################################
示例21
def __init__(self):
#############################################################################
rospy.init_node("diff_tf")
self.nodename = rospy.get_name()
rospy.loginfo("-I- %s started" % self.nodename)
#### parameters #######
self.rate = rospy.get_param('~rate',10.0) # the rate at which to publish the transform
self.ticks_meter = float(rospy.get_param('ticks_meter', 50)) # The number of wheel encoder ticks per meter of travel
self.base_width = float(rospy.get_param('~base_width', 0.3)) # The wheel base width in meters
self.base_frame_id = rospy.get_param('~base_frame_id','base_footprint') # the name of the base frame of the robot
self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame
self.encoder_min = rospy.get_param('encoder_min', -2147483648)
self.encoder_max = rospy.get_param('encoder_max', 2147483648)
self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
self.t_delta = rospy.Duration(1.0/self.rate)
self.t_next = rospy.Time.now() + self.t_delta
# internal data
self.enc_left = None # wheel encoder readings
self.enc_right = None
self.left = 0 # actual values coming back from robot
self.right = 0
self.lmult = 0
self.rmult = 0
self.prev_lencoder = 0
self.prev_rencoder = 0
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
self.dx = 0 # speeds in x/rotation
self.dr = 0
self.then = rospy.Time.now()
# subscriptions
rospy.Subscriber("lwheel", Int64, self.lwheelCallback)
rospy.Subscriber("rwheel", Int64, self.rwheelCallback)
self.odomPub = rospy.Publisher("odom", Odometry,queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
#############################################################################
示例22
def __init__(self):
#####################################################
rospy.init_node("lpid_velocity")
self.nodename = rospy.get_name()
rospy.loginfo("%s started" % self.nodename)
### initialize variables
self.target = 0
self.motor = 0
self.vel = 0
self.integral = 0
self.error = 0
self.derivative = 0
self.previous_error = 0
self.wheel_prev = 0
self.wheel_latest = 0
self.then = rospy.Time.now()
self.wheel_mult = 0
self.prev_encoder = 0
### get parameters ####
self.Kp = rospy.get_param('~Kp',10)
self.Ki = rospy.get_param('~Ki',10)
self.Kd = rospy.get_param('~Kd',0.001)
self.out_min = rospy.get_param('~out_min',-255)
self.out_max = rospy.get_param('~out_max',255)
self.rate = rospy.get_param('~rate',30)
self.rolling_pts = rospy.get_param('~rolling_pts',2)
self.timeout_ticks = rospy.get_param('~timeout_ticks',4)
self.ticks_per_meter = rospy.get_param('ticks_meter', 20)
self.vel_threshold = rospy.get_param('~vel_threshold', 0.001)
self.encoder_min = rospy.get_param('encoder_min', -32768)
self.encoder_max = rospy.get_param('encoder_max', 32768)
self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
self.prev_vel = [0.0] * self.rolling_pts
self.wheel_latest = 0.0
self.prev_pid_time = rospy.Time.now()
rospy.logdebug("%s got Kp:%0.3f Ki:%0.3f Kd:%0.3f tpm:%0.3f" % (self.nodename, self.Kp, self.Ki, self.Kd, self.ticks_per_meter))
#### subscribers/publishers
rospy.Subscriber("wheel", Int16, self.wheelCallback)
rospy.Subscriber("wheel_vtarget", Float32, self.targetCallback)
self.pub_motor = rospy.Publisher('motor_cmd',Float32,queue_size=10)
self.pub_vel = rospy.Publisher('wheel_vel', Float32,queue_size=10)
#####################################################
示例23
def __init__(self):
#####################################################
rospy.init_node("rpid_velocity")
self.nodename = rospy.get_name()
rospy.loginfo("%s started" % self.nodename)
### initialize variables
self.target = 0
self.motor = 0
self.vel = 0
self.integral = 0
self.error = 0
self.derivative = 0
self.previous_error = 0
self.wheel_prev = 0
self.wheel_latest = 0
self.then = rospy.Time.now()
self.wheel_mult = 0
self.prev_encoder = 0
### get parameters ####
self.Kp = rospy.get_param('~Kp',10)
self.Ki = rospy.get_param('~Ki',10)
self.Kd = rospy.get_param('~Kd',0.001)
self.out_min = rospy.get_param('~out_min',-255)
self.out_max = rospy.get_param('~out_max',255)
self.rate = rospy.get_param('~rate',30)
self.rolling_pts = rospy.get_param('~rolling_pts',2)
self.timeout_ticks = rospy.get_param('~timeout_ticks',4)
self.ticks_per_meter = rospy.get_param('ticks_meter', 20)
self.vel_threshold = rospy.get_param('~vel_threshold', 0.001)
self.encoder_min = rospy.get_param('encoder_min', -32768)
self.encoder_max = rospy.get_param('encoder_max', 32768)
self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
self.prev_vel = [0.0] * self.rolling_pts
self.wheel_latest = 0.0
self.prev_pid_time = rospy.Time.now()
rospy.logdebug("%s got Kp:%0.3f Ki:%0.3f Kd:%0.3f tpm:%0.3f" % (self.nodename, self.Kp, self.Ki, self.Kd, self.ticks_per_meter))
#### subscribers/publishers
rospy.Subscriber("wheel", Int16, self.wheelCallback)
rospy.Subscriber("wheel_vtarget", Float32, self.targetCallback)
self.pub_motor = rospy.Publisher('motor_cmd',Float32,queue_size=10)
self.pub_vel = rospy.Publisher('wheel_vel', Float32,queue_size=10)
#####################################################
示例24
def __init__(self):
#####################################################
rospy.init_node("pid_velocity")
self.nodename = rospy.get_name()
rospy.loginfo("%s started" % self.nodename)
### initialize variables
self.target = 0
self.motor = 0
self.vel = 0
self.integral = 0
self.error = 0
self.derivative = 0
self.previous_error = 0
self.wheel_prev = 0
self.wheel_latest = 0
self.then = rospy.Time.now()
self.wheel_mult = 0
self.prev_encoder = 0
### get parameters ####
self.Kp = rospy.get_param('~Kp',10)
self.Ki = rospy.get_param('~Ki',10)
self.Kd = rospy.get_param('~Kd',0.001)
self.out_min = rospy.get_param('~out_min',-255)
self.out_max = rospy.get_param('~out_max',255)
self.rate = rospy.get_param('~rate',30)
self.rolling_pts = rospy.get_param('~rolling_pts',2)
self.timeout_ticks = rospy.get_param('~timeout_ticks',4)
self.ticks_per_meter = rospy.get_param('ticks_meter', 14860)
self.vel_threshold = rospy.get_param('~vel_threshold', 0.001)
self.encoder_min = rospy.get_param('encoder_min', -2147483648)
self.encoder_max = rospy.get_param('encoder_max', 2147483648)
self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
self.prev_vel = [0.0] * self.rolling_pts
self.wheel_latest = 0.0
self.prev_pid_time = rospy.Time.now()
rospy.logdebug("%s got Kp:%0.3f Ki:%0.3f Kd:%0.3f tpm:%0.3f" % (self.nodename, self.Kp, self.Ki, self.Kd, self.ticks_per_meter))
#### subscribers/publishers
rospy.Subscriber("wheel", Int64, self.wheelCallback)
rospy.Subscriber("wheel_vtarget", Float32, self.targetCallback)
self.pub_motor = rospy.Publisher('motor_cmd',Float32,queue_size=10)
self.pub_vel = rospy.Publisher('wheel_vel', Float32,queue_size=10)
#####################################################
示例25
def __init__(self):
#####################################################
rospy.init_node("pid_velocity")
self.nodename = rospy.get_name()
rospy.loginfo("%s started" % self.nodename)
### initialize variables
self.target = 0
self.motor = 0
self.vel = 0
self.integral = 0
self.error = 0
self.derivative = 0
self.previous_error = 0
self.wheel_prev = 0
self.wheel_latest = 0
self.then = rospy.Time.now()
self.wheel_mult = 0
self.prev_encoder = 0
### get parameters ####
self.Kp = rospy.get_param('~Kp',10)
self.Ki = rospy.get_param('~Ki',10)
self.Kd = rospy.get_param('~Kd',0.001)
self.out_min = rospy.get_param('~out_min',-255)
self.out_max = rospy.get_param('~out_max',255)
self.rate = rospy.get_param('~rate',30)
self.rolling_pts = rospy.get_param('~rolling_pts',2)
self.timeout_ticks = rospy.get_param('~timeout_ticks',4)
self.ticks_per_meter = rospy.get_param('ticks_meter', 14860)
self.vel_threshold = rospy.get_param('~vel_threshold', 0.001)
self.encoder_min = rospy.get_param('encoder_min', -2147483648)
self.encoder_max = rospy.get_param('encoder_max', 2147483648)
self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
self.prev_vel = [0.0] * self.rolling_pts
self.wheel_latest = 0.0
self.prev_pid_time = rospy.Time.now()
rospy.logdebug("%s got Kp:%0.3f Ki:%0.3f Kd:%0.3f tpm:%0.3f" % (self.nodename, self.Kp, self.Ki, self.Kd, self.ticks_per_meter))
#### subscribers/publishers
rospy.Subscriber("wheel", Int64, self.wheelCallback)
rospy.Subscriber("wheel_vtarget", Float32, self.targetCallback)
self.pub_motor = rospy.Publisher('motor_cmd',Float32,queue_size=10)
self.pub_vel = rospy.Publisher('wheel_vel', Float32,queue_size=10)
#####################################################
示例26
def __init__(self):
#############################################################################
rospy.init_node("diff_tf")
self.nodename = rospy.get_name()
rospy.loginfo("-I- %s started" % self.nodename)
#### parameters #######
self.rate = rospy.get_param('~rate',10.0) # the rate at which to publish the transform
self.ticks_meter = float(rospy.get_param('ticks_meter', 50)) # The number of wheel encoder ticks per meter of travel
self.base_width = float(rospy.get_param('~base_width', 0.3)) # The wheel base width in meters
self.base_frame_id = rospy.get_param('~base_frame_id','base_footprint') # the name of the base frame of the robot
self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame
self.encoder_min = rospy.get_param('encoder_min', -2147483648)
self.encoder_max = rospy.get_param('encoder_max', 2147483648)
self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
self.t_delta = rospy.Duration(1.0/self.rate)
self.t_next = rospy.Time.now() + self.t_delta
# internal data
self.enc_left = None # wheel encoder readings
self.enc_right = None
self.left = 0 # actual values coming back from robot
self.right = 0
self.lmult = 0
self.rmult = 0
self.prev_lencoder = 0
self.prev_rencoder = 0
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
self.dx = 0 # speeds in x/rotation
self.dr = 0
self.then = rospy.Time.now()
# subscriptions
rospy.Subscriber("lwheel", Int64, self.lwheelCallback)
rospy.Subscriber("rwheel", Int64, self.rwheelCallback)
self.odomPub = rospy.Publisher("odom", Odometry,queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
#############################################################################
示例27
def __init__(self):
#####################################################
rospy.init_node("lpid_velocity")
self.nodename = rospy.get_name()
rospy.loginfo("%s started" % self.nodename)
### initialize variables
self.target = 0
self.motor = 0
self.vel = 0
self.integral = 0
self.error = 0
self.derivative = 0
self.previous_error = 0
self.wheel_prev = 0
self.wheel_latest = 0
self.then = rospy.Time.now()
self.wheel_mult = 0
self.prev_encoder = 0
### get parameters ####
self.Kp = rospy.get_param('~Kp',10)
self.Ki = rospy.get_param('~Ki',10)
self.Kd = rospy.get_param('~Kd',0.001)
self.out_min = rospy.get_param('~out_min',-255)
self.out_max = rospy.get_param('~out_max',255)
self.rate = rospy.get_param('~rate',30)
self.rolling_pts = rospy.get_param('~rolling_pts',2)
self.timeout_ticks = rospy.get_param('~timeout_ticks',4)
self.ticks_per_meter = rospy.get_param('ticks_meter', 20)
self.vel_threshold = rospy.get_param('~vel_threshold', 0.001)
self.encoder_min = rospy.get_param('encoder_min', -32768)
self.encoder_max = rospy.get_param('encoder_max', 32768)
self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
self.prev_vel = [0.0] * self.rolling_pts
self.wheel_latest = 0.0
self.prev_pid_time = rospy.Time.now()
rospy.logdebug("%s got Kp:%0.3f Ki:%0.3f Kd:%0.3f tpm:%0.3f" % (self.nodename, self.Kp, self.Ki, self.Kd, self.ticks_per_meter))
#### subscribers/publishers
rospy.Subscriber("wheel", Int16, self.wheelCallback)
rospy.Subscriber("wheel_vtarget", Float32, self.targetCallback)
self.pub_motor = rospy.Publisher('motor_cmd',Float32,queue_size=10)
self.pub_vel = rospy.Publisher('wheel_vel', Float32,queue_size=10)
#####################################################
示例28
def __init__(self):
#####################################################
rospy.init_node("pid_velocity")
self.nodename = rospy.get_name()
rospy.loginfo("%s started" % self.nodename)
### initialize variables
self.target = 0
self.motor = 0
self.vel = 0
self.integral = 0
self.error = 0
self.derivative = 0
self.previous_error = 0
self.wheel_prev = 0
self.wheel_latest = 0
self.then = rospy.Time.now()
self.wheel_mult = 0
self.prev_encoder = 0
### get parameters ####
self.Kp = rospy.get_param('~Kp',10)
self.Ki = rospy.get_param('~Ki',10)
self.Kd = rospy.get_param('~Kd',0.001)
self.out_min = rospy.get_param('~out_min',-255)
self.out_max = rospy.get_param('~out_max',255)
self.rate = rospy.get_param('~rate',30)
self.rolling_pts = rospy.get_param('~rolling_pts',2)
self.timeout_ticks = rospy.get_param('~timeout_ticks',4)
self.ticks_per_meter = rospy.get_param('ticks_meter', 14860)
self.vel_threshold = rospy.get_param('~vel_threshold', 0.001)
self.encoder_min = rospy.get_param('encoder_min', -2147483648)
self.encoder_max = rospy.get_param('encoder_max', 2147483648)
self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
self.prev_vel = [0.0] * self.rolling_pts
self.wheel_latest = 0.0
self.prev_pid_time = rospy.Time.now()
rospy.logdebug("%s got Kp:%0.3f Ki:%0.3f Kd:%0.3f tpm:%0.3f" % (self.nodename, self.Kp, self.Ki, self.Kd, self.ticks_per_meter))
#### subscribers/publishers
rospy.Subscriber("wheel", Int64, self.wheelCallback)
rospy.Subscriber("wheel_vtarget", Float32, self.targetCallback)
self.pub_motor = rospy.Publisher('motor_cmd',Float32,queue_size=10)
self.pub_vel = rospy.Publisher('wheel_vel', Float32,queue_size=10)
#####################################################
示例29
def __init__(self):
#####################################################
rospy.init_node("pid_velocity")
self.nodename = rospy.get_name()
rospy.loginfo("%s started" % self.nodename)
### initialize variables
self.target = 0
self.motor = 0
self.vel = 0
self.integral = 0
self.error = 0
self.derivative = 0
self.previous_error = 0
self.wheel_prev = 0
self.wheel_latest = 0
self.then = rospy.Time.now()
self.wheel_mult = 0
self.prev_encoder = 0
### get parameters ####
self.Kp = rospy.get_param('~Kp',10)
self.Ki = rospy.get_param('~Ki',10)
self.Kd = rospy.get_param('~Kd',0.001)
self.out_min = rospy.get_param('~out_min',-255)
self.out_max = rospy.get_param('~out_max',255)
self.rate = rospy.get_param('~rate',30)
self.rolling_pts = rospy.get_param('~rolling_pts',2)
self.timeout_ticks = rospy.get_param('~timeout_ticks',4)
self.ticks_per_meter = rospy.get_param('ticks_meter', 14860)
self.vel_threshold = rospy.get_param('~vel_threshold', 0.001)
self.encoder_min = rospy.get_param('encoder_min', -2147483648)
self.encoder_max = rospy.get_param('encoder_max', 2147483648)
self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
self.prev_vel = [0.0] * self.rolling_pts
self.wheel_latest = 0.0
self.prev_pid_time = rospy.Time.now()
rospy.logdebug("%s got Kp:%0.3f Ki:%0.3f Kd:%0.3f tpm:%0.3f" % (self.nodename, self.Kp, self.Ki, self.Kd, self.ticks_per_meter))
#### subscribers/publishers
rospy.Subscriber("wheel", Int64, self.wheelCallback)
rospy.Subscriber("wheel_vtarget", Float32, self.targetCallback)
self.pub_motor = rospy.Publisher('motor_cmd',Float32,queue_size=10)
self.pub_vel = rospy.Publisher('wheel_vel', Float32,queue_size=10)
#####################################################
示例30
def __init__(self, limb, reconfig_server, rate=100.0,
mode='position_w_id', interpolation='minjerk'):
self._dyn = reconfig_server
self._ns = 'robot/limb/' + limb
self._fjt_ns = self._ns + '/follow_joint_trajectory'
self._server = actionlib.SimpleActionServer(
self._fjt_ns,
FollowJointTrajectoryAction,
execute_cb=self._on_trajectory_action,
auto_start=False)
self._action_name = rospy.get_name()
self._limb = intera_interface.Limb(limb, synchronous_pub=True)
self._enable = intera_interface.RobotEnable()
self._name = limb
self._interpolation = interpolation
self._cuff = intera_interface.Cuff(limb=limb)
# Verify joint control mode
self._mode = mode
if (self._mode != 'position' and self._mode != 'position_w_id'
and self._mode != 'velocity'):
rospy.logerr("%s: Action Server Creation Failed - "
"Provided Invalid Joint Control Mode '%s' (Options: "
"'position_w_id', 'position', 'velocity')" %
(self._action_name, self._mode,))
return
self._server.start()
self._alive = True
# Action Feedback/Result
self._fdbk = FollowJointTrajectoryFeedback()
self._result = FollowJointTrajectoryResult()
# Controller parameters from arguments, messages, and dynamic
# reconfigure
self._control_rate = rate # Hz
self._control_joints = []
self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()}
self._goal_time = 0.0
self._stopped_velocity = 0.0
self._goal_error = dict()
self._path_thresh = dict()
# Create our PID controllers
self._pid = dict()
for joint in self._limb.joint_names():
self._pid[joint] = intera_control.PID()
# Create our spline coefficients
self._coeff = [None] * len(self._limb.joint_names())
# Set joint state publishing to specified control rate
self._pub_rate = rospy.Publisher(
'/robot/joint_state_publish_rate',
UInt16,
queue_size=10)
self._pub_rate.publish(self._control_rate)
