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 filter_command(data):
global flag_move
global flag_stop
pub_movement = rospy.Publisher('/vesc/ackermann_cmd_mux/input/navigation', AckermannDriveStamped, queue_size=1)
newData = AckermannDriveStamped()
newData.header = data.header
#print "flag is ", flag_stop
#if len(data.header.frame_id) > 0 or not flag_stop:
newData.drive = data.drive
pub_movement.publish(newData)
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 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)
#!/usr/bin/env python
import random
import rospy
from ackermann_msgs.msg import AckermannDriveStamped
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
drive_msg = AckermannDriveStamped()
drive_msg.header.seq = 0
drive_msg.header.stamp = rospy.Time()
drive_msg.header.frame_id = 'test'
drive_msg.drive.steering_angle = 0
drive_msg.drive.steering_angle_velocity = 1
drive_msg.drive.speed = 3
drive_msg.drive.acceleration = 0
drive_msg.drive.jerk = 0
pub = rospy.Publisher('/drive', AckermannDriveStamped, queue_size=10)
def driver_callback(data):
global s_angle
global car_state
# rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.ranges)
x = [min(xi, max_range) for xi in data.ranges]
lx = len(x)
N = 5
x_r = []
for i in range(N):
x_r[i] = np.average(x[int(lx * i / N):int(lx * (i + 1) / N)])
def MoveToBlob(self, blob_detect): #Moves to the largest blob.
global Wall_Follow, Turning
drive_msg = AckermannDriveStamped(
) #Sets the drive message to the AckermannDriveStamped message type
blob_Color = ColorRGBA()
drive_msg.drive.speed = self.speed #Sets a field of the drivemsg message.
drive_msg.drive.steering_angle = -.06 #Gets the position of the largest blob. The first will be the largest blob because the blobs are sorted by size. (index 0 is biggest size).
if (len(blob_detect.colors) == 0
): #Make sure the blob detect arrays are not empty
self.pub_mov_func.publish(drive_msg)
rospy.loginfo("No blobs detected")
return #exit
turning_start_time = rospy.get_time()
#if there are blobs
largest_blob_area = blob_detect.sizes[0].data
largest_blob_pos = blob_detect.locations[0]
blob_x_pos = largest_blob_pos.x #This will be in between 0 and 1 (inclusive) (0 is the left and 1 is the right)
blob_y_pos = largest_blob_pos.y #Same thing here
blob_color = blob_detect.colors[0] #In rgba
if (Wall_Follow == True and Turning
== False): #Stage 4. Stop subscriptions from this node.
rospy.loginfo(
"Stopping control system node and publishing to wall follower."
)
#Intiate wall follow by publishing the color to the wall follow's topic.
color = "r" if (blob_color.r == 255) else "g"
self.pub_blob_color_func(color)
#stop further subscriptions and basically end the node
self.blob_detect.unregister()
elif (Wall_Follow == False and Turning == False):
if (largest_blob_area > 8000
): #Stage 2. We are close to the blob, so we initiate turning
#PUT TURN HERE
rospy.loginfo("Intializing Turning.")
Turning = True
turning_start_time = rospy.get_time()
else: #Stage 1
#We are far from the blob, so we continue to navigate towards the blob.
#While we are relatively far away from the blob (ie blob size is small relative to screen), we drive towards the center of the blob.
rospy.loginfo("Navigating to blob")
if (blob_x_pos > 0.60): #If Blob is On the right side
drive_msg.drive.steering_angle = .3 #Turn left
elif (blob_x_pos < 0.40): #If Blob is on the left side.
drive_msg.drive.steering_angle = -.3 #Turn right
else: #If blob_x_pos (the x component of he blob's centroid) is in the middle 200 pixels, just move forward
drive_msg.drive.steering_angle = 0
if (Turning == True): #Stage 3. Start turning
rospy.loginfo("Turning")
if (turning_start_time + 4 > rospy.get_time()
): #Keeps turning for three seconds (approx)
if (blob_color.r == 255):
drive_msg.drive.steering_angle = 1.5
elif (blob_color.g == 255):
drive_msg.drive.steering_angle = -1.5
#If the blob color is red, turn left, else if it is green, turn right.
else:
Turning = False #After turning process finishes, change Turning to False and Wall_Follow to true
Wall_Follow = True
#publish the message
self.pub_mov_func.publish(drive_msg)
def pub_callback(self, event):
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 shutdown(self):
rospy.loginfo("Stopping the robot...")
# always make sure to leave the robot stopped
self.drive.publish(AckermannDriveStamped())
rospy.sleep(1)
if(msg.ranges[215] > msg.ranges[135]):
ackMsg.drive.steering_angle = -90
else:
ackMsg.drive.steering_angle = 90
ackMsg.drive.speed = 2
if(msg.ranges[170]<0.5 or msg.ranges[180]<0.5 or msg.ranges[190]<0.5):
ackMsg.drive.speed = -4
ackMsg.drive.steering_angle = 90
pub.publish(ackMsg)
if __name__ =='__main__':
ackMsg = AckermannDriveStamped()
rospy.init_node('proj', anonymous=True)
#subscribe to listener commands
sub = rospy.Subscriber('chatter',String,listenerCallback)
#subscribe to lidar
sub2 = rospy.Subscriber('/agent1/scan',LaserScan,wanderCallback)
#publish to hamster wheels(hamster movement control)
pub = rospy.Publisher('/agent1/ackermann_cmd',AckermannDriveStamped,queue_size=1)
while not rospy.is_shutdown():
rospy.spin()
def calculate_pure_pursuit(self):
L = 0.3302
# self.speed2 = self.speed # default: 2.0
# look-ahead distance ld based on the speed of the vehicle
# ld = self.coefficient * self.v + 0.5 # default: 1.0
ld = self.v * 0.15 + 0.1
diff_min = ld
pp = self.path_position
self.length2 = len(pp)
while self.length2 == 0:
pp = self.path_position
self.length2 = len(pp)
#print "duljina:", self.length2
self.velocity_controller(pp)
rob_x = self.robot_x
rob_y = self.robot_y
rob_qua_x = self.robot_qua_x
rob_qua_y = self.robot_qua_y
rob_qua_z = self.robot_qua_z
rob_qua_w = self.robot_qua_w
# find position(x,y) on the path that is ld away from the robot.
for i in range(0, self.length2):
path_x = pp[i][0]
path_y = pp[i][1]
path_base_link = self.calculate_pose(path_x, path_y)
if path_base_link.pose.position.x < -0.05:
continue
dist_rp = get_distance(rob_x, rob_y, path_x, path_y)
diff = abs(dist_rp - ld)
if diff < diff_min:
diff_min = diff
self.goal_x = path_x
self.goal_y = path_y
#show a point in Rviz on the Pure Pursuit goal position
goal = PointStamped()
goal.header.stamp = rospy.Time.now()
goal.header.frame_id = "/map"
goal.point.x = self.goal_x
goal.point.y = self.goal_y
goal.point.z = 0
self.pub2.publish(goal)
# calculate alpha1, alpha2 to get steering angle of the robot
# alpha1 - angle between robot and goal position in radians.
alpha1 = get_angle_rad(rob_x, self.goal_x, rob_y, self.goal_y)
quaternion = [rob_qua_x, rob_qua_y, rob_qua_z, rob_qua_w]
rpy = tf.transformations.euler_from_quaternion(quaternion)
# alpha2 - robots orientation with respect to map cordinate frame
alpha2 = rpy[2]
alpha = alpha1 - alpha2
self.delta = math.atan((2 * L * math.sin(alpha)) / ld)
# publish speed and steering angle of the robot
whereto = AckermannDriveStamped()
whereto.header.stamp = rospy.Time.now()
whereto.drive.steering_angle = self.delta
whereto.drive.steering_angle_velocity = 0
whereto.drive.speed = self.v
whereto.drive.acceleration = 0
whereto.drive.jerk = 0
self.pub.publish(whereto)
def drive(steer_val, car_run_speed):
global ack_publisher
ack_msg = AckermannDriveStamped()
ack_msg.drive.steering_angle = steer_val
ack_msg.drive.speed = car_run_speed
ack_publisher.publish(ack_msg)
def send_drive_command(self, speed, steering_angle):
ack_msg = AckermannDriveStamped()
ack_msg.drive.speed = speed
ack_msg.drive.steering_angle = steering_angle
self.ack_msg = ack_msg
def pursuit_callback(self, msg):
"""
given pose:
1) find closest line segment from trajectory
2) pure pursuit
3) ???
4) profit (set angle of ackermann message and publish)
"""
if not self.initialpose:
return
if not self.num_pts:
return
#get pose
np_pose_now = np.array([[msg.pose.pose.position.x, msg.pose.pose.position.y]])
theta_now = np.arctan2(msg.pose.pose.orientation.z, msg.pose.pose.orientation.w)*2
# now we have x,y,theta in world frame
seg, close_pt = self.get_closest_seg(np_pose_now)
self.prev_seg = seg
if self.do_rviz:
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.points = []
marker.colors = []
marker.scale.x = 0.5
marker.scale.y = 0.5
target_viz = Point()
target_viz.x = close_pt[0]
target_viz.y = close_pt[1]
target_viz.z = .05
marker.colors.append(ColorRGBA(r=0, g=0, b=1, a=1))
marker.points.append(target_viz)
self.closest_pub.publish(marker)
if not self.wrap and seg == self.num_pts-2:
self.speed = 0
self.stop = True
## FINDING TARGET POINT ##
## circle intersecting line segment stuff using quadratics ##
# everything so far in the world frame
def next_seg(seg):
return (seg+1)%self.num_pts
q = np_pose_now.reshape((2,))
# initial line seg to check bw
# p1 = pt.reshape((2,)) # dont check behind car
p1 = self.trajectory.np_points[seg]
p2 = self.trajectory.np_points[next_seg(seg)]
self.kdd = MAX_KDD
self.speed = MAX_SPEED
scale = ANGLE_SCALE_MAX
while not self.stop:
seg_len = np.sqrt(self.trajectory_dists[seg])
#if seg_len >= 2:
# self.kdd = 1.5
if seg_len <= MAX_SPEED*SMALL_SEG:
self.kdd = MIN_KDD
self.speed = SLOWDOWN_SPEED
scale = ANGLE_SCALE_MIN
self.lookahead = self.speed*self.kdd
vec = p2-p1
A = np.dot(vec, vec)
B = 2*(np.dot(vec, p1 - q))
C = np.dot(p1, p1) + np.dot(q,q) - np.dot(2*p1,q) - self.lookahead**2
disc = B**2 - 4*A*C
while disc <= 0:
# no intersection
# extend lookahead distance
disc = self.adjust_kdd(A, B, p1, q)
#rospy.loginfo("Point: " + str(p1) + str(p2))
#rospy.loginfo(str(disc))
t1 = (-B + np.sqrt(disc))/(2*A)
t2 = (-B - np.sqrt(disc))/(2*A)
#rospy.loginfo("Found expanded disc")
while t1 <= 0:
# if segment extended enough backwards it would touch, so we shouldn't move to
# next segment, increase lookahead again
disc = self.adjust_kdd(A, B, p1, q)
t1 = (-B + np.sqrt(disc))/(2* A)
t2 = (-B - np.sqrt(disc))/(2*A)
if t1 >= 1:
# also no intersection, but bc segment is not long enough
# here we should probably do the moving on to next line segment
# start point of next segment is endpoint of this segment
seg = (seg+1)%self.num_pts
p1 = p2
p2 = self.trajectory.np_points[next_seg(seg)]
continue
#else:
# we have intersection! yay
target_pos = p1 + t1*vec # position on line segment if 0<=t1<=1
target_theta = np.arctan2(p1[1]-p2[1], p1[0]-p2[0]) # direction of line segment
break
if not self.stop:
# convert to robot frame
target_coord = get_robot_frame_coord((target_pos, target_theta), (np_pose_now, theta_now))
target_coord = self.adjust_target_coord_for_obstacles(target_coord, STEERING_ANGLE)
if self.do_rviz:
marker = Marker()
marker.header.frame_id = "/base_link"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.points = []
marker.colors = []
marker.scale.x = 0.5
marker.scale.y = 0.5
target_viz = Point()
target_viz.x = target_coord[0]
target_viz.y = target_coord[1]
target_viz.z = .05
marker.colors.append(ColorRGBA(r=1, g=0, b=0, a=1))
marker.points.append(target_viz)
self.nearest_pub.publish(marker)
rospy.loginfo("TARGET: " + str(target_coord))
target_th = np.arctan2(target_coord[1],target_coord[0])
if abs(target_th) > SHARP_TURN:
self.kdd = MIN_KDD
self.speed = SLOWDOWN_SPEED
scale = ANGLE_SCALE_MIN
# find turning radius using method in that paper linked in piazza
R_track = self.lookahead**2/(2*abs(target_coord[1]))
# now get desired turning angle!
delta = np.sign(target_coord[1])*np.arctan2(self.wheelbase_length, R_track)
if self.stop:
delta = 0
drive_msg = AckermannDriveStamped()
drive_msg.header.stamp = msg.header.stamp
drive_msg.drive.speed = self.speed
drive_msg.drive.steering_angle = -1*scale*delta #NEEDS TO BE SET
self.drive_pub.publish(drive_msg)
def drive(self):
new_msg = AckermannDriveStamped()
new_msg.drive.speed = self.speed
new_msg.drive.steering_angle = self.steering_angle
self.pub.publish(new_msg)
def drive_cb(event):
global init_flag, waypoint
if not init_flag:
x = pos[0]
y = pos[1]
path.append((x,y))
init_flag = True
return
x = pos[0]
y = pos[1]
path.append((x,y))
curr_loc = path[-1]
prev_loc = path[-2]
next_loc = get_next_loc(curr_loc, prev_loc)
#d_target = get_distance(waypoint, curr_loc)
a_error = get_angle_between_3_pts(curr_loc, waypoint, next_loc)
if a_error > math.pi:
a_error = a_error - 2*math.pi
elif a_error < -math.pi:
a_error = a_error + 2*math.pi
d_target = get_distance(waypoint, curr_loc)
#print(d_target, reached[0])
if d_target < EPSILON_RADIUS and single_ack_entry_flag[0]:
#print ("WAYPOINT REACHED, d_target = {}".format(d_target))
#waypoint = points.pop(0)
reached[0] = True
a_integral = 0.0
d_integral = 0.0
a_prev = 0.0
d_prev = 0.0
#continue
#print("STOPPING")
#stop()
stop_cmd = True;
print("ACK SENT")
ack_msg = String(data="TRUE")
ack_publisher.publish(ack_msg)
single_ack_entry_flag[0] = False
#stop()
#continue
if stop_cmd:
print("STOPPING")
speed = STOP
stop_cmd = False;
msg = AckermannDriveStamped()
msg.drive.speed = speed
msg.drive.steering_angle = 0
pub.publish(msg)
if single_ack_entry_flag[0]:
print("Driving")
d_prev = d_target
d_integral += d_target
a_prev = a_error
a_integral += a_error
if a_error < -EPSILON_ANGLE:
kLeft = 0
kRight = 1
elif a_error > EPSILON_ANGLE:
kRight = 0
kLeft = 1
else:
kLeft = 0
kRight = 0
x_prev = x
y_prev = y
msg = AckermannDriveStamped()
# reset speed & direction accordingly.
speed += DELTA_SPEED_DRIVE * (kUp - kDown)
direction += -DELTA_DIRECTION * (kRight - kLeft)
speed = max(min(speed, 2), -2)
direction = max(min(direction, 0.35), -0.35)
msg.drive.speed = speed
msg.drive.steering_angle = direction
# print(kUp, kDown, kLeft, kRight)
#print(speed, direction)
# sys.stdout.flush()
pub.publish(msg)
def stop():
#print("STOPPING")
msg = AckermannDriveStamped()
msg.drive.speed = STOP
msg.drive.steering_angle = 0
pub.publish(msg)
def JoyCB(data):
#Use global/static vars
global controllerInitializationStarted
global controllerInitialized
global lastSteeringState
global steeringState
global lastThrottleState
global throttleState
global lastFineSteeringState
global fineSteeringState
#steering direction
if (data.buttons[2] != lastSteeringState and lastSteeringState == 0):
steeringState = -steeringState
rospy.logdebug("Changing steering direction")
lastSteeringState = data.buttons[2]
#driving mode
if (data.buttons[0] != lastThrottleState and lastThrottleState == 0):
throttleState = not throttleState
rospy.logdebug("Changing driving mode")
lastThrottleState = data.buttons[0]
#fine steering mode
if (data.buttons[3] != lastFineSteeringState
and lastFineSteeringState == 0):
fineSteeringState = not fineSteeringState
rospy.logdebug("Changing steering mode")
lastFineSteeringState = data.buttons[3]
#apply modificaitons to throttle based on drive mode selection
#acceptible values for physical jetson car are +-5 (not in m/s) for now
if not controllerInitialized:
throttleVal = 0
if (controllerInitializationStarted and data.axes[2] == 1
and data.axes[5] == 1): #controller should be corrected by now
controllerInitialized = True
elif (data.axes[2] == -1 and data.axes[5] == -1):
controllerInitializationStarted = True
elif throttleState:
throttleVal = map(data.axes[5], 1, -1, 0, 2) + map(
data.axes[2], 1, -1, 0, 2) #range is 0 to 4
else:
throttleVal = map(data.axes[5], 1, -1, 0, 2) - map(
data.axes[2], 1, -1, 0, 2) #range is -2 to 2
# #handle braking if applied (doesn't correlate to anything in simulation)
# if(data.buttons[4] == 1 or data.buttons[5] == 1):
# throttleVal = 6 #out of range value triggers active brake in ARC1 firmware
#apply modifications to steering based on steering mode selection
if fineSteeringState:
steeringVal = data.axes[0] #range is -1 to 1
else:
steeringVal = data.axes[0] / 2 #range is -0.5 to 0.5
msg = AckermannDriveStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "base_link"
msg.drive.acceleration = 0
#drive acceleration unknown
msg.drive.speed = throttleVal
msg.drive.jerk = 0
#jerk unknown
msg.drive.steering_angle = steeringVal * steeringState #steering state is 1 or -1 to flip steering if needed
msg.drive.steering_angle_velocity = 0 #angle velocity unknown
pub.publish(msg)
def perform_retrace(waypoint, i):
print waypoint, ODOM_INF
global pix_bot_center, pix_bot_theta, pix_bot_velocity, cmd_pub
rospy.wait_for_message(ODOM_INF, Odometry, 5)
dx = waypoint[0] - pix_bot_center.position.x
dy = waypoint[1] - pix_bot_center.position.y
target_distance = math.sqrt(dx*dx + dy*dy)
cmd = AckermannDriveStamped()
cmd.header.stamp = rospy.Time.now()
cmd.header.frame_id = "base_link"
cmd.drive.speed = pix_bot_velocity.linear.x
cmd.drive.acceleration = max_acc[i]
delta_error = 0.0
last_error = 0.0
while target_distance > retrace_waypoint_tol:
dx = pix_bot_center.position.x - waypoint[0]
dy = pix_bot_center.position.y - waypoint[1]
lookahead_dist = np.sqrt(dx * dx + dy * dy)
lookahead_theta = math.atan2(abs(dy), abs(dx))
# lookahead_theta = waypoint[2]
alpha = shortest_angular_distance(lookahead_theta, pix_bot_theta)
cmd.header.stamp = rospy.Time.now()
cmd.header.frame_id = "base_link"
# Publishing constant speed of 1m/s
cmd.drive.speed = 1
# Reactive steering
# print(pix_bot_theta, lookahead_theta )
# print("ALPHA")
# print(alpha)
if alpha < 0:
st_ang = max(-max_steering_angle_ret, alpha)
else:
st_ang = min(max_steering_angle_ret, alpha)
cmd.drive.steering_angle = st_ang
target_distance = math.sqrt(dx * dx + dy * dy)
error_speed = target_distance
if last_error == 0:
pass
else:
delta_error = error_speed - last_error
velocity = Kp * error_speed + Kd * delta_error
MIN_VEL = 0.1
MAX_VEL = 0.75
velocity = max(MIN_VEL, min(MAX_VEL , velocity))
cmd.drive.speed = -velocity
cmd_pub.publish(cmd)
rospy.wait_for_message(ODOM_INF, Odometry, 5)
def lidar_callback(data):
global prev_threshold_angle, integral_prior
gauss_range = np.arange(0, 1080, 1)
gaussian_weights = gaussian(gauss_range, 540, 150)
gaussian_weights = gaussian_weights + (1 - gaussian_weights[0])
#limited_ranges = limited_ranges*gaussian_weights
limited_ranges = np.asarray(data.ranges)
limited_ranges[0:right_extreme] = 0.0
limited_ranges[(left_extreme + 1):] = 0.0
limited_ranges[(left_extreme + 1)] = limited_ranges[left_extreme]
indices = np.where(limited_ranges >= lidar_max_range)[0]
limited_ranges[indices] = lidar_max_range - 0.1 #data.range_m - 0.1
disparities = find_disparities(limited_ranges, disparity_threshold)
# Experimental section
right = []
left = []
front = []
# Handling Multiple Disparities
a = 14
for i in disparities:
if i > 540 - a and i < 540 + a:
front.append(i)
elif i < 540:
right.append(i)
#print(right)
else:
left.append(i)
if len(right) > len(left) and len(right) > len(front):
disparities = right
elif len(left) > len(right) and len(left) > len(front):
disparities = left
else:
disparities = front
new_ranges = extend_disparities(limited_ranges, disparities, car_width)
new_ranges = new_ranges * gaussian_weights
max_value = max(new_ranges)
target_distances = np.where(
new_ranges >= (max_value - 1))[0] #index values
driving_distance_index, chosen_index = calculate_target_distance(
target_distances)
driving_angle = calculate_angle(driving_distance_index)
thresholded_angle = threshold_angle(driving_angle)
behind_car = np.asarray(data.ranges)
behind_car_right = behind_car[0:right_extreme]
behind_car_left = behind_car[(left_extreme + 1):]
#change the steering angle based on whether we are safe
thresholded_angle = adjust_turning_for_safety(behind_car_left,
behind_car_right,
thresholded_angle)
velocity = calculate_min_turning_radius(thresholded_angle)
velocity = threshold_speed(velocity, new_ranges[driving_distance_index],
new_ranges[540])
# Publish drive and Steer
msg = AckermannDriveStamped()
Kd = 0.02 #0.055
Ki = 0.075
Kp = 0.75
integral = integral_prior + (thresholded_angle * 0.004)
msg.drive.steering_angle = Kp * thresholded_angle + (
Kd *
(thresholded_angle - prev_threshold_angle) / 0.004) + Ki * integral
prev_threshold_angle = thresholded_angle
integral_prior = integral
msg.drive.speed = velocity
pub_drive_param.publish(msg)
current_time = rospy.Time.now()
# Publish Modified Scan
scan = LaserScan()
scan.header.stamp = current_time
scan.header.frame_id = 'ego_racecar/laser'
scan.angle_min = -1.57
scan.angle_max = 1.57
scan.angle_increment = 3.14 / 1080
scan.time_increment = (1.0 / 250) / (1080)
scan.range_min = 0.0
scan.range_max = 270.0
scan.ranges = new_ranges
scan.intensities = []
scan_pub.publish(scan)
# Publish marker
markermsg = Marker()
markermsg.header.frame_id = "ego_racecar/laser"
markermsg.header.stamp = current_time
markermsg.id = 0
markermsg.type = 2
markermsg.action = 0
markermsg.pose.position.x = np.sin(math.pi - np.deg2rad((
(driving_distance_index - 180) /
4))) * new_ranges[driving_distance_index]
markermsg.pose.position.y = np.cos(math.pi - np.deg2rad((
(driving_distance_index - 180) /
4))) * new_ranges[driving_distance_index]
markermsg.pose.position.z = 0
markermsg.scale.x = 0.5
markermsg.scale.y = 0.5
markermsg.scale.z = 0.5
markermsg.pose.orientation.x = 0
markermsg.pose.orientation.y = 0
markermsg.pose.orientation.z = 0
markermsg.pose.orientation.w = 1.0
markermsg.color.r = 0.0
markermsg.color.g = 1.0
markermsg.color.b = 0.0
markermsg.color.a = 1.0
markermsg.lifetime = rospy.Duration()
marker_pub.publish(markermsg)
"""Scale the speed in accordance to the forward distance"""
def generate_drive_stamp(self, steering):
stamp = AckermannDriveStamped()
stamp.drive.steering_angle = steering
stamp.drive.speed = -2.
rospy.logwarn("steering: %s", steering)
return stamp
import sys from ackermann_msgs.msg import AckermannDrive from ackermann_msgs.msg import AckermannDriveStamped __author__ = "Ilyas M Abbas ([email protected])" if __name__ == '__main__': if len(sys.argv) != 3: print sys.argv sys.exit(1) rospy.init_node('ackermann_test_publisher', anonymous=True) pub = rospy.Publisher('auto_cmd', AckermannDriveStamped, queue_size=1) ack_msg = AckermannDriveStamped() ack_msg.drive = AckermannDrive() ack_msg.drive.speed = float(sys.argv[1]) ack_msg.drive.steering_angle = float(sys.argv[2]) then = rospy.Time.now() r = rospy.Rate(10) while not rospy.is_shutdown(): if ((rospy.Time.now() - then).to_sec() > 10 and (rospy.Time.now() - then).to_sec() < 14): ack_msg.drive.speed = 0 ack_msg.drive.steering_angle = 0 pub.publish(ack_msg) r.sleep()
#!/usr/bin/env python3
import rospy, math, serial
import time
import os
from collections import deque
# ROS Msgs
from std_msgs.msg import Int16
from ackermann_msgs.msg import AckermannDriveStamped
steering_pub = rospy.Publisher('pot_angle', AckermannDriveStamped, queue_size=10)
steering_msg = AckermannDriveStamped()
#REGION SerialInit.
command ='udevadm info -e'
p=os.popen(command,"r")
data = p.read()
lines= data.split('\n\n')
default_port='/dev/ttyACM9'
for i in range(len(lines)):
lines[i].replace("P","\nP")
# if 'Uno' in lines[i] and '855313036' in lines[i]:
if 'Uno' in lines[i] and 'dev/ttyACM' in lines[i]:
start = lines[i].find('/dev/ttyACM')
if (start != -1):
default_port = lines[i][start:start+12]
print("Arduino port was found after search: ", default_port, "Srart was: ", start)
ard = serial.Serial(default_port,9600,timeout=0.0001)
ard.flushInput()
#ENDREGION SerialInit.
def send_action(self, steering_angle, throttle):
ack_msg = AckermannDriveStamped()
ack_msg.header.stamp = rospy.Time.now()
ack_msg.drive.steering_angle = steering_angle
ack_msg.drive.speed = throttle
self.ack_publisher.publish(ack_msg)
rospy.Subscriber('/sim_car_pose/pose', PoseStamped, cb_heading)
# Publish initial position
init_pose = PoseWithCovarianceStamped()
init_pose.header.stamp = rospy.Time.now()
init_pose.header.frame_id = 'map'
init_pose.pose.pose.position.x = 0.0
init_pose.pose.pose.position.y = 0.0
init_pose.pose.pose.orientation = utils.angle_to_quaternion(0.0)
rospy.sleep(1) # publisher needs some time before it can publish
rospy.loginfo('Publish initialpose')
p_initpose.publish(init_pose)
rospy.sleep(1)
# Publish drive info
cmd = AckermannDriveStamped()
cmd.header.frame_id = 'map'
cmd.drive.steering_angle = 0.1
cmd.drive.speed = 2
r = rospy.Rate(20)
while not rospy.is_shutdown():
if heading < -0.5 and circle == 0:
circle = 1
elif heading > -0.1 and circle == 1:
cmd.drive.steering_angle = -cmd.drive.steering_angle
circle = 2
elif heading > 0.5 and circle == 2:
circle = 3
elif heading < 0.1 and circle == 3:
break
p_nav0.publish(cmd)
