def create_steer(self, val):
st = SteeringReport()
st.steering_wheel_angle_cmd = val * math.pi / 180. ### KINETIC
#st.steering_wheel_cmd = val * math.pi/180. ### MELODIC
st.enabled = True
st.speed = self.vel
return st
def create_steer(self, val):
st = SteeringReport()
#st.steering_wheel_angle_cmd = val * math.pi/180.
st.steering_wheel_angle = val * math.pi / 180.
st.enabled = True
st.speed = self.vel
return st
def simu():
global action, car
rospy.init_node('simu', anonymous=True)
rate = rospy.Rate(1 / dt)
rospy.loginfo("simulator node starts")
rospy.Subscriber('/vehicle/cmd_vel_stamped',
TwistStamped,
cmd_vel_stampedCallback,
queue_size=1)
rospy.Subscriber('/vehicle/steering_cmd',
SteeringCmd,
steering_cmdCallback,
queue_size=1)
pub1 = rospy.Publisher('/vehicle/steering_report',
SteeringReport,
queue_size=1)
pub2 = rospy.Publisher('state_estimate', state_Dynamic, queue_size=1)
steering_report = SteeringReport()
state_report = state_Dynamic()
while (rospy.is_shutdown() != 1):
car.simulate(action)
steering_report.steering_wheel_angle = car.state[6] * car.steeringRatio
state_report.vx = car.state[3]
state_report.vy = car.state[4] - car.state[5] * car.vhMdl[1]
state_report.X = car.state[0] - cos(car.state[2]) * car.vhMdl[1]
state_report.Y = car.state[1] - sin(car.state[2]) * car.vhMdl[1]
state_report.psi = car.state[2]
state_report.wz = car.state[5]
pub1.publish(steering_report)
pub2.publish(state_report)
rate.sleep()
def create_steer(self, val):
st = SteeringReport()
st.steering_wheel_angle = val * math.pi / 180.
## manuall removed _cmd as 1.1.1 library is not available
st.enabled = True
st.speed = self.vel
return st
def create_steer(self, val):
# rospy.logdebug("In Bridge:create_steer\n")
st = SteeringReport()
st.steering_wheel_angle_cmd = val * math.pi / 180.
st.enabled = True
st.speed = self.vel
return st
def __init__(self):
rospy.init_node('brake_thrash')
# Shutdown
self.shutdown = False
# Received messages
self.dbw_enabled = False
self.msg_steering_report_ready = False
self.msg_gear_report = GearReport()
self.msg_gear_report_ready = False
self.msg_steering_report = SteeringReport()
self.msg_steering_report_ready = False
# Parameters
self.started = False
rospy.loginfo('Preparing to thrash the brake pedal command to try and induce a fault...')
rospy.loginfo('Validating that vehicle is parked...')
# Publishers and subscribers
self.pub = rospy.Publisher('/vehicle/brake_cmd', BrakeCmd, queue_size=10)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.recv_enabled)
rospy.Subscriber('/vehicle/gear_report', GearReport, self.recv_gear)
rospy.Subscriber('/vehicle/steering_report', SteeringReport, self.recv_steering)
rospy.Timer(rospy.Duration(0.2), self.timer_process)
def speed_display_tester():
pubSteering = rospy.Publisher('test_steering',
SteeringReport,
queue_size=10)
pubBrake = rospy.Publisher('test_brake', BrakeReport, queue_size=10)
pubTrottle = rospy.Publisher('test_trottle', ThrottleReport, queue_size=10)
pubSignal = rospy.Publisher('test_signal', TurnSignal, queue_size=10)
rospy.init_node('vehicle_control_tester', anonymous=False)
rate = rospy.Rate(10)
steering = 0.0
brake = 0.15
throttle = 0.15
signal = 0
while not rospy.is_shutdown():
ch = getchar()
if (ch == 'q'): signal = 1
elif (ch == 'e'): signal = 2
elif (ch == 'a'): steering -= 0.1
elif (ch == 'd'): steering += 0.1
elif (ch == 'w'): throttle += 0.03
elif (ch == 's'): brake += 0.03
elif (ch == 'p'): break
else:
throttle -= 0.03
brake -= 0.03
if (throttle > 0.5): throttle = 0.5
elif (throttle < 0.15): throttle = 0.15
if (brake > 0.5): brake = 0.5
elif (brake < 0.15): brake = 0.15
msgSteering = SteeringReport()
msgBrake = BrakeReport()
msgThrottle = ThrottleReport()
msgSignal = TurnSignal()
msgSteering.steering_wheel_angle = steering
msgBrake.pedal_input = brake
msgThrottle.pedal_input = throttle
msgSignal.value = signal
pubSteering.publish(msgSteering)
pubBrake.publish(msgBrake)
pubTrottle.publish(msgThrottle)
pubSignal.publish(msgSignal)
rate.sleep()
def create_steer(self, val):
st = SteeringReport()
st.steering_wheel_cmd = val * math.pi / 180.
st.steering_wheel_cmd_type = st.CMD_ANGLE
st.enabled = True
st.speed = self.vel
return st
def simu():
global action
rospy.init_node('simu', anonymous=True)
time.sleep(5)
rate = rospy.Rate(1 / dt)
rospy.loginfo("simulator node starts")
state = [558586.699614032, 4196501.20232720, 1.25432777404785, 5, 0, 0, 0]
#state = [0, 1, 0, 10, 0, 0, 0]
#state = [558641.76, 4196659.62, 1.25, 5, 0, 0, 0]
car.setState(state)
rospy.Subscriber('/vehicle/cmd_vel_stamped',
TwistStamped,
cmd_vel_stampedCallback,
queue_size=1)
rospy.Subscriber('/vehicle/steering_cmd',
SteeringCmd,
steering_cmdCallback,
queue_size=1)
pub1 = rospy.Publisher('/vehicle/steering_report',
SteeringReport,
queue_size=1)
pub2 = rospy.Publisher('state_estimate', state_Dynamic, queue_size=1)
# srv = Server(DynamicParamConfig, errorcallback)
steering_report = SteeringReport()
state_report = state_Dynamic()
while (rospy.is_shutdown() != 1):
car.simulate(action)
steering_report.steering_wheel_angle = car.state[6] * car.steeringRatio
state_report.vx = car.state[3]
state_report.vy = car.state[4]
state_report.X = car.state[0]
state_report.Y = car.state[1]
state_report.psi = car.state[2]
state_report.wz = car.state[5]
pub1.publish(steering_report)
pub2.publish(state_report)
rate.sleep()
def __init__(self):
rospy.init_node('throttle_sweep')
# Variables for logging
self.throttle_cmd = 0.0
self.msg_throttle_report = ThrottleReport()
self.msg_throttle_report_ready = False
self.msg_throttle_info_report = ThrottleInfoReport()
self.msg_throttle_info_report_ready = False
# Other drive-by-wire variables
self.msg_gear_report = GearReport()
self.msg_gear_report_ready = False
self.msg_steering_report = SteeringReport()
self.msg_steering_report_ready = False
# Parameters
self.i = -1
self.start = 0.15
self.end = 0.80
self.resolution = 0.001
self.duration = rospy.Duration(1.5)
rospy.loginfo('Recording throttle pedal data every ' +
str(self.duration.to_sec()) + ' seconds from ' +
str(self.start) + ' to ' + str(self.end) + ' with ' +
str(self.resolution) + ' increments.')
rospy.loginfo('This will take ' +
str((self.end - self.start) / self.resolution *
self.duration.to_sec() / 60.0) + ' minutes.')
# Open CSV file
self.csv_file = open('throttle_sweep_data.csv', 'w')
self.csv_writer = csv.writer(self.csv_file, delimiter=',')
self.csv_writer.writerow(['Throttle (%)', 'Measured (%)'])
# Publishers and subscribers
self.pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
rospy.Subscriber('/vehicle/throttle_report', ThrottleReport,
self.recv_throttle)
rospy.Subscriber('/vehicle/throttle_info_report', ThrottleInfoReport,
self.recv_throttle_info)
rospy.Subscriber('/vehicle/gear_report', GearReport, self.recv_gear)
rospy.Subscriber('/vehicle/steering_report', SteeringReport,
self.recv_steering)
rospy.Timer(rospy.Duration(0.02), self.timer_cmd)
rospy.Timer(self.duration, self.timer_process)
def process(self, carla_actor):
"""
first odom, then tf, then Reports
"""
# --- --- Get Data --- ---
old_control = InputController().get_old_control()
ego_pose = carla_actor.get_transform()
velocity = carla_actor.get_velocity()
forward_speed = sqrt(
pow(velocity.x, 2.0) + pow(velocity.y, 2.0) + pow(velocity.z, 2.0))
simtime = TimedEventHandler().getCurrentSimTime()
cur_time = rospy.Time()
cur_time.set(int(simtime), int(1000000000 * (simtime - int(simtime))))
# carla to ros corrections
# ego_pose.location.x = ego_pose.location.x
ego_pose.location.y = -ego_pose.location.y
# ego_pose.location.z = ego_pose.location.z
ego_pose.rotation.roll = -math.radians(ego_pose.rotation.roll)
ego_pose.rotation.pitch = math.radians(ego_pose.rotation.pitch)
ego_pose.rotation.yaw = -math.radians(ego_pose.rotation.yaw)
# velocity.x = velocity.x
velocity.y = -velocity.y
# velocity.z = velocity.z
# --- --- Odometry --- ---
o = Odometry()
o.header.stamp = cur_time
o.header.frame_id = "utm"
o.child_frame_id = "gps"
o.pose.pose.position.x = ego_pose.location.x
o.pose.pose.position.y = ego_pose.location.y
o.pose.pose.position.z = ego_pose.location.z
q = tf.transformations.quaternion_from_euler(
ego_pose.rotation.roll, ego_pose.rotation.pitch,
ego_pose.rotation.yaw) # RPY
o.pose.pose.orientation.x = q[0]
o.pose.pose.orientation.y = q[1]
o.pose.pose.orientation.z = q[2]
o.pose.pose.orientation.w = q[3]
# o.pose.covariance == empty
# calculate the center-axle offset
A = numpy.matrix(
[[cos(-ego_pose.rotation.yaw),
sin(-ego_pose.rotation.yaw), 0],
[-sin(-ego_pose.rotation.yaw),
cos(-ego_pose.rotation.yaw), 0], [0, 0, 1]])
d = numpy.matrix([[self.center_axle_offset], [0], [1]])
tmp = A.getI() * d
o.pose.pose.position.x += tmp.item(0, 0)
o.pose.pose.position.y += -tmp.item(1, 0)
# o.twist.twist.angular == not available
o.twist.twist.linear.x = velocity.x
o.twist.twist.linear.y = velocity.y
o.twist.twist.linear.z = velocity.z
# o.twist.covariance == empty
self.pub_odom.publish(o)
# --- --- TF --- ---
# --- base_footprint ---
t = TransformStamped()
t.header.stamp = cur_time
t.header.frame_id = "map"
t.child_frame_id = "base_footprint"
t.transform.translation.x = o.pose.pose.position.x
t.transform.translation.y = o.pose.pose.position.y
t.transform.translation.z = o.pose.pose.position.z
t.transform.rotation.x = o.pose.pose.orientation.x
t.transform.rotation.y = o.pose.pose.orientation.y
t.transform.rotation.z = o.pose.pose.orientation.z
t.transform.rotation.w = o.pose.pose.orientation.w
# # NOTE center-axle offset already calculated for Odometry
tf_msg = []
tf_msg.append(t)
self.pub_tf.publish(TFMessage(tf_msg))
# --- --- Joint States --- ---
# TODO calculate joints
jsv = forward_speed / self.wheel_radius
js = JointState()
js.header.frame_id = ""
js.header.stamp = cur_time
js.name = [
'wheel_fl', 'wheel_fr', 'wheel_rl', 'wheel_rr', 'steer_fl',
'steer_fr'
]
js.effort = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
js.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
js.velocity = [jsv, jsv, jsv, jsv, 0.0, 0.0]
self.pub_js.publish(js)
# --- --- Reports --- ---
# --- WheelSpeedReport ---
w = WheelSpeedReport()
w.header.frame_id = ""
w.header.stamp = cur_time
w.front_left = forward_speed / self.wheel_radius
w.front_right = forward_speed / self.wheel_radius
w.rear_left = forward_speed / self.wheel_radius
w.rear_right = forward_speed / self.wheel_radius
self.pub_wsr.publish(w)
# --- BrakeReport ---
br = BrakeReport()
br.header.frame_id = ""
br.header.stamp = cur_time
br.override = old_control['brake'] > 0.0
br.pedal_input = old_control['brake']
br.pedal_output = old_control['brake']
self.pub_br.publish(br)
# --- GearReport ---
gr = GearReport()
gr.header.frame_id = ""
gr.header.stamp = cur_time
gr.cmd.gear = old_control['gear']
gr.state.gear = old_control['gear']
self.pub_gr.publish(gr)
# --- ThrottleReport ---
tr = ThrottleReport()
tr.header.frame_id = ""
tr.header.stamp = cur_time
tr.override = old_control['throttle'] > 0.0
tr.pedal_input = old_control['throttle']
tr.pedal_output = old_control['throttle']
self.pub_tr.publish(tr)
# --- SteeringReport ---
sr = SteeringReport()
sr.header.frame_id = ""
sr.header.stamp = cur_time
sr.speed = forward_speed
steer = -old_control['steer']
sr.steering_wheel_angle = steer * self.max_steer_angle
sr.steering_wheel_angle_cmd = steer * self.max_steer_angle
self.pub_sr.publish(sr)
def create_steer(self, val):
st = SteeringReport()
st.steering_wheel_angle_cmd = val * math.pi/180.
st.enabled = True
st.speed = self.vel
return st
