def gear_cmd_publisher(self, gear_value=0):
"""
ID = 102 in BusCan
# Gear command enumeration
Gear cmd
>>>
uint8 gear
uint8 NONE=0
uint8 PARK=1
uint8 REVERSE=2
uint8 NEUTRAL=3
uint8 DRIVE=4
uint8 LOW=5
>>>
gear is an uint8, but in the MsgGearCmd in dispatch.h only gets 3 Bits, which make up to
8 (0-7) possible gears, which could be: 1,2,3,4,5,R,P,N.
But in th emessage deffinition the only options given are None, Park, Reverse, Neutral, Drive and Low.
so 6 options (0-5)
"""
#msg_str = "Publishing Gear message in /vehicle/gear_cmd %s" % rospy.get_time()
#rospy.loginfo(msg_str)
gear_command_object = GearCmd()
gear_object = Gear()
#gear_object.gear = random.randint(0, 5)
gear_object.gear = gear_value
gear_command_object.cmd = gear_object
rospy.loginfo("Gear Publish ::>" + str(gear_object.gear))
self.pub_gear_cmd.publish(gear_command_object)
def set_vehicle_namespace(self, vehicle):
if vehicle == "mkz":
self.trkr_out_llc_in_node = 'mkz_llc_node'
self.trkr_out_llc_in_topic = '/mkz/ptracker'
self.feedback_out_llc_in_topic = '/mkz/odom'
self.thrtl_topic = '/mkz/throttle_cmd'
self.brake_topic = '/mkz/brake_cmd'
self.steer_topic = '/mkz/steering_cmd'
self.gear_topic = '/mkz/gear_cmd'
self.cmd_vel_topic = 'mkz/cmd_vel'
self.turn_signal_topic = '/mkz/turn_signal_cmd'
self.trkr_msg_t = Float64MultiArray()
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = ThrottleCmd()
self.brake_msg_t = BrakeCmd()
self.steer_msg_t = SteeringCmd()
self.gear_msg_t = GearCmd()
self.turn_signal_msg_t = TurnSignalCmd()
self.cmd_vel_msg_t = Twist()
elif vehicle == "fusion":
self.trkr_out_llc_in_node = 'fusion_llc_node'
self.trkr_out_llc_in_topic = '/fusion/ptracker'
self.thrtl_topic = '/fusion/throttle_cmd'
self.brake_topic = '/fusion/brake_cmd'
self.steer_topic = '/fusion/steering_cmd'
self.gear_topic = '/fusion/gear_cmd'
self.turn_signal_topic = '/fusion/turn_signal_cmd'
self.cmd_vel_topic = 'fusion/cmd_vel'
self.thrtl_msg_t = ThrottleCmd()
self.brake_msg_t = BrakeCmd()
self.steer_msg_t = SteeringCmd()
self.gear_msg_t = GearCmd()
self.turn_signal_msg_t = TurnSignalCmd()
self.cmd_vel_msg_t = Twist()
def __init__(self):
self._NONE = 0
self._PARK = 1
self._REVERSE = 2
self._NEUTRAL = 3
self._DRIVE = 4
self._LOW = 5
self._msg = GearCmd()
def to_mkz_gear(cmd):
assert type(cmd) == ControlCommand
new_cmd = GearCmd()
new_cmd.enable = True
if cmd.gear > 0:
new_cmd.cmd.gear = Gear.DRIVE
elif cmd.gear == ControlCommand.GEAR_NONE:
new_cmd.cmd.gear = Gear.NONE
elif cmd.gear == ControlCommand.GEAR_PARKING:
new_cmd.cmd.gear = Gear.PARK
elif cmd.gear == ControlCommand.GEAR_REVERSE:
new_cmd.cmd.gear = Gear.REVERSE
elif cmd.gear == ControlCommand.GEAR_NEUTRAL:
new_cmd.cmd.gear = Gear.NEUTRAL
elif cmd.gear == ControlCommand.GEAR_DRIVE:
new_cmd.cmd.gear = Gear.DRIVE
else:
rospy.logerr("Incorrect gear command value!")
return None
return new_cmd
def gear_shift_to(gear_no):
''' Gear
uint8 gear
uint8 NONE=0
uint8 PARK=1
uint8 REVERSE=2
uint8 NEUTRAL=3
uint8 DRIVE=4
uint8 LOW=5
'''
''' GearCmd
# Gear command enumeration
Gear cmd
'''
gear = Gear()
gear.gear = gear_no
gear_msg = GearCmd()
gear_msg.cmd = gear
gear_cmd_pub.publish(gear_msg)
print("Gear shifted")
def __init__(self):
self.steering = SteeringCmd()
self.steering.enable = True
self.steering.steering_wheel_angle_cmd = 0
self.accel = ThrottleCmd()
self.accel.enable = True
self.accel.pedal_cmd = 0
self.accel.pedal_cmd_type = 2
self.brake = BrakeCmd()
self.brake.enable = True
self.brake.pedal_cmd = 0
self.brake.pedal_cmd_type = 2
self.gear = GearCmd()
self.gear.cmd.gear = 1
self.parking_state = 0
#self.turn_signal = TurnSignalCmd()
self.pub_steering = rospy.Publisher('vehicle/steering_cmd',
SteeringCmd,
queue_size=10) # red car : vehicle
self.pub_brake = rospy.Publisher('vehicle/brake_cmd',
BrakeCmd,
queue_size=10)
self.pub_accel = rospy.Publisher('vehicle/throttle_cmd',
ThrottleCmd,
queue_size=10)
self.pub_gear = rospy.Publisher('vehicle/gear_cmd',
GearCmd,
queue_size=10)
#pub_turn_signal = rospy.Publisher('vehicle/turn_signal_cmd', TurnSignalCmd, queue_size=10)
rospy.init_node('controller', anonymous=True)
self.sub = rospy.Subscriber('/joy', Joy, self.callback, queue_size=10)
def __init__(self, vehicle = 'mkz'):
self.gen_files_path = '/home/oks/catkin_ws/src/framework_sim/gen_txtfiles/course_txt.txt'
if vehicle == 'mkz':
self.trkr_out_node = 'mkz_ptracker'
self.trkr_out_topic = '/mkz/ptracker'
self.feedback_out_topic = '/mkz/odom'
self.thrtl_topic = '/mkz/throttle_cmd'
self.brake_topic = '/mkz/brake_cmd'
self.steer_topic = '/mkz/steering_cmd'
self.gear_topic = '/mkz/gear_cmd'
self.cmd_vel_topic = '/mkz/cmd_vel'
self.turn_signal_topic = '/mkz/turn_signal_cmd'
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = ThrottleCmd()
self.brake_msg_t = BrakeCmd()
self.steer_msg_t = SteeringCmd()
self.gear_msg_t = GearCmd()
self.turn_signal_msg_t = TurnSignalCmd()
self.cmd_vel_msg_t = Twist()
self.cmd_list_msg_t = Float64MultiArray()
self.time_step = 0.1
elif vehicle == 'blue':
self.trkr_out_node = 'blue_ptracker'
self.trkr_out_topic = '/blue/ptracker'
self.feedback_out_topic = '/blue/odom'
self.thrtl_topic = '/blue/throttle_cmd'
self.brake_topic = '/blue/brake_cmd'
self.steer_topic = '/blue/steering_cmd'
self.cmd_vel_topic = '/blue/cmd_vel'
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = Float64()
self.brake_msg_t = Float64()
self.steer_msg_t = Float64()
self.cmd_vel_msg_t = Twist()
self.cmd_list_msg_t = Float64MultiArray()
self.time_step = 0.1
def __init__(self, vehicle_ns='fusion'):
self.ns = vehicle_ns
if vehicle_ns == 'fusion':
self.llc_out_node = 'fusion_llc'
self.llc_out_topic = '/fusion/llc'
self.trkr_out_topic = '/fusion/ptracker'
self.feedback_out_topic = '/fusion/odom'
self.thrtl_topic = '/fusion/throttle_cmd'
self.brake_topic = '/fusion/brake_cmd'
self.steer_topic = '/fusion/steering_cmd'
self.gear_topic = '/fusion/gear_cmd'
self.cmd_vel_topic = '/fusion/cmd_vel'
self.turn_signal_topic = '/fusion/turn_signal_cmd'
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = ThrottleCmd()
self.brake_msg_t = BrakeCmd()
self.steer_msg_t = SteeringCmd()
self.gear_msg_t = GearCmd()
self.turn_signal_msg_t = TurnSignalCmd()
self.cmd_vel_msg_t = Twist()
self.cmd_list_msg_t = Float64MultiArray()
self.time_step = 0.1
elif vehicle_ns == 'mkz':
self.llc_out_node = 'mkz_llc'
self.llc_out_topic = '/mkz/llc'
self.trkr_out_topic = '/mkz/ptracker'
self.feedback_out_topic = '/mkz/odom'
self.thrtl_topic = '/mkz/throttle_cmd'
self.brake_topic = '/mkz/brake_cmd'
self.steer_topic = '/mkz/steering_cmd'
self.gear_topic = '/mkz/gear_cmd'
self.cmd_vel_topic = '/mkz/cmd_vel'
self.turn_signal_topic = '/mkz/turn_signal_cmd'
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = ThrottleCmd()
self.brake_msg_t = BrakeCmd()
self.steer_msg_t = SteeringCmd()
self.gear_msg_t = GearCmd()
self.turn_signal_msg_t = TurnSignalCmd()
self.cmd_vel_msg_t = Twist()
self.cmd_list_msg_t = Float64MultiArray()
self.time_step = 0.1
elif vehicle_ns == 'blue':
self.llc_out_node = 'blue_llc'
self.llc_out_topic = '/blue/llc'
self.trkr_out_topic = '/blue/ptracker'
self.feedback_out_topic = '/blue/odom'
self.thrtl_topic = '/blue/throttle_cmd'
self.brake_topic = '/blue/brake_cmd'
self.steer_topic = '/blue/steering_cmd'
self.cmd_vel_topic = '/blue/cmd_vel'
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = Float64()
self.brake_msg_t = Float64()
self.steer_msg_t = Float64()
self.cmd_vel_msg_t = Twist()
self.cmd_list_msg_t = Float64MultiArray()
self.time_step = 0.1
elif vehicle_ns == 'orange':
self.llc_out_node = 'orange_llc'
self.llc_out_topic = '/orange/llc'
self.trkr_out_topic = '/orange/ptracker'
self.feedback_out_topic = '/orange/odom'
self.thrtl_topic = '/orange/throttle_cmd'
self.brake_topic = '/orange/brake_cmd'
self.steer_topic = '/orange/steering_cmd'
self.cmd_vel_topic = '/orange/cmd_vel'
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = Float64()
self.brake_msg_t = Float64()
self.steer_msg_t = Float64()
self.cmd_vel_msg_t = Twist()
self.cmd_list_msg_t = Float64MultiArray()
self.time_step = 0.1
def publishGearCmd(self, gear): msg = GearCmd() msg.cmd.gear = gear self.gear_pub.publish(msg)
def __init__(self, vehicle_ns):
self.ns = vehicle_ns
self.gen_file_cont = '/home/oks/catkin_ws/src/framework_sim/gen_txtfiles/controller_output.txt'
self.gen_file_cont_fb = '/home/oks/catkin_ws/src/framework_sim/gen_txtfiles/cont_fb_out.txt'
self.gen_file_odom = '/home/oks/catkin_ws/src/framework_sim/gen_txtfiles/odom_data_file.txt'
self.gen_file_course = '/home/oks/catkin_ws/src/framework_sim/gen_txtfiles/course_txt.txt'
self.gen_file_x_path = '/home/oks/catkin_ws/src/framework_sim/gen_txtfiles/x_path_txt.txt'
self.gen_file_y_path = '/home/oks/catkin_ws/src/framework_sim/gen_txtfiles/y_path_txt.txt'
self.gen_file_yaw_path = '/home/oks/catkin_ws/src/framework_sim/gen_txtfiles/yaw_path_txt.txt'
self.gen_gile_max_speed = '/home/oks/catkin_ws/src/framework_sim/gen_txtfiles/target_max_speed.txt'
if self.ns == 'fusion':
self.llc_out_node = 'fusion_llc'
self.trkr_out_node = 'fusion_ptracker'
self.llc_out_topic = '/fusion/llc'
self.trkr_out_topic = '/fusion/ptracker'
self.feedback_out_topic = '/fusion/odom'
self.thrtl_topic = '/fusion/throttle_cmd'
self.brake_topic = '/fusion/brake_cmd'
self.steer_topic = '/fusion/steering_cmd'
self.gear_topic = '/fusion/gear_cmd'
self.cmd_vel_topic = '/fusion/cmd_vel'
self.turn_signal_topic = '/fusion/turn_signal_cmd'
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = ThrottleCmd()
self.brake_msg_t = BrakeCmd()
self.steer_msg_t = SteeringCmd()
self.gear_msg_t = GearCmd()
self.turn_signal_msg_t = TurnSignalCmd()
self.cmd_vel_msg_t = Twist()
self.cmd_list_msg_t = Float64MultiArray()
self.time_step = 0.1
elif self.ns == 'mkz':
self.llc_out_node = 'mkz_llc'
self.llc_out_topic = '/mkz/llc'
self.trkr_out_node = 'mkz_ptracker'
self.trkr_out_topic = '/mkz/ptracker'
self.feedback_out_topic = '/mkz/odom'
self.thrtl_topic = '/mkz/throttle_cmd'
self.brake_topic = '/mkz/brake_cmd'
self.steer_topic = '/mkz/steering_cmd'
self.gear_topic = '/mkz/gear_cmd'
self.cmd_vel_topic = '/mkz/cmd_vel'
self.turn_signal_topic = '/mkz/turn_signal_cmd'
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = ThrottleCmd()
self.brake_msg_t = BrakeCmd()
self.steer_msg_t = SteeringCmd()
self.gear_msg_t = GearCmd()
self.turn_signal_msg_t = TurnSignalCmd()
self.cmd_vel_msg_t = Twist()
self.cmd_list_msg_t = Float64MultiArray()
self.time_step = 0.1
elif self.ns == 'blue':
self.llc_out_node = 'blue_llc'
self.trkr_out_node = 'blue_ptracker'
self.llc_out_topic = '/blue/llc'
self.trkr_out_topic = '/blue/ptracker'
self.feedback_out_topic = '/blue/odom'
self.thrtl_topic = '/blue/throttle_cmd'
self.brake_topic = '/blue/brake_cmd'
self.steer_topic = '/blue/steering_cmd'
self.cmd_vel_topic = '/blue/cmd_vel'
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = Float64()
self.brake_msg_t = Float64()
self.steer_msg_t = Float64()
self.cmd_vel_msg_t = Twist()
self.cmd_list_msg_t = Float64MultiArray()
self.time_step = 0.1
elif self.ns == 'orange':
self.llc_out_node = 'orange_llc'
self.trkr_out_node = 'orange_ptracker'
self.llc_out_topic = '/orange/llc'
self.trkr_out_topic = '/orange/ptracker'
self.feedback_out_topic = '/orange/odom'
self.thrtl_topic = '/orange/throttle_cmd'
self.brake_topic = '/orange/brake_cmd'
self.steer_topic = '/orange/steering_cmd'
self.cmd_vel_topic = '/orange/cmd_vel'
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = Float64()
self.brake_msg_t = Float64()
self.steer_msg_t = Float64()
self.cmd_vel_msg_t = Twist()
self.cmd_list_msg_t = Float64MultiArray()
self.time_step = 0.1
def __init__(self):
rospy.init_node("rl_sim_env")
#self.initial_model_state = None
rospy.wait_for_service("/gazebo/set_model_state")
self.set_state_pub = rospy.Publisher("/gazebo/set_model_state",
ModelState,
queue_size=10)
#self.set_state_proxy = rospy.ServiceProxy("/gazebo/set_model_state",SetModelState)
self.car_name = "vehicle"
self.init_pose = Pose(Point(-180, -2.3, 0.1), Quaternion(0, 0, 0, 1))
self.init_state = ModelState()
self.init_state.model_name = self.car_name
self.init_state.pose = self.init_pose
self.steering = SteeringCmd()
self.steering.enable = True
self.steering.steering_wheel_angle_cmd = 0
self.accel = ThrottleCmd()
self.accel.enable = True
self.accel.pedal_cmd = 0
self.accel.pedal_cmd_type = 2
self.brake = BrakeCmd()
self.brake.enable = True
self.brake.pedal_cmd = 0
self.brake.pedal_cmd_type = 2
self.gear = GearCmd()
self.gear.cmd.gear = 1
self.pub_steering = rospy.Publisher('vehicle/steering_cmd',
SteeringCmd,
queue_size=10) # red car : vehicle
self.pub_accel = rospy.Publisher('vehicle/throttle_cmd',
ThrottleCmd,
queue_size=10)
self.pub_brake = rospy.Publisher('vehicle/brake_cmd',
BrakeCmd,
queue_size=10)
self.pub_gear = rospy.Publisher('vehicle/gear_cmd',
GearCmd,
queue_size=10)
self.bridge = CvBridge()
self.feature_state = np.zeros(10)
self.deviation = 0
self.dev_ = 0
self.reach_goal = False
self.out_of_lane = False
self.x_coord = self.init_pose.position.x
self.goal_x = 180
self.state_sub = rospy.Subscriber('gazebo/model_states', ModelStates,
self.state_callback)
self.lane_sub = rospy.Subscriber('/vehicle/lane_number', Int32,
self.lane_callback)
self.deviation_sub = rospy.Subscriber('/vehicle/deviation', Float32,
self.deviation_callback)
self.observation_space = 8
self.action_space = 3
self.time_limit = 100
self.time = 0