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