From 1f4b217bfcf6c56e2b1d81fe08b4fb603a1139ec Mon Sep 17 00:00:00 2001 From: Abdul_Rahman <94268081+RoboticistAbdulrahman@users.noreply.github.com> Date: Wed, 21 Sep 2022 19:07:38 +0300 Subject: [PATCH] added brake feature v0.1.1 --- .../scripts/beamng_control/bridge.py | 2 +- .../scripts/beamng_control/publishers.py | 6 +- .../scripts/beamng_control/sensorHelper.py | 2 +- beamng_teleop_keyboard/nodes/converter | 114 +++++++++--------- beamng_teleop_keyboard/nodes/teleop_key | 10 ++ 5 files changed, 69 insertions(+), 65 deletions(-) diff --git a/beamng_control/scripts/beamng_control/bridge.py b/beamng_control/scripts/beamng_control/bridge.py index 495239b..020162f 100644 --- a/beamng_control/scripts/beamng_control/bridge.py +++ b/beamng_control/scripts/beamng_control/bridge.py @@ -383,7 +383,7 @@ def main(): f' version is {available_version}, aborting process.') sys.exit(1) - bngpy.setup_logging() + # bngpy.setup_logging() argv = rospy.myargv(argv=sys.argv) rospy.loginfo("cmd args"+str(argv)) diff --git a/beamng_control/scripts/beamng_control/publishers.py b/beamng_control/scripts/beamng_control/publishers.py index 9b9b719..59368ca 100644 --- a/beamng_control/scripts/beamng_control/publishers.py +++ b/beamng_control/scripts/beamng_control/publishers.py @@ -9,7 +9,7 @@ import std_msgs.msg import beamngpy.sensors as bng_sensors -from beamngpy.noise import RandomImageNoise, RandomLIDARNoise +#from beamngpy.noise import RandomImageNoise, RandomLIDARNoise from visualization_msgs.msg import Marker, MarkerArray @@ -27,8 +27,8 @@ def get_sensor_publisher(sensor): bng_sensors.Electrics: ElectricsPublisher, bng_sensors.Camera: CameraPublisher, bng_sensors.Lidar: LidarPublisher, - RandomImageNoise: CameraPublisher, - RandomLIDARNoise: LidarPublisher + # RandomImageNoise: CameraPublisher, + #s RandomLIDARNoise: LidarPublisher } for k, v in sensor_mapping.items(): if isinstance(sensor, k): diff --git a/beamng_control/scripts/beamng_control/sensorHelper.py b/beamng_control/scripts/beamng_control/sensorHelper.py index c9577ae..bcb6366 100644 --- a/beamng_control/scripts/beamng_control/sensorHelper.py +++ b/beamng_control/scripts/beamng_control/sensorHelper.py @@ -2,7 +2,7 @@ import copy import beamngpy.sensors as bng_sensors -from beamngpy.noise import RandomImageNoise, RandomLIDARNoise +#from beamngpy.noise import RandomImageNoise, RandomLIDARNoise class SensorSpecificationError(TypeError): diff --git a/beamng_teleop_keyboard/nodes/converter b/beamng_teleop_keyboard/nodes/converter index f72af11..6b008a1 100644 --- a/beamng_teleop_keyboard/nodes/converter +++ b/beamng_teleop_keyboard/nodes/converter @@ -3,6 +3,7 @@ from logging import exception import rospy from geometry_msgs.msg import Twist +from std_msgs.msg import String import subprocess import os import beamngpy as bngpy @@ -13,6 +14,8 @@ class twist2bng(): def __init__(self): self.BNG_pub = rospy.Publisher('/control', VehicleControl ,queue_size=10) self.cmd_vel_sub = rospy.Subscriber( '/cmd_vel', Twist, self.cmd_vel_callback , queue_size=10 ) + self.brake_sub = rospy.Subscriber( '/brake', String, self.brake_callback , queue_size=10 ) + # self.brake_sub = rospy.Subscriber( '/brake', String, self.brake_callback , queue_size=10 ) #set puplish rate self.checker_rate = rospy.Rate(10) self.Vehicle_Control_msg=VehicleControl() @@ -23,11 +26,16 @@ class twist2bng(): self.control_linear_vel=0 self.control_angular_vel=0 self.has_velosity=False + self.brake_state="False" + self.vehicle_state="initial" self.main_loop() - # ------------- call back functions ---------------- # + def brake_callback(self, brake_state): + self.brake_state=brake_state.data + # print ("brake_state", self.brake_state) + def cmd_vel_callback(self, cmd_vel): # self.has_velosity= cmd_vel self.control_linear_vel_new= cmd_vel.linear.x @@ -36,22 +44,20 @@ class twist2bng(): def twist_msg_comparison(self): if self.control_linear_vel_old != self.control_linear_vel_new: - # print ("new reading") - print ("new data") - self.control_linear_vel_old = self.control_linear_vel_new + # print ("new data") self.convert_cmd_to_bng(self.control_linear_vel_new,self.control_angular_vel_new) + self.control_linear_vel_old = self.control_linear_vel_new return True if self.control_angular_vel_old != self.control_angular_vel_new: - # print ("new reading") - print ("new data") - self.control_angular_vel_old = self.control_angular_vel_new + # print ("new data") self.convert_cmd_to_bng(self.control_linear_vel_new,self.control_angular_vel_new) + self.control_angular_vel_old = self.control_angular_vel_new return True else: - print ("old data") + # print ("old data") return False @@ -61,58 +67,47 @@ class twist2bng(): def convert_cmd_to_bng(self,control_linear_vel,control_angular_vel): + self.Vehicle_Control_msg=VehicleControl() # print ("inside convert_cmd_to_bng") - self.Vehicle_Control_msg=VehicleControl() - - if control_linear_vel == 0.0 : - self.Vehicle_Control_msg.throttle = 0.0 - self.Vehicle_Control_msg.steering = 0.0 - self.Vehicle_Control_msg.brake = 1 - self.Vehicle_Control_msg.parkingbrake = 1 - self.Vehicle_Control_msg.clutch = 1 - self.Vehicle_Control_msg.gear = 0 - - if control_linear_vel < 0.0 : - self.Vehicle_Control_msg.throttle = abs(control_linear_vel) - self.Vehicle_Control_msg.steering = control_angular_vel - self.Vehicle_Control_msg.brake = 0 - self.Vehicle_Control_msg.parkingbrake = 0 - self.Vehicle_Control_msg.clutch = 0 - self.Vehicle_Control_msg.gear = -1 - - - else : - self.Vehicle_Control_msg.throttle = abs(control_linear_vel) - self.Vehicle_Control_msg.steering = control_angular_vel - self.Vehicle_Control_msg.brake = 0 - self.Vehicle_Control_msg.parkingbrake = 0 - self.Vehicle_Control_msg.clutch = 0 - self.Vehicle_Control_msg.gear = 1 - - # print ("",self.Vehicle_Control_msg) + # print ("",control_linear_vel) + if self.brake_state=="True": + self.Vehicle_Control_msg.throttle = 0.0 + self.Vehicle_Control_msg.steering = 0.0 + self.Vehicle_Control_msg.brake = 1 + self.Vehicle_Control_msg.parkingbrake = 1 + self.Vehicle_Control_msg.clutch = 1 + self.Vehicle_Control_msg.gear = 1 + self.vehicle_state="stop" + # print ("",self.vehicle_state) + + if self.brake_state=="False": + if control_linear_vel < 0.0: + self.Vehicle_Control_msg.throttle = abs(control_linear_vel) + self.Vehicle_Control_msg.steering = control_angular_vel + self.Vehicle_Control_msg.brake = 0 + self.Vehicle_Control_msg.parkingbrake = 0 + self.Vehicle_Control_msg.clutch = 0 + self.Vehicle_Control_msg.gear = -1 + self.vehicle_state="reverse" + # print ("",self.vehicle_state) + + else: + self.Vehicle_Control_msg.throttle = abs(control_linear_vel) + self.Vehicle_Control_msg.steering = control_angular_vel + self.Vehicle_Control_msg.brake = 0 + self.Vehicle_Control_msg.parkingbrake = 0 + self.Vehicle_Control_msg.clutch = 0 + self.Vehicle_Control_msg.gear = 1 + self.vehicle_state="go" + print ("",self.vehicle_state) + print ("",self.brake_state) + + + + # self.BNG_pub.publish(self.Vehicle_Control_msg) - def move_from_terminal(self): - command= "rostopic pub -r 1 control beamng_msgs/VehicleControl " - throttle= self.Vehicle_Control_msg.throttle - steering= self.Vehicle_Control_msg.steering - brake= self.Vehicle_Control_msg.brake - parkingbrake= self.Vehicle_Control_msg.parkingbrake - clutch= self.Vehicle_Control_msg.clutch - gear= self.Vehicle_Control_msg.gear - combine = command + str(steering) + " " + str(throttle) + " " + str(brake) + " " + str(parkingbrake) + " " + str(clutch) + " " + str(gear) - # subprocess.run([combine], universal_newlines=True, shell=True) - try: - print ("calling terminal") - print ("" , combine) - cp = subprocess.run([combine], universal_newlines=False, shell=False) - except : - print(" ------------------") - print(" ------------------") - print(" bridge error") - print(" ------------------") - print(" ------------------") - + @@ -122,9 +117,8 @@ class twist2bng(): # print(self.twist_msg_comparison) if self.twist_msg_comparison: self.BNG_pub.publish(self.Vehicle_Control_msg) - # self.move_from_terminal() - # else: - + + # print("",self.vehicle_state) self.checker_rate.sleep() diff --git a/beamng_teleop_keyboard/nodes/teleop_key b/beamng_teleop_keyboard/nodes/teleop_key index d152532..c77490b 100644 --- a/beamng_teleop_keyboard/nodes/teleop_key +++ b/beamng_teleop_keyboard/nodes/teleop_key @@ -5,6 +5,8 @@ import rospy from geometry_msgs.msg import Twist +from std_msgs.msg import String + import sys, select, os if os.name == 'nt': import msvcrt @@ -103,6 +105,7 @@ if __name__=="__main__": rospy.init_node('teleop') pub = rospy.Publisher('cmd_vel', Twist, queue_size=10) + brakepub = rospy.Publisher('brake', String, queue_size=10) # BNG_pub = rospy.Publisher('control', bng_msgs.VehicleControl ,queue_size=10) # BNG_pub = rospy.Publisher('control', VehicleControl ,queue_size=10) # BNG_pub = rospy.Publisher('control', BNG_msgs,queue_size=2) @@ -114,6 +117,7 @@ if __name__=="__main__": target_angular_vel = 0.0 control_linear_vel = 0.0 control_angular_vel = 0.0 + brake_state= "False" try: print(msg) @@ -123,24 +127,29 @@ if __name__=="__main__": target_linear_vel = checkLinearLimitVelocity(target_linear_vel + LIN_VEL_STEP_SIZE) status = status + 1 print(vels(target_linear_vel,target_angular_vel)) + brake_state= "False" elif key == 'x' : target_linear_vel = checkLinearLimitVelocity(target_linear_vel - LIN_VEL_STEP_SIZE) status = status + 1 print(vels(target_linear_vel,target_angular_vel)) + brake_state= "False" elif key == 'd' : target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE) status = status + 1 print(vels(target_linear_vel,target_angular_vel)) + brake_state= "False" elif key == 'a' : target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE) status = status + 1 print(vels(target_linear_vel,target_angular_vel)) + brake_state= "False" elif key == ' ' or key == 's' : target_linear_vel = 0.0 control_linear_vel = 0.0 target_angular_vel = 0.0 control_angular_vel = 0.0 print(vels(target_linear_vel, target_angular_vel)) + brake_state= "True" else: if (key == '\x03'): break @@ -158,6 +167,7 @@ if __name__=="__main__": twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel pub.publish(twist) + brakepub.publish(brake_state) except: print(e)