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 __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)