def drive_straight(self):
while not rospy.is_shutdown():
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = 2.0
drive_msg.steering_angle = 0.0
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.pub.publish(drive_msg_stamped)
if (SPEED == 0):
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = 0.0
drive_msg.steering_angle = 0.0
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.pub.publish(drive_msg_stamped)
rospy.sleep(5000)
# don't spin too fast
rospy.sleep(1.0 / PUBLISH_RATE)
def __on_camera_image(self, message):
img = message.data
img = np.frombuffer(img, dtype=np.uint8).reshape((message.height, message.width, 4))
img = img[380:420, :]
# Segment the image by color in HSV color space
img = cv2.cvtColor(img, cv2.COLOR_RGB2HSV)
mask = cv2.inRange(img, np.array([50, 110, 150]), np.array([120, 255, 255]))
# Find the largest segmented contour
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
command_message = AckermannDrive()
command_message.speed = 50.0
command_message.steering_angle = 0.0
if contours:
largest_contour = max(contours, key=cv2.contourArea)
largest_contour_center = cv2.moments(largest_contour)
if largest_contour_center['m00'] != 0:
center_x = int(largest_contour_center['m10'] / largest_contour_center['m00'])
# Find error (the lane distance from the target distance)
error = center_x - 190
command_message.steering_angle = error*CONTROL_COEFFICIENT
self.__ackermann_publisher.publish(command_message)
def twist_ackermann_node(data):
command = AckermannDrive()
if data.linear.x == 0.0 or data.angular.z == 0.0:
command.steering_angle = 0.0
else:
radius = data.linear.x / data.angular.z
command.steering_angle = math.atan(wheelbase / radius)
command.speed = data.linear.x
command_pub.publish(command)
def call_back2(self, data):
ack_cmd = AckermannDriveStamped()
ack_cmd.header.stamp = rospy.Time.now()
drive = AckermannDrive()
drive.speed = data.linear.x
drive.steering_angle = data.angular.z
# impose limits on commanded angle
# datetime object containing current date and time
now = datetime.now()
print(self.i)
drive.steering_angle = self.i
ack_cmd.drive = drive
self.publisher.publish(ack_cmd)
def processError(self, error, dist):
if dist == 0:
steering_angle_cmd = 0
speed_cmd = 0
else:
error = error / 1000.0
steering_angle_cmd = self.pid.update(error)
if dist < SLOWDOWN_DISTANCE:
speed_cmd = self.speed_slow
else:
speed_cmd = self.speed
if not self.sim:
speed_cmd += abs(
self.latest_trailer_angle
) * SPEED_INCREASE_COEFFICIENT / MAX_TRAILER_ANGLE
ack = AckermannDrive()
ack.steering_angle = steering_angle_cmd
ack.speed = speed_cmd
if self.lock_stop:
ack.speed = 0
self.drive_publisher.publish(ack)
def odom_callback(msg):
ack_msg = AckermannDrive()
ack_msg_stmp = AckermannDriveStamped()
#Speed: Set constant half max speed
ack_msg.speed = max_speed / 2.0
#Steering: Random btwn 0 and 1
rand = random()
steering_range = max_steering_angle / 2.0
rand_angle = steering_range * rand - steering_range / 2.0
# sometimes change sign so it turns more (basically add bias to continue turning in current direction)
rand = random()
if rand > 0.8 and prev_angle != 0.0:
sign_rand = rand_angle / abs(rand_angle)
sign_prev = prev_angle / abs(prev_angle)
rand_angle *= sign_rand * sign_prev
#set angle
ack_msg.steering_angle = min(max(prev_angle + rand_ang, -max_steering_angle), max_steering_angle))
#reset prev_angle
prev_angle = ack_msg.steering_angle
ack_msg_stmp = ack_msg
drive_pub.publish(ack_msg_stmp)
def joy_command_callback(data):
global current_command_topic
global offboard_command
passthrough_command = AckermannDrive()
# listen to control transfer commands
if data.buttons[ctl_offboard_button] and not data.buttons[ctl_teleop_button]:
if not message_display[0]:
rospy.loginfo(log_message.format(control_priority[0]))
message_display[0] = True
message_display[1] = False
if current_command_topic != command_topic[0]:
current_command_topic = command_topic[0]
if not data.buttons[ctl_offboard_button] and data.buttons[ctl_teleop_button]:
if not message_display[1]:
rospy.loginfo(log_message.format(control_priority[1]))
message_display[0] = False
message_display[1] = True
if current_command_topic != command_topic[1]:
current_command_topic = command_topic[1]
if current_command_topic == command_topic[1]:
# identify and scale raw command data
passthrough_command.steering_angle = data.axes[joy_angle_axis] * joy_angle_scaler
passthrough_command.speed = data.axes[joy_speed_axis] * joy_speed_scaler
elif current_command_topic == command_topic[0] and listen_offboard == 'true':
passthrough_command = offboard_command
multiplexer_pub.publish(passthrough_command)
def rotate_leg(self, speed, radius, arc, update_rate):
# Initialize the movement command
move_cmd = AckermannDrive()
# Set the forward speed
# Setting no acceleration or jerk will accelerate at maximum rate
move_cmd.speed = speed
# Set the steering angle - Need to derive from CLMR Kinematics
# TODO: Check to see that it is within the geometric limits of the robot
angle = math.atan(self.wheelbase / radius)
move_cmd.steering_angle = angle
# Move forward for a time to go the desired distance
distance = arc * radius
duration = distance / speed
ticks = int(duration * update_rate)
for t in range(ticks):
self.cmd_ack.publish(move_cmd)
self.rate.sleep()
# Stop the robot before the next leg
self.cmd_ack.publish(AckermannDrive())
rospy.sleep(1)
def cmd_callback(data):
global wheelbase
global ackermann_cmd_topic
global frame_id
global pub
global message_type
if message_type == 'ackermann_drive':
v = data.linear.x
steering = convert_trans_rot_vel_to_steering_angle(
v, data.angular.z, wheelbase)
msg = AckermannDrive()
msg.steering_angle = steering
msg.speed = v
pub.publish(msg)
else:
v = data.linear.x
steering = convert_trans_rot_vel_to_steering_angle(
v, data.angular.z, wheelbase)
msg = AckermannDriveStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = frame_id
msg.drive.steering_angle = steering
msg.drive.speed = v
pub.publish(msg)
def call_back2(self,data):
ack_cmd = AckermannDriveStamped()
ack_cmd.header.stamp = rospy.Time.now()
drive = AckermannDrive()
drive.speed = data.linear.x
drive.steering_angle = data.angular.z
# impose limits on commanded angle
# datetime object containing current date and time
now = datetime.now()
self.i = now.strftime("%H:%M:%S")
my_name="youssf"
# dd/mm/YY H:M:S
image =self.overlay()[210:,:]
if(self.temp != str(self.i)):
with open("data_index.txt", "a") as myfile:
cv2.imwrite("data/"+my_name+self.i+'.jpg',image)
myfile.write("data/"+my_name+self.i+'.jpg,'+str(drive.steering_angle)+"\n")
print("now =", str(self.number) + " with id = "+str(self.i)+" shape"+str(image.shape))
self.temp=str(self.i)
self.number+=1
ack_cmd.drive = drive
self.publisher.publish(ack_cmd)
def keyboard_command_callback(data):
global current_command_topic
global offboard_command
passthrough_command = AckermannDrive()
# listen to control transfer commands
if rospy.get_param(
'/{}/command_priority'.format(car_name)) == control_priority[0]:
if not message_display[0]:
rospy.loginfo(log_message.format(control_priority[0]))
message_display[0] = True
message_display[1] = False
if current_command_topic != command_topic[0]:
current_command_topic = command_topic[0]
if rospy.get_param(
'/{}/command_priority'.format(car_name)) == control_priority[1]:
if not message_display[1]:
rospy.loginfo(log_message.format(control_priority[1]))
message_display[0] = False
message_display[1] = True
if current_command_topic != command_topic[1]:
current_command_topic = command_topic[1]
if current_command_topic == command_topic[1]:
# identify and scale raw command data
passthrough_command = data
elif current_command_topic == command_topic[
0] and listen_offboard == 'true':
passthrough_command = offboard_command
if rospy.get_param('/{}/suspend_control'.format(car_name)) in [
'True', 'true', '1', '1.0'
]:
passthrough_command.steering_angle = 0.0
passthrough_command.speed = 0.0
multiplexer_pub.publish(passthrough_command)
else:
multiplexer_pub.publish(passthrough_command)
def cmd_callback(data):
global wheelbase
global ackermann_cmd_topic
global frame_id
global pub
global message_type
# Hard simulation limit
if data.angular.z > 1.30:
data.angular.z = 1.30
elif data.angular.z < -1.30:
data.angular.z = -1.30
if message_type == 'ackermann_drive':
v = data.linear.x
steering = data.angular.z
msg = AckermannDrive()
msg.steering_angle = steering
msg.speed = v
pub.publish(msg)
else:
v = data.linear.x
steering = data.angular.z
msg = AckermannDriveStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = frame_id
msg.drive.steering_angle = steering
msg.drive.speed = v
pub.publish(msg)
def control(data):
global prev_error
global vel_input
global kp
global kd
global angle = 0.0
print("PID Control Node is Listening to error")
## Your PID code goes here
#TODO: Use kp, ki & kd to implement a PID controller
# 1. Scale the error
# 2. Apply the PID equation on error to compute steering
# An empty AckermannDrive message is created. You will populate the steering_angle and the speed fields.
command = AckermannDrive()
# TODO: Make sure the steering value is within bounds [-100,100]
command.steering_angle = angle
# TODO: Make sure the velocity is within bounds [0,100]
command.speed = vel_input
# Move the car autonomously
command_pub.publish(command)
def __init__(self):
rospy.init_node("self_driving_system_bridge_node")
self.steering_cmd = None
self.speed_cmd = None
rospy.Subscriber('/vehicle/dbw/steering_cmds/', Float32, callback=self.steering_cmd_callback)
rospy.Subscriber('/vehicle/dbw/cruise_cmds', Float32, callback=self.speed_cmd_callback)
ackermann_pub = rospy.Publisher('/carla/ego_vehicle/ackermann_cmd', AckermannDrive)
rate = rospy.Rate(30)
rospy.loginfo("self-driving system bridge started")
while not rospy.is_shutdown():
if self.steering_cmd is not None and self.speed_cmd is not None:
ackermann_msg = AckermannDrive()
ackermann_msg.steering_angle = -1.0 * float(self.steering_cmd)
ackermann_msg.speed = 5 + float(self.speed_cmd)
ackermann_pub.publish(ackermann_msg)
rate.sleep()
def set_steer(data):
car_ctl = AckermannDrive()
car_msg = AckermannDriveStamped()
steering_angle = rospy.Publisher('/vesc/racecar/ackermann_cmd_mux/input/teleop', AckermannDriveStamped, queue_size=1)
k_distance = 6.0
k_angle = 2.0
if data.data[0] and data.data[3]:
print "section 1"
#error = k_distance * (data.data[4] - data.data[1])
error = k_angle * (90.0 - data.data[2])
print "k_distance", k_distance, "data.data[4]", data.data[4], "data_data[1]", data.data[1], "error", error
elif data.data[0] and not data.data[3]:
print "section 2"
error = k_angle * (90.0 - data.data[2])
elif data.data[3] and not data.data[0]:
print "section 3"
error = k_angle * (-90.0 - data.data[5])
else:
print "section 4"
pass
#car_ctl.acceleration = 0.0
car_ctl.speed = 0.8
car_ctl.steering_angle = error
#steering_angle.publish('{drive: {steering_angle: ' + str(error) + ', speed: ' + str(prev_speed) + '}}')
#rospy.Publisher('ackermann_cmd', AckermannDriveStamped)
"publish pilot command message"
car_msg.header.stamp = rospy.Time.now()
car_msg.header.frame_id = "wall_follower"
car_msg.drive = car_ctl
print "right before publish, steering_angle: ", error
steering_angle.publish(car_msg)
def finalize(self):
rospy.loginfo('Halting motors, aligning wheels and exiting...')
ackermann_cmd_msg = AckermannDrive()
ackermann_cmd_msg.speed = 0
ackermann_cmd_msg.steering_angle = 0
self.drive_pub.publish(ackermann_cmd_msg)
sys.exit()
def control(self,data):
car_ctl = AckermannDrive()
car_msg = AckermannDriveStamped()
object_angle=data.data[0]*math.pi/180
object_distance=data.data[1]
if data.data[0] != -1:
desired = -.3 #how wide to clear cone
multiplier=1
if(not self.right):
desired = .3
desired_distance=.1
distance_y = object_distance * math.cos(object_angle)
if distance_y < .4:
desired *= 2./3
steering_gain=.1
steering_max = .34
actual=multiplier * math.sin(object_angle)*object_distance
#if actual < desired and object_distance > desired * 1.2:
# actual = object_distance
error=desired-actual
print actual, " actual"
print object_distance, " object_distance"
print steering_gain * error, " steering"
steering_angle = multiplier*steering_gain * error
car_max_speed=1.0
#desired_distance=.1
#distance_y = object_distance * math.cos(object_angle)
distance_error=distance_y-desired_distance
print 'distance y ', distance_y
speed_gain=.4
speed=distance_error*speed_gain
print 'speed ', speed
#speed=.3
car_ctl.speed = speed
car_ctl.steering_angle = steering_angle
car_msg.header.stamp = rospy.Time.now()
car_msg.header.frame_id = "IPSController"
car_msg.drive = car_ctl
self.control_pub.publish(car_msg)
else:
car_ctl.steering_angle = 0
car_ctl.speed = 0 #change for, later crawl forward
car_msg.header.frame_id = "IPSController"
car_msg.drive = car_ctl
self.control_pub.publish(car_msg)
def finalize(self):
rospy.loginfo('Halting motors, aligning wheels and exiting...')
self.settings = termios.tcgetattr(sys.stdin)
ackermann_cmd_msg = AckermannDrive()
ackermann_cmd_msg.speed = 0
ackermann_cmd_msg.steering_angle = 0
self.motors_pub.publish(ackermann_cmd_msg)
sys.exit()
def pub_callback(self, event):
ackermann_cmd_msg = AckermannDrive()
ackermann_cmd_msg.speed = self.speed
ackermann_cmd_msg.steering_angle = self.steering_angle
# ackermann_cmd_msg = AckermannDriveStamped()
# ackermann_cmd_msg.drive.speed = self.speed
# ackermann_cmd_msg.drive.steering_angle = self.steering_angle
self.motors_pub.publish(ackermann_cmd_msg)
def stop_vehicle(self):
msg = AckermannDrive()
msg.speed = 0
msg.acceleration = 0
msg.jerk = 0
msg.steering_angle = 0
msg.steering_angle_velocity = 0
self.pub_ackermann_cmd.publish(msg)
rospy.signal_shutdown('Made it to goal')
def set_vehicle_command(self, velocity, steering_angle):
''' Publishes the calculated steering angle '''
drive = AckermannDrive()
drive.speed = velocity
drive.steering_angle = steering_angle
drive.steering_angle_velocity = 0.0
self.tracker_pub.publish(drive)
def drive(self, speed, angle):
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = speed
drive_msg.steering_angle = angle
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.pub.publish(drive_msg_stamped)
def control_vehicle(speed, steering_angle):
publisher = rospy.Publisher('/carla/ego_vehicle/ackermann_cmd', AckermannDrive, queue_size=5)
rospy.init_node('vehicle_controller', anonymous=True)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
ackermann_cmd = AckermannDrive()
ackermann_cmd.steering_angle = steering_angle
ackermann_cmd.speed = speed
publisher.publish(ackermann_cmd)
rate.sleep()
def timerCallBack(self,event):
state=self.path[self.state]
steer_angle = -1.*state[3]
vel = self.EXT_VEL
newControl = AckermannDriveStamped()
newDrive = AckermannDrive()
newDrive.steering_angle = steer_angle
newDrive.speed = vel
self.newControl.drive = newDrive
self.state+=1
def callback(self, data):
#rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.buttons[self.enable_button])
if data.buttons[self.enable_button] is 1:
ackermann_msg = AckermannDrive()
if abs(data.axes[self.axis_linear]) > self.axis_dead_zone:
ackermann_msg.speed = data.axes[
self.axis_linear] * self.scale_linear #left up-down stick
else:
ackermann_msg.speed = 0.0
if abs(data.axes[self.axis_front_steering]) > self.axis_dead_zone:
ackermann_msg.steering_angle = data.axes[
self.
axis_front_steering] * self.scale_front_steering #right left-right stick
else:
ackermann_msg.steering_angle = 0.0
self.pub.publish(ackermann_msg)
def callback(data):
#rospy.loginfo(rospy.get_caller_id() + 'I heard %s', data.axes[0])
ack_msg = AckermannDrive()
#ack_msg.steering_angle=90+data.axes[0]*50
ack_msg.steering_angle=maprange( (-1, 1), (-100, 100), data.axes[0])
ack_publisher.publish(ack_msg)
steer_msg = JointState()
steer_msg.position = maprange( (-1, 1), (-100, 100), data.axes[0])
steer_publisher.publish(steer_msg)
def callback(data):
rospy.loginfo(rospy.get_caller_id() + 'I heard %s', data.axes[0])
ack_cmd = AckermannDriveStamped()
ack_cmd.header.stamp = rospy.Time.now()
drive = AckermannDrive()
drive.speed = (data.axes[5])
drive.steering_angle = (data.axes[0])
ack_cmd.drive = drive
ack_publisher.publish(ack_cmd)
def stop(self):
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = 0
drive_msg.steering_angle = 0
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.drive_pub.publish(drive_msg_stamped)
def joy_callback(self, joy):
global temp
data = AckermannDrive()
# x button pressed behaves as a deadman, switch R2 is forward L2 is reverse
# if x is engadged and L2 and not active
if joy.buttons[1] and not joy.buttons[6]:
if not joy.buttons[7]:
temp = 1
elif joy.buttons[7] and temp:
# if rising edge on R2 drive forward
# joy.axes[4] moves from 1 to -1
data.speed = self.MAX_FORWARD_VELOCITY*0.5*(-1*(joy.axes[4])+1)
# also send steering angle
# joy.axes[0] is left +1 to right -1
# right turn should correspond to a negative steering angle
# left turn should correspond to positive steering angle
data.steering_angle = joy.axes[0]*self.MAX_STEERING_ANGLE
self.ackermann_publisher.publish(data)
# if x is engadged and R2 and not active
elif joy.buttons[1] and not joy.buttons[7]:
if not joy.buttons[6]:
temp = 1
elif joy.buttons[6] and temp:
# if rising edge on L2 drive reverse
# joy.axes[3] from 1 to -1
data.speed = self.MAX_REVERSE_VELOCITY*0.5*(joy.axes[3]-1)
# also send steering angle
# joy.axes[0] is left +1 to right -1
# right turn should correspond to a negative steering angle
# left turn should correspond to a positivev steering angle
data.steering_angle = joy.axes[0]*self.MAX_STEERING_ANGLE
self.ackermann_publisher.publish(data)
# deadman switch activated (i think i was trying to make a state-machine)
elif (not joy.buttons[7] and temp) or not joy.buttons[1] or joy.buttons[6]:
temp = 0
def callback(self, data):
ack_cmd = AckermannDriveStamped()
ack_cmd.header.stamp = rospy.Time.now()
drive = AckermannDrive()
drive.speed = -1 * data.axes[5]
drive.steering_angle = data.axes[0]
# impose limits on commanded angle
if drive.steering_angle > self.max_steering:
drive.steering_angle = self.max_steering
if drive.steering_angle < self.min_steering:
drive.steering_angle = self.min_steering
# clean up angle if it is very close to zero
if math.fabs(drive.steering_angle) < self.epsilon_steering:
drive.steering_angle = 0.0
ack_cmd.drive = drive
self.publisher.publish(ack_cmd)
def driveTest():
rp.init_node("pleaseWork", anonymous=False)
pub = rp.Publisher("/vesc/ackermann_cmd_mux/input/navigation",
AckermannDriveStamped,
queue_size=10)
rate = rp.Rate(60)
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = 1.5
drive_msg.steering_angle = -1.0
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
i = 0
while True:
pub.publish(drive_msg_stamped)
rate.sleep()
drive_msg.steering_angle = math.sin(i)
i = i + 0.1
def apply_control(self, speed, steering_angle):
self.actual_speed = speed
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = speed
drive_msg.steering_angle = steering_angle
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.control_pub.publish(drive_msg_stamped)
def stop(self):
print "Stopping"
drive_msg_stamped = AckermannDriveStamped()
drive_msg_stamped.header = utils.make_header("/base_link")
drive_msg = AckermannDrive()
drive_msg.speed = 0
drive_msg.steering_angle = 0
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.control_pub.publish(drive_msg_stamped)
def control(self,data):
car_ctl = AckermannDrive()
car_msg = AckermannDriveStamped()
[centerX,centerY,width,height,actual_angle]=data.data
if actual_angle != -1 and width < 180 and height < 250:
desired_angle = 0
steering_gain=.4
steering_range = .34
error = desired_angle - actual_angle
steering_angle = steering_gain * error
car_ctl.speed = 0.3
car_ctl.steering_angle = steering_angle
car_msg.header.stamp = rospy.Time.now()
car_msg.header.frame_id = "IBSController"
car_msg.drive = car_ctl
self.control_pub.publish(car_msg)
else:
car_ctl.speed = 0.0
car_msg.header.frame_id = "IBSController"
car_msg.drive = car_ctl
self.control_pub.publish(car_msg)
