Python std_msgs.msg.Float32() Examples
The following are 30
code examples of std_msgs.msg.Float32().
You can vote up the ones you like or vote down the ones you don't like,
and go to the original project or source file by following the links above each example.
You may also want to check out all available functions/classes of the module
std_msgs.msg
, or try the search function
.
.
Example #1
| Source File: monitor.py From cpu_monitor with MIT License | 5 votes |
|
def publish(self):
self.cpu_publisher.publish(Float32(self.proc.cpu_percent()))
self.mem_publisher.publish(UInt64(self.proc.memory_info().rss))
Example #2
| Source File: twist_to_motors.py From Learning-Robotics-using-Python-Second-Edition with MIT License | 5 votes |
|
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
#############################################################
Example #3
| Source File: twist_to_motors.py From differential-drive with GNU General Public License v3.0 | 5 votes |
|
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
#############################################################
Example #4
| Source File: twist_to_motors.py From Learning-Robotics-using-Python-Second-Edition with MIT License | 5 votes |
|
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
#############################################################
Example #5
| Source File: twist_to_motors.py From ROS-Programming-Building-Powerful-Robots with MIT License | 5 votes |
|
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
#############################################################
Example #6
| Source File: vars.py From relaxed_ik with MIT License | 5 votes |
|
def __populate_publishers(self):
for o in self.objectives:
self.objective_publishers.append(rospy.Publisher('/objective/' + o.name(), Float32, queue_size=5))
for c in self.constraints:
self.constraint_publishers.append(rospy.Publisher('/constraint/' + c.name(), Float32, queue_size=5))
for wf in self.weight_funcs:
self.weight_func_publishers.append(rospy.Publisher('/weight_function/' + wf.name(), Float32, queue_size=5))
Example #7
| Source File: monitor.py From cpu_monitor with MIT License | 5 votes |
|
def __init__(self, name, pid):
self.name = name
self.proc = psutil.Process(pid)
self.cpu_publisher = rospy.Publisher(ns_join("cpu_monitor", name[1:], "cpu"), Float32, queue_size=20)
self.mem_publisher = rospy.Publisher(ns_join("cpu_monitor", name[1:], "mem"), UInt64, queue_size=20)
Example #8
| Source File: __init__.py From ravestate with BSD 3-Clause "New" or "Revised" License | 5 votes |
|
def move_head(ctx: rs.ContextWrapper):
for axis, lower, upper in [(prop_head_axis0, ctx.conf(key=AXIS0_LOWER_LIMIT_KEY), ctx.conf(key=AXIS0_UPPER_LIMIT_KEY)),
(prop_head_axis1, ctx.conf(key=AXIS1_LOWER_LIMIT_KEY), ctx.conf(key=AXIS1_UPPER_LIMIT_KEY)),
(prop_head_axis2, ctx.conf(key=AXIS2_LOWER_LIMIT_KEY), ctx.conf(key=AXIS2_UPPER_LIMIT_KEY))]:
data = random.uniform(lower, upper)
if random.random() < ctx.conf(key=HEAD_MOVEMENT_PROBABILITY_KEY): # move or don't move axis with probability
logger.info(f"Publishing {data} to {axis.topic}")
ctx[axis] = Float32(data=data)
Example #9
| Source File: ndt_autoware_operator.py From pylot with Apache License 2.0 | 5 votes |
|
def run(self):
rospy.init_node(self.config.name, anonymous=True, disable_signals=True)
rospy.Subscriber(self._topic_name, PoseStamped, self.on_pose_update)
rospy.Subscriber('/estimated_vel_mps', Float32,
self.on_velocity_update)
rospy.spin()
Example #10
| Source File: mpc_cmd_pub.py From genesis_path_follower with MIT License | 5 votes |
|
def start_mpc_node():
rospy.init_node("dbw_mpc_pf")
acc_pub = rospy.Publisher("/control/accel", Float32Msg, queue_size=2)
steer_pub = rospy.Publisher("/control/steer_angle", Float32Msg, queue_size=2)
acc_enable_pub = rospy.Publisher("/control/enable_accel", UInt8Msg, queue_size=2, latch=True)
steer_enable_pub = rospy.Publisher("/control/enable_spas", UInt8Msg, queue_size=2, latch=True)
mpc_path_pub = rospy.Publisher("mpc_path", mpc_path, queue_size=2)
sub_state = rospy.Subscriber("state_est", state_est, state_est_callback, queue_size=2)
# Start up Ipopt/Solver.
for i in range(3):
kmpc.solve_model()
acc_enable_pub.publish(UInt8Msg(2))
steer_enable_pub.publish(UInt8Msg(1))
pub_loop(acc_pub, steer_pub, mpc_path_pub)
Example #11
| Source File: twist_to_motors.py From ROS-Programming-Building-Powerful-Robots with MIT License | 5 votes |
|
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
#############################################################
Example #12
| Source File: vehicle_simulator.py From genesis_path_follower with MIT License | 5 votes |
|
def __init__(self):
rospy.init_node('vehicle_simulator', anonymous=True)
rospy.Subscriber('/control/accel', float_msg, self._acc_cmd_callback, queue_size=1)
rospy.Subscriber('/control/steer_angle', float_msg, self._df_cmd_callback, queue_size=1)
self.state_pub = rospy.Publisher('state_est', state_est, queue_size=1)
self.tcmd_a = None # rostime (s) of received acc command
self.tcmd_d = None # rostime (s) of received df command
self.acc = 0.0 # actual acceleration (m/s^2)
self.df = 0.0 # actual steering angle (rad)
self.acc_des = 0.0 # desired acceleration (m/s^2)
self.df_des = 0.0 # desired steering_angle (rad)
self.dt_model = 0.01 # vehicle model update period (s) and frequency (Hz)
self.hz = int(1.0/self.dt_model)
self.r = rospy.Rate(self.hz)
# Simulated Vehicle State.
self.X = rospy.get_param('X0', -300.0) # X position (m)
self.Y = rospy.get_param('Y0', -450.0) # Y position (m)
self.psi = rospy.get_param('Psi0', 1.0) # yaw angle (rad)
self.vx = rospy.get_param('V0', 0.0) # longitudinal velocity (m/s)
self.vy = 0.0 # lateral velocity (m/s)
self.wz = 0.0 # yaw rate (rad/s)
self.acc_time_constant = 0.4 # s
self.df_time_constant = 0.1 # s
self.pub_loop()
Example #13
| Source File: wheel_speed.py From plantbot with MIT License | 5 votes |
|
def speed_converter():
rospy.init_node('wheel_speed', anonymous=True)
pub1 = rospy.Publisher('lwheel_vtarget', Float32, queue_size=10)
pub2 = rospy.Publisher('rwheel_vtarget', Float32, queue_size=10)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
rospy.Subscriber('cmd_vel', Twist, callback)
s1 = "The left wheel's target speed is %f m/s." % lv
s2 = "The right wheel's target speed is %f m/s." % rv
rospy.loginfo(s1)
rospy.loginfo(s2)
pub1.publish(lv)
pub2.publish(rv)
rate.sleep()
Example #14
| Source File: pid_velocity_sim.py From ROS-Programming-Building-Powerful-Robots with MIT License | 4 votes |
|
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)
#####################################################
Example #15
| Source File: vars.py From relaxed_ik with MIT License | 4 votes |
|
def __init__(self, name, objective_function, init_state, objectives, weight_funcs, weight_priors, constraints=(), bounds=(), unconstrained=False):
self.name = name
self.objective_function = objective_function
self.init_state = init_state
self.objectives = objectives
self.weight_funcs = weight_funcs
self.weight_priors = weight_priors
self.constraints = constraints
self.bounds = bounds
self.vel_objectives_on = True
self.objective_publishers = []
self.constraint_publishers = []
self.weight_func_publishers = []
self.f_obj_publisher = rospy.Publisher(self.name + '_f_obj', Float32, queue_size=5)
self.__populate_publishers()
self.xopt = init_state
self.prev_state = init_state
self.prev_state2 = init_state
self.prev_state3 = init_state
self.f_obj = 0.0
self.unconstrained = unconstrained
self.all_states = []
Example #16
| Source File: pid_velocity.py From Learning-Robotics-using-Python-Second-Edition with MIT License | 4 votes |
|
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)
#####################################################
Example #17
| Source File: pid_control.py From plantbot with MIT License | 4 votes |
|
def __init__(self):
# Initialize the node
rospy.init_node('pid_control', anonymous=True)
# Set varibles
self.l_enc = 0.0
self.l_enc_prev = 0.0
self.lwheel_number = 0
self.lwheel_dis = 0.0
self.r_enc = 0.0
self.r_enc_prev = 0.0
self.rwheel_number = 0
self.rwheel_dis = 0.0
self.l_then = rospy.Time.now()
self.lwheel_dis_prev = 0.0
self.r_then = rospy.Time.now()
self.rwheel_dis_prev = 0.0
self.l_error_int = 0.0
self.l_pid_duration_prev = rospy.Time.now()
self.lwheel_vel = 0.0
self.l_error_int = 0.0
self.l_target = 0.0
self.r_pid_duration_prev = rospy.Time.now()
self.rwheel_vel = 0.0
self.r_error_int = 0.0
self.r_target = 0.0
# Set parameters
self.kp = rospy.get_param('Kp',500)
self.ki = rospy.get_param('ki',200)
self.kd = rospy.get_param('kd',10)
self.rate = rospy.get_param('rate', 10)
self.ticks_per_meter = rospy.get_param('ticks_per_meter', 4685) #4685
self.encoder_min = rospy.get_param('encoder_min', -4294967296)
self.encoder_max = rospy.get_param('encoder_max', +4294967296)
self.encoder_low_wrap = rospy.get_param('encoder_low_wrap', 0.2*(self.encoder_max-self.encoder_min)+self.encoder_min)
self.encoder_high_wrap = rospy.get_param('encode_high_wrap', 0.8*(self.encoder_max-self.encoder_max)+self.encoder_max)
self.control_max = rospy.get_param('out_max', 255)
self.control_min = rospy.get_param('out_min', -255)
# Subscrption and publishment message
rospy.Subscriber('lwheel_vtarget', Float32, self.l_pid_func)
rospy.Subscriber('rwheel_vtarget', Float32, self.r_pid_func)
rospy.Subscriber('lwheel', Int64, self.l_distance)
rospy.Subscriber('rwheel', Int64, self.r_distance)
rospy.Subscriber('str', String, self.stop)
self.l_speed_pub = rospy.Publisher('lwheel_vel', Float32, queue_size=10)
self.r_speed_pub = rospy.Publisher('rwheel_vel', Float32, queue_size=10)
self.l_control_pub = rospy.Publisher('left_wheel_control', Float32, queue_size=10)
self.r_control_pub = rospy.Publisher('right_wheel_control', Float32, queue_size=10)
# Calculate the odom accroding to the encoder data from launchpad_node
Example #18
| Source File: sec_control.py From plantbot with MIT License | 4 votes |
|
def __init__(self):
rospy.init_node('sec_control', anonymous=True)
# Set varibles
self.l_enc = 0.0
self.l_enc_prev = 0.0
self.lwheel_number = 0
self.lwheel_dis = 0.0
self.r_enc = 0.0
self.r_enc_prev = 0.0
self.rwheel_number = 0
self.rwheel_dis = 0.0
self.l_then = rospy.Time.now()
self.lwheel_dis_prev = 0.0
self.r_then = rospy.Time.now()
self.rwheel_dis_prev = 0.0
self.l_error_int = 0.0
self.l_pid_duration_prev = rospy.Time.now()
self.lwheel_vel = 0.0
self.r_error_int = 0.0
self.r_pid_duration_prev = rospy.Time.now()
self.rwheel_vel = 0.0
self.r_error_int = 0.0
self.lwheel_control = 0
self.rwheel_control = 0
self.l_number = 0
self.r_number = 0
self.l_flag = 1
self.r_flag = 1
self.left = (59.42, 61.05, 63.05, 65.78, 67.75, 69.16, 70.31, 71.63, 73.13, 74.15, 75.22, 76.50, 79.19, 80.00, 80.73, 82.18, 83.63, 84.82, 86.19, 87.90, 88.92, 89.05, 89.86, 91.31, 92.38, 93.83, 94.94, 95.58, 96.52, 97.89, 96.82, 99.04, 99.59, 100.49, 102.28, 103.27, 104.72, 106.13, 106.04, 107.07, 108.22, 108.52, 108.30, 110.61, 110.27, 110.27, 111.38, 111.85, 113.98, 114.24, 112.96, 112.70, 113.55, 116.37, 117.57, 118.76, 117.14, 117.74, 118.89, 120.38, 121.32, 122.09, 122.90, 123.16, 124.31, 125.04, 125.55, 126.02, 125.55, 125.34, 126.40, 126.92, 128.41, 128.11, 128.41, 129.52, 129.69, 130.16, 130.42, 131.31, 131.48, 132.51, 134.00, 133.40, 134.60, 134.77, 135.75, 136.05, 135.80, 136.39, 136.22, 136.31, 136.56, 136.99, 138.06, 138.36, 139.08, 139.09, 140.06, 141.13, 141.55, 141.62, 141.88, 141.94, 141.52, 141.13, 142.37, 143.56, 142.97, 142.97, 143.78, 144.12, 144.97, 145.57, 145.78, 145.66, 145.87, 146.21, 146.30, 147.07, 147.28, 147.58, 147.53, 148.09, 148.35, 148.73, 148.39, 148.86, 149.46, 149.28, 150.01, 150.22, 150.35, 151.12, 151.59, 151.63, 152.06, 152.15, 152.74, 152.36, 152.66, 152.79, 153.51, 153.77, 154.11, 153.68, 154.19, 154.71, 155.60, 155.60, 155.77, 155.99, 156.20, 156.41, 156.67, 156.41, 156.50, 156.54, 156.20, 156.97, 157.10, 157.52, 157.61, 157.87, 158.34, 158.93, 159.02, 159.74, 159.74, 159.96, 160.34, 160.43, 160.51, 160.60, 160.98, 160.94, 160.30, 160.64, 160.94, 161.41, 162.69, 164.31, 165.12)
#the left motor doesn't rotate steadily until pwm = 72, and corresponding speed is 59.42mm/s.
self.right = (59.89, 69.07, 69.20, 69.71, 70.57, 73.85, 76.07, 78.25, 79.87, 80.81, 81.92, 83.37, 84.70, 85.68, 86.66, 89.61, 90.59, 92.42, 93.40, 94.47, 96.05, 98.48, 99.77, 101.30, 101.60, 102.97, 103.78, 104.50, 106.04, 107.62, 108.22, 107.32, 109.11, 110.74, 110.69, 111.63, 113.17, 112.96, 111.97, 112.74, 113.77, 115.05, 115.47, 116.37, 118.16, 118.46, 117.82, 118.72, 120.26, 120.55, 121.02, 120.90, 122.35, 122.52, 123.80, 124.06, 124.35, 125.21, 125.98, 127.39, 127.51, 127.17, 128.11, 128.62, 128.84, 129.73, 130.07, 130.16, 129.73, 130.80, 131.14, 131.87, 131.87, 132.21, 132.72, 133.66, 133.87, 135.11, 134.94, 135.24, 135.41, 136.44, 136.99, 136.39, 136.99, 136.99, 137.59, 137.59, 138.44, 138.14, 137.80, 138.19, 138.61, 139.08, 139.12, 139.72, 139.89, 140.02, 139.94, 140.36, 140.53, 141.17, 141.56, 141.81, 142.37, 142.92, 143.95, 144.72, 145.23, 145.36, 145.31, 145.83, 145.96, 146.30, 146.94, 147.71, 147.79, 148.26, 147.96, 148.18, 148.05, 148.86, 148.43, 148.64, 149.11, 149.46, 149.50, 149.97, 150.18, 150.65, 150.95, 150.95, 151.50, 151.63, 151.80, 151.97, 152.32, 152.53, 152.02, 151.72, 152.15, 152.19, 151.80, 151.80, 152.87, 153.60, 153.04, 153.90, 153.64, 153.90, 153.72, 153.72, 153.98, 154.49, 154.75, 154.15, 154.41, 151.55, 151.55, 153.68, 153.68, 155.09, 155.05, 155.22, 155.65, 156.07, 156.80, 156.46, 156.07, 156.63, 156.97, 157.31, 157.27, 157.01, 157.69, 157.95, 157.31, 157.31, 157.27, 157.10, 157.48, 157.10, 157.57, 158.46, 159.70, 160.51, 160.77)
#the right motor doesn't rotate steadily until pwm = 68, and corresponding speed is 58.89mm/s.
self.rate = rospy.get_param('rate', 10)
self.vel_threshold = rospy.get_param('~vel_threshold', 0.001)
self.ticks_per_meter = rospy.get_param('ticks_per_meter', 4685)
self.encoder_min = rospy.get_param('encoder_min', -4294967296)
self.encoder_max = rospy.get_param('encoder_max', +4294967296)
self.encoder_low_wrap = rospy.get_param('encoder_low_wrap', 0.2*(self.encoder_max-self.encoder_min)+self.encoder_min)
self.encoder_high_wrap = rospy.get_param('encode_high_wrap', 0.8*(self.encoder_max-self.encoder_max)+self.encoder_max)
rospy.Subscriber('lwheel_vtarget', Float32, self.l_sec_func, queue_size=1)
rospy.Subscriber('rwheel_vtarget', Float32, self.r_sec_func, queue_size=1)
rospy.Subscriber('lwheel', Int64, self.l_distance, queue_size=1)
rospy.Subscriber('rwheel', Int64, self.r_distance, queue_size=1)
rospy.Subscriber('str', String, self.stop)
self.l_speed_pub = rospy.Publisher('lwheel_vel', Float32, queue_size=10)
self.r_speed_pub = rospy.Publisher('rwheel_vel', Float32, queue_size=10)
self.l_control_pub = rospy.Publisher('left_wheel_control', Float32, queue_size=10)
self.r_control_pub = rospy.Publisher('right_wheel_control', Float32, queue_size=10)
Example #19
| Source File: pid_velocity.py From Learning-Robotics-using-Python-Second-Edition with MIT License | 4 votes |
|
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)
#####################################################
Example #20
| Source File: pid_velocity.py From ROS-Programming-Building-Powerful-Robots with MIT License | 4 votes |
|
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)
#####################################################
Example #21
| Source File: rpid_velocity.py From ROS-Programming-Building-Powerful-Robots with MIT License | 4 votes |
|
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)
#####################################################
Example #22
| Source File: pid_velocity.py From ROS-Programming-Building-Powerful-Robots with MIT License | 4 votes |
|
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)
#####################################################
Example #23
| Source File: pid_velocity_sim.py From ROS-Programming-Building-Powerful-Robots with MIT License | 4 votes |
|
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)
#####################################################
Example #24
| Source File: rpid_velocity.py From ROS-Programming-Building-Powerful-Robots with MIT License | 4 votes |
|
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)
#####################################################
Example #25
| Source File: lpid_velocity.py From ROS-Programming-Building-Powerful-Robots with MIT License | 4 votes |
|
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)
#####################################################
Example #26
| Source File: rpid_velocity.py From ROS-Programming-Building-Powerful-Robots with MIT License | 4 votes |
|
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)
#####################################################
Example #27
| Source File: lpid_velocity.py From ROS-Programming-Building-Powerful-Robots with MIT License | 4 votes |
|
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)
#####################################################
Example #28
| Source File: environment.py From Training-Neural-Networks-for-Event-Based-End-to-End-Robot-Control with GNU General Public License v3.0 | 4 votes |
|
def __init__(self):
self.dvs_sub = rospy.Subscriber('dvsData', Int8MultiArray, self.dvs_callback)
self.pos_sub = rospy.Subscriber('transformData', Transform, self.pos_callback)
self.left_pub = rospy.Publisher('leftMotorSpeed', Float32, queue_size=1)
self.right_pub = rospy.Publisher('rightMotorSpeed', Float32, queue_size=1)
self.reset_pub = rospy.Publisher('resetRobot', Bool, queue_size=None)
self.dvs_data = np.array([0,0])
self.pos_data = []
self.v_pre = v_pre
self.turn_pre = turn_pre
self.resize_factor = [dvs_resolution[0]//resolution[0], (dvs_resolution[1]-crop_bottom-crop_top)//resolution[1]]
self.outer = False
rospy.init_node('dvs_controller')
self.rate = rospy.Rate(rate)
#Some values for calculating distance to center of lane
self.v1 = 2.5
self.v2 = 7.5
self.scale = 1.0
self.c1 = np.array([self.scale*self.v1,self.scale*self.v1])
self.c2 = np.array([self.scale*self.v2,self.scale*self.v2])
self.c3 = np.array([self.scale*self.v2,self.scale*self.v1])
self.c4 = np.array([self.scale*self.v1,self.scale*self.v2])
self.r1_outer = self.scale*(self.v1-0.25)
self.r2_outer = self.scale*(self.v1+0.25)
self.l1_outer = self.scale*(self.v1+self.v2-0.25)
self.l2_outer = self.scale*(0.25)
self.r1_inner = self.scale*(self.v1-0.75)
self.r2_inner = self.scale*(self.v1+0.75)
self.l1_inner = self.scale*(self.v1+self.v2-0.75)
self.l2_inner = self.scale*(0.75)
self.d1_outer = 5.0
self.d2_outer = 2*math.pi*self.r1_outer*0.25
self.d3_outer = 5.0
self.d4_outer = 2*math.pi*self.r1_outer*0.5
self.d5_outer = 2*math.pi*self.r2_outer*0.25
self.d6_outer = 2*math.pi*self.r1_outer*0.5
self.d1_inner = 5.0
self.d2_inner = 2*math.pi*self.r1_inner*0.25
self.d3_inner = 5.0
self.d4_inner = 2*math.pi*self.r1_inner*0.5
self.d5_inner = 2*math.pi*self.r2_inner*0.25
self.d6_inner = 2*math.pi*self.r1_inner*0.5
self.d_outer = self.d1_outer + self.d2_outer + self.d3_outer + self.d4_outer + self.d5_outer + self.d6_outer
self.d_inner = self.d1_inner + self.d2_inner + self.d3_inner + self.d4_inner + self.d5_inner + self.d6_inner
Example #29
| Source File: environment.py From Training-Neural-Networks-for-Event-Based-End-to-End-Robot-Control with GNU General Public License v3.0 | 4 votes |
|
def __init__(self):
self.dvs_sub = rospy.Subscriber('dvsData', Int8MultiArray, self.dvs_callback)
self.pos_sub = rospy.Subscriber('transformData', Transform, self.pos_callback)
self.left_pub = rospy.Publisher('leftMotorSpeed', Float32, queue_size=1)
self.right_pub = rospy.Publisher('rightMotorSpeed', Float32, queue_size=1)
self.reset_pub = rospy.Publisher('resetRobot', Bool, queue_size=None)
self.dvs_data = np.array([0,0])
self.pos_data = []
self.distance = 0
self.steps = 0
self.v_pre = v_pre
self.turn_pre = turn_pre
self.resize_factor = [dvs_resolution[0]//resolution[0], (dvs_resolution[1]-crop_bottom-crop_top)//resolution[1]]
self.outer = False
rospy.init_node('dvs_controller')
self.rate = rospy.Rate(rate)
#Some values for calculating distance to center of lane
self.v1 = 2.5
self.v2 = 7.5
self.scale = 1.0
self.c1 = np.array([self.scale*self.v1,self.scale*self.v1])
self.c2 = np.array([self.scale*self.v2,self.scale*self.v2])
self.c3 = np.array([self.scale*self.v2,self.scale*self.v1])
self.c4 = np.array([self.scale*self.v1,self.scale*self.v2])
self.r1_outer = self.scale*(self.v1-0.25)
self.r2_outer = self.scale*(self.v1+0.25)
self.l1_outer = self.scale*(self.v1+self.v2-0.25)
self.l2_outer = self.scale*(0.25)
self.r1_inner = self.scale*(self.v1-0.75)
self.r2_inner = self.scale*(self.v1+0.75)
self.l1_inner = self.scale*(self.v1+self.v2-0.75)
self.l2_inner = self.scale*(0.75)
self.d1_outer = 5.0
self.d2_outer = 2*math.pi*self.r1_outer*0.25
self.d3_outer = 5.0
self.d4_outer = 2*math.pi*self.r1_outer*0.5
self.d5_outer = 2*math.pi*self.r2_outer*0.25
self.d6_outer = 2*math.pi*self.r1_outer*0.5
self.d1_inner = 5.0
self.d2_inner = 2*math.pi*self.r1_inner*0.25
self.d3_inner = 5.0
self.d4_inner = 2*math.pi*self.r1_inner*0.5
self.d5_inner = 2*math.pi*self.r2_inner*0.25
self.d6_inner = 2*math.pi*self.r1_inner*0.5
self.d_outer = self.d1_outer + self.d2_outer + self.d3_outer + self.d4_outer + self.d5_outer + self.d6_outer
self.d_inner = self.d1_inner + self.d2_inner + self.d3_inner + self.d4_inner + self.d5_inner + self.d6_inner
Example #30
| Source File: environment.py From Training-Neural-Networks-for-Event-Based-End-to-End-Robot-Control with GNU General Public License v3.0 | 4 votes |
|
def __init__(self,speed,turn,resolution,reset_distance, rate, dvs_queue, resize_factor, crop):
self.dvs_sub = rospy.Subscriber('dvsData', Int8MultiArray, self.dvs_callback)
self.left_pub = rospy.Publisher('leftMotorSpeed', Float32, queue_size=1)
self.right_pub = rospy.Publisher('rightMotorSpeed', Float32, queue_size=1)
self.reset_pub = rospy.Publisher('resetRobot', Bool, queue_size=None)
self.v_forward = speed
self.v_turn = turn
self.resolution = resolution
self.reset_distance = reset_distance
self.dvs_queue = dvs_queue
self.fifo = deque(np.zeros((dvs_queue, resolution[0]*resolution[1])))
self.resize_factor = resize_factor
self.crop = crop
self.outer = False
rospy.init_node('dvs_controller')
self.rate = rospy.Rate(rate)
#Some values for calculating distance to center of lane
self.v1 = 2.5
self.v2 = 7.5
self.scale = 1.0
self.c1 = np.array([self.scale*self.v1,self.scale*self.v1])
self.c2 = np.array([self.scale*self.v2,self.scale*self.v2])
self.c3 = np.array([self.scale*self.v2,self.scale*self.v1])
self.c4 = np.array([self.scale*self.v1,self.scale*self.v2])
self.r1_outer = self.scale*(self.v1-0.25)
self.r2_outer = self.scale*(self.v1+0.25)
self.l1_outer = self.scale*(self.v1+self.v2-0.25)
self.l2_outer = self.scale*(0.25)
self.r1_inner = self.scale*(self.v1-0.75)
self.r2_inner = self.scale*(self.v1+0.75)
self.l1_inner = self.scale*(self.v1+self.v2-0.75)
self.l2_inner = self.scale*(0.75)
self.d1_outer = 5.0
self.d2_outer = 2*math.pi*self.r1_outer*0.25
self.d3_outer = 5.0
self.d4_outer = 2*math.pi*self.r1_outer*0.5
self.d5_outer = 2*math.pi*self.r2_outer*0.25
self.d6_outer = 2*math.pi*self.r1_outer*0.5
self.d1_inner = 5.0
self.d2_inner = 2*math.pi*self.r1_inner*0.25
self.d3_inner = 5.0
self.d4_inner = 2*math.pi*self.r1_inner*0.5
self.d5_inner = 2*math.pi*self.r2_inner*0.25
self.d6_inner = 2*math.pi*self.r1_inner*0.5
self.d_outer = self.d1_outer + self.d2_outer + self.d3_outer + self.d4_outer + self.d5_outer + self.d6_outer
self.d_inner = self.d1_inner + self.d2_inner + self.d3_inner + self.d4_inner + self.d5_inner + self.d6_inner
