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.drive_pub.publish(drive_msg_stamped)
rospy.sleep(1.0/PUBLISH_RATE)
def apply_control(self, speed, steering_angle):
self.actual_speed = speed
drive_msg_stamped = AckermannDriveStamped()
drive_msg_stamped.header = utils.make_header("/base_link")
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 callback(data):
speed = data.linear.x
turn = data.angular.z
msg = AckermannDrive()
msg.speed = speed
msg.acceleration = 1
msg.jerk = 1
msg.steering_angle = turn
msg.steering_angle_velocity = 1
pub.publish(msg)
def rearWheelFeedback(self, currentPose, targetPose):
#Gain Values
k1 = 0.2
k2 = 2
k3 = 1
#give targetVel and targetAngVel
# targetVel = math.sqrt((targetPose.twist.linear.x*targetPose.twist.linear.x) + ((targetPose.twist.linear.y*targetPose.twist.linear.y)))
targetVel = 2
targetAngVel = targetPose.twist.angular.z
#print (targetVel, targetAngVel)
currentEuler = self.quaternion_to_euler(currentPose.pose.orientation.x,
currentPose.pose.orientation.y,
currentPose.pose.orientation.z,
currentPose.pose.orientation.w)
targetEuler = self.quaternion_to_euler(targetPose.pose.orientation.x,
targetPose.pose.orientation.y,
targetPose.pose.orientation.z,
targetPose.pose.orientation.w)
#compute errors
xError = (
(targetPose.pose.position.x - currentPose.pose.position.x) *
math.cos(currentEuler[2])) + (
(targetPose.pose.position.y - currentPose.pose.position.y) *
math.sin(currentEuler[2]))
yError = (
(targetPose.pose.position.x - currentPose.pose.position.x) *
math.sin(currentEuler[2]) * -1) + (
(targetPose.pose.position.y - currentPose.pose.position.y) *
math.cos(currentEuler[2]))
thetaError = targetEuler[2] - currentEuler[2]
#print("Error:", xError, yError, thetaError)
#give blank ackermannCmd
newAckermannCmd = AckermannDrive()
# if (targetVel * math.cos(thetaError)) + (k1 * xError) <= targetVel:
newAckermannCmd.speed = (targetVel * math.cos(thetaError)) + (k1 *
xError)
# else:
# newAckermannCmd.speed = targetVel
newAckermannCmd.steering_angle_velocity = (targetAngVel) + (
(targetVel) * ((k2 * yError) + (k3 * math.sin(thetaError))))
#print(newAckermannCmd)
return newAckermannCmd
def drive_straight(self):
while not rospy.is_shutdown():
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = 0.5
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)
# don't spin too fast
rospy.sleep(1.0/PUBLISH_RATE)
def drive(self, tags_tan):
turning_factor = self.get_turning_factor(tags_tan)
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = MAX_SPEED - abs(turning_factor / 2)
drive_msg.steering_angle = self.get_steering_angle(turning_factor)
drive_msg.acceleration = 0.0
drive_msg.jerk = 0.0
drive_msg.steering_angle_velocity = 0.1
drive_msg_stamped.drive = drive_msg
self.previous_steering_angle = drive_msg.steering_angle
self.pub.publish(drive_msg_stamped)
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 = 0.8
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
while True:
pub.publish(drive_msg_stamped)
rate.sleep()
def drive(self):
while not rospy.is_shutdown():
if self.received_data is None or self.parsed_data is None:
rospy.sleep(0.5)
continue
if np.any(self.parsed_data['front'][:, 0] < MIN_FRONT_DIST):
rospy.loginfo("stoping!")
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(.1)
def path_tracking(self, event):
if self.path and self.state:
if self.path.x_states[0] == -100: # shutdown procedure
self.stop_vehicle()
else:
cx = self.path.x_states
cy = self.path.y_states
cyaw = self.path.yaw_states
# for i in range(len(cyaw)):
# cyaw[i] -= math.radians(270)
#self.target_index = pure_pursuit.calculate_target_index(self.state, cx, cy)
target_index, mind = stanley_controller.calc_target_index(
self.state, cx, cy)
ai = pure_pursuit.pid_control(self.target_speed, self.state.v)
#di, self.target_index = pure_pursuit.pure_pursuit_control(self.state,
# cx,
# cy,
# self.target_index)
di, target_index = stanley_controller.stanley_control(
self.state, cx, cy, cyaw, target_index)
print(self.state.v, di)
self.state.v = self.state.v + ai * self.dt
if di > self.max_steering_angle:
di = self.max_steering_angle
if di < -self.max_steering_angle:
di = -self.max_steering_angle
if self.state.v > self.target_speed:
self.state.v = self.target_speed
rospy.loginfo("path_tracking_node: v: " + str(self.state.v) +
" steering: " + str(di))
msg = AckermannDrive()
msg.speed = 0.50
msg.acceleration = 0.5
msg.jerk = 1.0
msg.steering_angle = di
msg.steering_angle_velocity = 1
self.pub_ackermann_cmd.publish(msg)
def drive(self):
while not rospy.is_shutdown():
if self.received_data is None or self.parsed_data is None:
rospy.sleep(0.5)
continue
if np.any(self.parsed_data['front'][:, 0] < MIN_FRONT_DIST):
rospy.loginfo("stoping!")
# this is overkill to specify the message, but it doesn't hurt
# to be overly explicit
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)
# don't spin too fast
rospy.sleep(.1)
def __init__(self):
self.pub = rospy.Publisher("ackermann_cmd",\
AckermannDriveStamped, queue_size =1 )
while not rospy.is_shutdown():
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
header_msg = Header()
drive_msg.speed = 1
drive_msg.steering_angle = 0.1
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
header_msg.stamp = rospy.Time.now()
header_msg.frame_id = '1'
drive_msg_stamped.header = header_msg
pub = rospy.Publisher('/ackermann_cmd',
AckermannDriveStamped,
queue_size=1)
pub.publish(drive_msg_stamped)
def apply_control(self):
while not rospy.is_shutdown():
if self.control is None:
print "No control data"
rospy.sleep(0.5)
else:
self.steering_hist.append(self.control[0])
# smoothed_steering = self.steering_hist.mean()
smoothed_steering = self.steering_hist.median()
# print smoothed_steering, self.control[0]
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = self.control[1]
drive_msg.steering_angle = smoothed_steering
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(1.0/PUBLISH_RATE)
def drive(self):
while not rospy.is_shutdown():
if self.received_data is None or self.parsed_data is None:
rospy.sleep(0.5)
continue
if np.any(self.parsed_data['front'][:, 0] < MIN_FRONT_DIST):
rospy.loginfo("stoping!")
msg = String()
msg.data = 'stop_event'
self.failsafe_pub.publish(msg)
# just in case
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.pub.publish(drive_msg_stamped)
# don't spin too fast
rospy.sleep(.1)
def talker():
angle_new = 0.5
angle_old = 0.4
acceleration = 0.5
velocity = 0
deltat = 0.1
pub = rospy.Publisher('/carla/ego_vehicle/ackermann_cmd', AckermannDrive, queue_size=10)
rospy.init_node('CAV_talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
angle_vel = 2
speed = 10
jerk = 1
while not rospy.is_shutdown():
# control_cmd = "{steering_angle: %f, steering_angle_velocity: %f, speed: %f, acceleration: %f, jerk: 0.0}" % (angle_new,angle_vel,speed,acceleration)
#control_cmd = ['%f'%(angle_new),'%f'%(angle_vel),'%f'%(speed),'%f'%(acceleration),'%f'%(jerk)]
control_cmd = AckermannDrive()
control_cmd.steering_angle = angle_new
control_cmd.steering_angle_velocity = angle_vel
control_cmd.speed = speed
control_cmd.acceleration = acceleration
control_cmd.jerk = jerk
rospy.loginfo(control_cmd)
pub.publish(control_cmd)
rate.sleep()
def drive(self):
while not rospy.is_shutdown():
if self.received_data is None or self.parsed_data is None:
rospy.sleep(0.5)
continue
print "front", self.range_hist_front.mean()#, self.parsed_data["front"][:,0]
print "left", self.range_hist_left.mean()#, self.parsed_data["left"][:,0]
print "right", self.range_hist_right.mean()#, self.parsed_data["right"][:,0]
if (np.any(self.parsed_data['front'][:,0] < MIN_FRONT_DIST) or (not self.status)):
#print "stop!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
rospy.loginfo("stoping!")
# this is overkill to specify the message, but it doesn't hurt
# to be overly explicit
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
self.steer = 0.0
if(self.right_is_long):
self.steer = LEFT_STEER
else:
self.steer = RIGHT_STEER
drive_msg.steering_angle = self.steer
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
#if GEAR:
# print "GEAR>>>>>"
# for i in range(390, 500):
# print "BACK>>>>>>>>>"
# drive_msg.speed = 403
# self.pub.publish(drive_msg_stamped)
GEAR = 0
drive_msg.speed = BACKWARD_SPEED
self.pub.publish(drive_msg_stamped)
rospy.sleep(.4)
elif np.any(self.parsed_data['front'][:,0] < CHANGE_FRON_DIST):
rospy.loginfo("change!")
GEAR = 1
if np.sum(self.range_hist_left.mean() > self.range_hist_right.mean()):
self.right_is_long = False
self.left_is_long = True
#print "turn left"
#print np.sum(self.parsed_data['left'][:,0]), np.sum(self.parsed_data['right'][:,0])
#print self.parsed_data['right'][:,0]
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = DEFAULT_SPEED
#PID CONTROL
ERROR_CURRENT = LEFT_STEER - self.steer # 0 - 260
E_DIFF = ERROR_CURRENT - self.error_last
self.error_last = ERROR_CURRENT
result = KP*ERROR_CURRENT - KD*E_DIFF
self.steer = result
#print "steer : ", self.steer
#print "error_last : ", self.error_last
#print "error_current : ", ERROR_CURRENT
#print "e_diff : ", E_DIFF
#print "error_last : ", self.error_last
print "turn left PID result>>>> ", result
#self.steering_hist.append(result)
self.steer = result
drive_msg.steering_angle = self.steer
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)
else:
self.right_is_long = True
self.left_is_long = False
#print "turn right"
#print np.sum(self.parsed_data['left'][:,0]), np.sum(self.parsed_data['right'][:,0])
#print self.parsed_data['right'][:,0]
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = DEFAULT_SPEED
#PID CONTROL
ERROR_CURRENT = RIGHT_STEER - self.steer
E_DIFF = ERROR_CURRENT - self.error_last
self.error_last = ERROR_CURRENT
result = KP*ERROR_CURRENT - KD*E_DIFF
#print "steer : ", self.steer
#print "error_current : ", ERROR_CURRENT
#print "e_diff : ", E_DIFF
#print "error_last : ", self.error_last
print "turn right PID result>>>> ", result
self.steer = result
drive_msg.steering_angle = self.steer
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)
else:
#print "drive~!!>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"
self.right_is_long = False
self.left_is_long = False
GEAR = 1
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = DEFAULT_SPEED
#PID CONTROL
ERROR_CURRENT = FRONT_STEER - self.steer
E_DIFF = ERROR_CURRENT - self.error_last
self.error_last = ERROR_CURRENT
result = KP*ERROR_CURRENT - KD*E_DIFF
#print "steer : ", self.steer
#print "error_current : ", ERROR_CURRENT
#print "e_diff : ", E_DIFF
#print "error_last : ", self.error_last
print "go straight PID result>>>> ", result
self.steer = result
drive_msg.steering_angle = self.steer
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)
# don't spin too fast
rospy.sleep(.1)
import numpy as np
import cv2
import random
from sensor_msgs.msg import LaserScan, Image
from cv_bridge import CvBridge, CvBridgeError
from ackermann_msgs.msg import AckermannDriveStamped, AckermannDrive
bridge = CvBridge()
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = 0.4
drive_msg.steering_angle = -1.0
drive_msg.acceleration = 1
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
global proportional_error
global integral_error
global derivative_error
global active_lidar_in
global inactive_lidar_in
global left_lidar_pts
global right_lidar_pts
global previous_error
def callback2(data):
global prepSwitch
global direction
def timer_cb(self, event):
cmd_msg = AckermannDrive()
current_time = rospy.get_time()
# delta_t = current_time - self.time
delta_t = self.time_step
# delta_t = 0.05
self.time = current_time
jerk = 2.0
# cmd_msg.header.stamp = rospy.Time.now()
cmd_msg.steering_angle = 0.0
cmd_msg.steering_angle_velocity = 0.0
cmd_msg.speed = 0.0
cmd_msg.acceleration = 0.0
cmd_msg.jerk = jerk
if self.pathReady and self.stateReady:
pos_x, pos_y = self.state.get_position()
# pick target w/o collision avoidance, closest point on the traj and one point ahead
_, idx = self.path_tree.query([pos_x, pos_y])
# Target point for steering
if idx < self.traj_steps - 3:
target_pt = self.path_tree.data[idx + 3, :]
# str_idx = idx + 2
else:
target_pt = self.path_tree.data[-1, :]
# str_idx = self.vel_path.shape[0] - 1
print("CONTROLLER: at the end of the desired waypoits!!!")
# Target point for velocity
if idx < self.traj_steps:
target_vel = self.vel_path[idx, :]
# str_idx = idx + 2
else:
target_vel = self.vel_path[-1, :]
# str_idx = self.vel_path.shape[0] - 1
target_speed = np.linalg.norm(target_vel)
speed_diff = target_speed - self.state.get_speed()
# pos_diff = target_pt - self.state.get_position()
# acceleration = cmd_msg.acceleration + 2.0*(
# pos_diff/self.time_step**2.0 - speed_diff/self.time_step
# )
acceleration = abs(speed_diff) / (2.0 * delta_t)
cmd_msg.acceleration = np.min([1.5, acceleration])
steer = self.compute_ackermann_steer(target_pt)
steer_diff = abs(steer - self.steer_prev)
if steer_diff >= 0.3:
print(" 0.3")
steer = self.steer_prev
elif steer_diff >= 0.05:
print(" 0.05")
acceleration = 0.0
target_speed = target_speed / 5.0
self.steer_prev = steer
if self.state.get_speed() - target_speed > 0.0:
# cmd_msg.acceleration = acceleration - 5
acceleration = -100.0
cmd_msg.speed = target_speed / 5.0
jerk = 1000.0
elif target_speed - self.state.get_speed() > 0.2:
acceleration = np.min([3.0, acceleration])
else:
acceleration = 0.0
cmd_msg.steering_angle = steer
cmd_msg.speed = target_speed
cmd_msg.acceleration
cmd_msg.jerk = jerk
# Print out some debugging information
if abs(target_speed - self.max_speed) > 2.0:
# print("Posit diff:", pos_diff)
print("Speed diff:", speed_diff)
print("Current speed:", self.state.get_speed())
print("Desired speed:", target_speed)
print("Desired accel:", acceleration)
print("Desired jerk:", jerk)
print("Speed (sent):", cmd_msg.speed)
print("Accel (sent):", cmd_msg.acceleration)
print("Jerk (sent):", cmd_msg.jerk)
print("delta t:", delta_t)
# for visualization purposes and debuging control node
mk_msg = Marker()
mk_msg.header.stamp = rospy.Time.now()
mk_msg.header.frame_id = 'map'
mk_msg.pose.position.x = target_pt[0]
mk_msg.pose.position.y = target_pt[1]
mk_msg.type = Marker.CUBE
mk_msg.scale.x = 3
mk_msg.scale.y = 3
mk_msg.scale.z = 3
mk_msg.color.a = 1.0
mk_msg.color.r = 1
mk_msg.color.b = 1
self.tracking_pt_viz_pub.publish(mk_msg)
# self.vehicle_cmd_pub.publish(vehicle_cmd_msg)
self.command_pub.publish(cmd_msg)
def convert_to_ackermann(self, joy_msg):
axes = joy_msg.axes
buttons = joy_msg.buttons
if buttons[7] == 1: # button a clicked
self.start_autonomous = not self.start_autonomous
bool_msg = Bool()
bool_msg = self.start_autonomous
self.pub_start_autonomous.publish(bool_msg)
if self.start_autonomous:
rospy.loginfo("joy_to_ackermann: start autonomous mode")
self.timer.shutdown()
else:
rospy.loginfo("joy_to_ackermann: start manual mode")
self.timer = rospy.Timer(rospy.Duration(0.1), self.publish_cmd)
if buttons[3] == 1: # button y clicked
self.front_camera = not self.front_camera
bool_msg = Bool()
bool_msg = self.front_camera
self.pub_switch_cameras.publish(bool_msg)
if axes[-1] == 1: # up on the D pad
self.max_motor_vel += 0.05
rospy.loginfo(
"joy_to_ackermann: max motor velocity increased to: " +
str(self.max_motor_vel))
msg = Float32()
msg.data = self.max_motor_vel
self.pub_max_vel.publish(msg)
elif axes[-1] == -1: # down on the D pad
self.max_motor_vel -= 0.05
rospy.loginfo(
"joy_to_ackermann: max motor velocity increased to: " +
str(self.max_motor_vel))
msg = Float32()
msg.data = self.max_motor_vel
self.pub_max_vel.publish(msg)
if buttons[2] == 1: # button x clicked
motor_velocity = 0
steering_angle = 0
else:
steering_axis = axes[0]
reverse_trigger = axes[2]
gas_trigger = axes[5]
# convert to speed
if gas_trigger == 1 and reverse_trigger == 1:
motor_velocity = 0
elif reverse_trigger != 1:
motor_velocity = -1 * abs(reverse_trigger * self.max_motor_vel)
else:
motor_velocity = abs(gas_trigger * self.max_motor_vel)
# convert to steering angle and reverse it since
# the xbox controller returns right in the negatives
steering_angle = steering_axis * self.max_steering_angle
steering_angle *= -1
msg = AckermannDrive()
msg.speed = motor_velocity
msg.acceleration = 1
msg.jerk = 1
msg.steering_angle = -1 * steering_angle
msg.steering_angle_velocity = 1
self.current_cmd_msg = msg
print(msg)
import math
import numpy as np
from PCANBasic import *
from e2o.msg import e2o_ctrl, e2o_status
from ackermann_msgs.msg import AckermannDrive
car_ctrl = e2o_ctrl()
car_ctrl.Accel = 0
car_ctrl.Brake = 0
car_ctrl.Steer = 0
car_ctrl.RNDB = 'N'
ackermann_ctrl = AckermannDrive()
ackermann_ctrl.steering_angle_velocity = 1
ackermann_ctrl.speed = 0
ackermann_ctrl.steering_angle = 0
ackermann_ctrl.acceleration = 0
ackermann_ctrl.jerk = 0
last_speed = 0
a1 = 0.2 # if input acceleration is "'a' then top speed that can be reached is a*a1
a2 = 0.02/50 # increment speed by a2*a
a3 = 0.005/50 # decrement speed by a3*a
b1 = 1.0/1500 # decrement speed by b1*brake %
c1 = 1.0/1000 # when gear is neutral, decrement speed by c1
print("a1 ", a1)
print("a2 ", a2)
print("a3 ", a3)
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from ackermann_msgs.msg import AckermannDrive
t = AckermannDrive()
t.steering_angle_velocity = 1
key_mapping = {
'w': [0, 1],
'x': [0, -1],
'a': [-1, 0],
'd': [1, 0],
's': [0, 0]
}
def keys_cb(msg, twist_pub):
if len(msg.data) == 0 or not key_mapping.has_key(msg.data[0]):
return # unknown key
vels = key_mapping[msg.data[0]]
if msg.data[0] == 'w':
if t.speed < 0:
t.speed = 0
else:
t.speed += 0.05
elif msg.data[0] == 's':
if t.speed > 0:
t.speed = 0
else:
def goto(rxx, ryy, rphi, stop=False):
rx = rxx - H * np.cos(rphi)
ry = ryy - H * np.sin(rphi)
xhist = []
yhist = []
global min_radius, x, y, phi, x_circ, y_circ, first_curve, sign_curr, sign_goal, v, curr_x, curr_y, goal_x, goal_y
rphi = rphi - np.pi / 2
# Generate optimal path
e_old = 0
#time.sleep(3)
sign_curr, sign_goal, xx0, yy0, xx1, yy1, curr_x, curr_y, goal_x, goal_y = opt_path.get_path(
x, y, phi, rx, ry, rphi)
#print sign_curr, sign_goal, xx0, yy0, xx1, yy1, curr_x, curr_y, goal_x, goal_y, rx, ry
final_curve = False
first_curve = True
line = False
msg = AckermannDrive()
msg.acceleration = 20
msg.steering_angle_velocity = 50000
pub.publish(msg)
out = sign_curr * np.pi / 12
msg.steering_angle = sign_curr * np.pi / 12
pub.publish(msg)
msg.speed = v
side = 0
min_radius = 0.32 / np.tan(np.pi / 10)
#time.sleep(1)
pub.publish(msg)
#time.sleep(0.05)
stt = time.time()
r_temp_x = x
r_temp_y = y
r_temp_phi = phi
phi_old = phi
I = 0
u = out
inputs = np.ones([2, 1]) * sign_curr * np.pi / 6 * 0
bnd = (-np.pi * 25 / 180, np.pi * 25 / 180)
bnds = (bnd, bnd)
#print bnds
while (not rospy.is_shutdown()) and (np.abs(x - rx) > 0.2
or np.abs(y - ry) > 0.2):
r_temp_x_old = r_temp_x
r_temp_y_old = r_temp_y
r_temp_phi_old = r_temp_phi
theta = msg.steering_angle + 0.001
r_temp_x, r_temp_y, r_temp_phi, side, futureX, futureY, futurePhi, final_curve, first_curve, line = findR(
x, y, phi, aim_gain, msg, theta, v, H, rate, sign_curr, sign_goal,
xx0, yy0, xx1, yy1, curr_x, curr_y, goal_x, goal_y, final_curve,
first_curve, line)
m = 0.1 # Design parameters
aa = 1
ff = 1
if first_curve:
output = -sign_curr * np.pi / 180 * 25 * (
np.arctan(((x - curr_x)**2 +
(y - curr_y)**2 - min_radius**2) * 10) / (np.pi / 2)
) * 500 #*np.sign((x+H*np.sin(phi)-curr_x)**2+(y-H*np.cos(phi)-curr_y)**2-min_radius**2)
theta = output
#x_int=((yy0-y)*np.cos(phi)*(curr_x-xx0)+x*np.sin(phi)*(curr_x-xx0)-xx0*(curr_y-yy0)*np.cos(phi))/(np.sin(phi)*(curr_x-xx0)-(curr_y-yy0)*np.cos(phi))
#y_int=y+(x_int-xx0)*np.tan(phi)
#R=np.sqrt((xx0-x_int)**2+(yy0-y_int)**2)
#print x_circ,y_circ
#side=np.sign((-np.sin(phi))*(yy0-y)-(np.cos(phi))*(xx0-x_circ))
#theta=side*np.arctan(H/R)
#if np.abs(phi)<0.5:
# theta=side*np.arctan(H/min_radius)
#sol=minimize(circFunc,inputs,method='SLSQP',bounds=bnds)
#theta=sol.x[0]
#inputs=sign_curr*theta*np.ones([2,1])
#print sol
elif final_curve:
output = -sign_goal * np.pi * 25 / 180 * (
np.arctan(((x - goal_x)**2 +
(y - goal_y)**2 - min_radius**2) * 10) / (np.pi / 2)
) * 500 #np.sign((x+H*np.sin(phi)-goal_x)**2+(y-H*np.cos(phi)-goal_y)**2-min_radius**2)#v/H*2
theta = output
#x_int=((ry-y)*np.cos(phi)*(goal_x-rx)+x*np.sin(phi)*(goal_x-rx)-rx*(goal_y-ry)*np.cos(phi))/(np.sin(phi)*(goal_x-rx)-(goal_y-ry)*np.cos(phi))
#y_int=y+(x_int-rx)*np.tan(phi)
#R=np.sqrt((rx-x_int)**2+(ry-y_int)**2)
#side=np.sign((-np.sin(phi))*(ry-y)-(np.cos(phi))*(rx-x_circ))
#theta=side*np.arctan((H/R))
#if np.abs(phi)<0.5:
# theta=side*np.arctan(H/min_radius)
#sol=minimize(circFunc,inputs,method='SLSQP',bounds=bnds)
#theta=sol.x[0]
#inputs=sign_goal*theta*np.ones([2,1])
#print sol
elif line and np.abs((x - xx0) * (yy1 - yy0) - (xx1 - xx0) *
(y - yy0)) < 0.005:
phi0 = np.arctan2(yy1 - yy0, xx1 - xx0)
phi0 = phi0 - np.pi / 2
phi0 = np.mod(phi0 + np.pi, 2 * np.pi) - np.pi
f = (x - H * np.sin(phi) -
xx0) * (yy1 - yy0) / (xx1 - xx0) - y - H * np.cos(phi) + yy0
a = np.mod(phi - phi0 + np.pi, 2 * np.pi) - np.pi
b = f + m * a
fp = (-np.sin(phi) * (yy1 - yy0) / (xx1 - xx0) - np.cos(phi)) * v
if a > 0:
output = (-2 * f * fp - ff * f**2 - a**2)
elif a < 0:
output = (2 * f * fp + ff * f**2 + a**2)
else:
output = 0
theta = np.arctan(H / v * output)
else:
output = np.sign((x - H * np.sin(phi) - xx0) * (yy1 - yy0) -
(xx1 - xx0) * (y + H * np.cos(phi) - yy0)) * v / H
theta = np.arctan(H / v * output)
u = np.clip(theta, -np.pi * 25 / 180, np.pi * 25 / 180)
#print u
msg.steering_angle = u
pub.publish(msg)
rat.sleep()
stt = time.time()
#print u
#msg.steering_angle=sign_goal*np.pi*25/180
#pub.publish(msg)
#time.sleep(0.8)
while (not rospy.is_shutdown()) and np.sqrt(
(x - rxx)**2 +
(y - ryy)**2) > 0.05: #(np.abs(x-rxx)>0.04 or np.abs(y-ryy)>0.04):
#print 'movedon',rx,rxx,ry,ryy
if np.abs((x - rx) * (ryy - ry) - (rxx - rx) * (y - ry)) < 0.005:
phi0 = np.arctan2(ryy - ry, rxx - rx)
phi0 = phi0 - np.pi / 2
phi0 = np.mod(phi0 + np.pi, 2 * np.pi) - np.pi
f = (x - H * np.sin(phi) -
rx) * (ryy - ry) / (rxx - rx) - y - H * np.cos(phi) + ry
a = np.mod(phi - phi0 + np.pi, 2 * np.pi) - np.pi
m = 0.1 # Design parameters
aa = 1
ff = 1
b = f + m * a
fp = (-np.sin(phi) * (ryy - ry) / (rxx - rx) - np.cos(phi)) * v
if a > 0:
output = (-2 * f * fp - ff * f**2 - a**2)
elif a < 0:
output = (2 * f * fp + ff * f**2 + a**2)
else:
output = 0
theta = np.arctan(H / v * output)
else:
output = np.sign((x - H * np.sin(phi) - rx) * (ryy - ry) -
(rxx - rx) * (y + H * np.cos(phi) - ry)) * v / H
theta = np.arctan(H / v * output)
u = np.clip(theta, -np.pi * 25 / 180, np.pi * 25 / 180)
#print u
msg.steering_angle = u
pub.publish(msg)
rat.sleep()
stt = time.time()
print 'finished'
if stop:
msg.speed = 0
msg.steering_angle = 0
pub.publish(msg)
if stop:
time.sleep(5)
def main_fonk(data, a):
global x, y, paylas
global cur_pos, q_goal, angular_speed, linear_speed, linear_unit, angular_unit
global cur_action, next_action_time
linear_speed = 0.5
angular_speed = 0.5
linear_unit = 0.5
rate = rospy.Rate(20)
angular_unit = math.pi / 6.
# Initialize points
cur_pos.x = x
cur_pos.y = y
start_position = cur_pos
q_goal.z = 0.0
# Bayraklar
cur_action = 'gf' # 'tl': sola dön; 'tr': sağa dön; 'fw': düz ilerle
# Set parameters for actions
next_action_time = rospy.Time.now() # zaman hareketi başlıyor
safe_distance = 2.0 # bir engele yakalaşılacak min nokta
goal_tolarence = 1.5 # hedefe olan tolerans
if a == True:
print("\nAraç hedefe yöneliyor!")
i = 0
for p in data:
q_goal.x = p[0]
q_goal.y = p[1]
if paylas == False:
break
# eğer hedefe uzaksam ve ros kapanmadı ise döngüye başla (kontrol döngüsü)
while get_distance_between(
cur_pos,
q_goal) > goal_tolarence and not rospy.is_shutdown():
global regions_
get_goal_direction() # yön bul
ackerman = AckermannDrive()
ackerman.steering_angle_velocity = 0.0
ackerman.acceleration = 0.5
ackerman.jerk = 0.0
if cur_action == 'tr':
ackerman.speed = linear_speed
ackerman.steering_angle = -angular_speed
elif cur_action == 'tl':
ackerman.speed = linear_speed
ackerman.steering_angle = angular_speed
elif cur_action == 'gf':
ackerman.steering_angle = 0.0
ackerman.speed = linear_speed
elif cur_action == 'lgf':
ackerman.steering_angle = 0.0
ackerman.steering_angle = angular_speed + 0.25 #geriye dönerken yönünü daha fazla çevirir
ackerman.speed = -linear_speed
elif cur_action == 'rgf':
ackerman.steering_angle = 0.0
ackerman.steering_angle = -angular_speed - 0.25 #geriye dönerken yönünü daha fazla çevirir
ackerman.speed = -linear_speed
else:
print("[Hata Durumu]")
break
cmd_vel_pub.publish(ackerman)
rate.sleep()
# pozisyon güncellendi
cur_pos.x = x
cur_pos.y = y
# hedefe ulaşmak kontrol ediliyor
if get_distance_between(cur_pos, q_goal) <= goal_tolarence:
i = i + 1
print i, ". hedefe ulaşıldı!!!"
break
elif paylas == False:
a = False
print("Araç yeni hedef bekliyor!")
cur_action = 'stop'
ackerman = AckermannDrive()
ackerman.steering_angle_velocity = 0.0
ackerman.acceleration = 0.0
ackerman.jerk = 0.0
if cur_action == 'stop':
ackerman.speed = 0.0
ackerman.steering_angle = 0.0
cmd_vel_pub.publish(ackerman)
