Skip to content

Commit

Permalink
added brake feature v0.1.1
Browse files Browse the repository at this point in the history
  • Loading branch information
@AbdelrahmanElsaidElsawy
AbdelrahmanElsaidElsawy committed Sep 21, 2022
1 parent 32f9cbf commit 1f4b217
Show file tree
Hide file tree
Showing 5 changed files with 69 additions and 65 deletions.
2 changes: 1 addition & 1 deletion beamng_control/scripts/beamng_control/bridge.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Expand Down
6 changes: 3 additions & 3 deletions beamng_control/scripts/beamng_control/publishers.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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):
Expand Down
2 changes: 1 addition & 1 deletion beamng_control/scripts/beamng_control/sensorHelper.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down
114 changes: 54 additions & 60 deletions beamng_teleop_keyboard/nodes/converter
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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()
Expand All @@ -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
Expand All @@ -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


Expand All @@ -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(" ------------------")





Expand All @@ -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()


Expand Down
10 changes: 10 additions & 0 deletions beamng_teleop_keyboard/nodes/teleop_key
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand All @@ -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)
Expand All @@ -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
Expand All @@ -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)
Expand Down

0 comments on commit 1f4b217

Please sign in to comment.