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 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 updateControl(self, cone_msg):
shift, height, color = cone_msg.data
dist = max(np.polyval(self.polyfit, height), 0)
if dist <= control_dist:
print "avoiding cone"
setpoint = self.get_shift_setpoint(height, color)
shiftError = shift - setpoint
u_s = self.Kp * shiftError
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = self.speed
drive_msg.steering_angle = u_s
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.control_pub.publish()
self.pub.publish(drive_msg_stamped)
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 publish_drive_msg(self, speed, steer):
msg = AckermannDriveStamped()
msg.drive = AckermannDrive()
msg.drive.speed = speed
msg.drive.steering_angle = steer
self.drive_pub.publish(msg)
def publish(self):
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
# APPLY LOGIC
drive_msg_stamped.drive = drive_msg
self.vesc.publish(drive_msg_stamped)
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 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 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 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 velCallback(self, msg):
"""
Callback for keyboard teleop velocity message. It adds time stamp to the incoming message
Args
msg (AckermannDrive): Incoming ackerman velocity messages
"""
out_msg = AckermannDriveStamped()
out_msg.header.stamp = rospy.Time.now()
out_msg.drive = msg
self.ackermann_drive_timestamp_pub.publish(out_msg)
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.pub.publish(drive_msg_stamped)
def tape_present(self, msg):
frame = self.bridge.imgmsg_to_cv2(msg, "mono8")
white_pixels = cv2.countNonZero(frame)
if white_pixels < MIN_PIXELS_TO_DRIVE:
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = 0.0
drive_msg.steering_angle = 0.0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.pub.publish(drive_msg_stamped)
def con_input():
# Starts a new node
rospy.init_node('con_in_node', anonymous=True)
rate = rospy.Rate(20) # 20hz
con_in_pub = rospy.Publisher(
'/vesc/high_level/ackermann_cmd_mux/input/nav_0',
AckermannDriveStamped,
queue_size=10)
#Setting the current time for distance calculus
# let's build a 3x3 matrix:
drive_in = AckermannDriveStamped()
drive_in.header = Header()
drive_in.header.frame_id = 'base_link'
drive_in.header.stamp = rospy.Time.now()
drive_in.header.seq = 0
drive_in.drive = AckermannDrive()
drive_in.drive.steering_angle = 0
drive_in.drive.steering_angle_velocity = 0
drive_in.drive.speed = 0
drive_in.drive.acceleration = 0
drive_in.drive.jerk = 0
os.system("read -p 'Press Enter to continue...' var")
#print('Any key to start')
while not rospy.is_shutdown():
t0 = rospy.Time.now().to_sec()
t1 = t0
i = 0
while (t1 - t0 < 200):
#rospy.loginfo("t:%f\n",t1 - t0)
con_in_pub.publish(drive_in)
if t1 - t0 < 3:
drive_in.drive.steering_angle = 0
drive_in.drive.steering_angle_velocity = 0
drive_in.drive.speed = 0
drive_in.drive.acceleration = 0
drive_in.header.seq = i
else:
if i < 50:
drive_in.drive.speed = 2 * state_sig[i + 1, 3]
drive_in.drive.acceleration = 2 * con_sig[i, 0]
drive_in.drive.steering_angle = state_sig[i + 1, 4]
drive_in.drive.steering_angle_velocity = con_sig[i, 1]
i = i + 1
else:
drive_in.drive.steering_angle = 0
drive_in.drive.steering_angle_velocity = 0
drive_in.drive.speed = 0
drive_in.drive.acceleration = 0
drive_in.header.seq = i
t1 = rospy.Time.now().to_sec()
rate.sleep()
rate.sleep()
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)
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 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 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 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 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 callback(data):
rospy.loginfo(str(data.steer) + " - " + str(data.throttle))
ackermsg = AckermannDriveStamped()
ackermsg.header = Header()
ackermsg.header.stamp = rospy.Time.now()
ackermsg.drive = AckermannDrive()
ackermsg.drive.steering_angle = data.steer
ackermsg.drive.steering_angle_velocity = steering_angle_velocity
ackermsg.drive.speed = data.throttle
ackermsg.drive.acceleration = acceleration
ackermsg.drive.jerk = jerk
publisher.publish(ackermsg)
def __init__(self, data, pilotMode, vesc):
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive() #self.drive_msg = AckermannDrive()
if data[1]!='zed' and data[1]!='lidar':
#self.both(data[0], data[1])
logicData = [self.zedData, self.lidarData]
elif data[1]=='zed':
self.zed(data[0][0], data[0][1], data[0][2], data[0][3])
logicData = [self.zedData, 'zed']
elif data[1]=='lidar':
self.lidar(data[0][0], data[0][1], data[0][2], data[0][3], data[0][4], data[0][5])
logicData = [self.lidarData, 'lidar']
Logic(logicData, drive_msg, pilotMode)
drive_msg_stamped.drive = drive_msg
vesc.publish(drive_msg_stamped)
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 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 Command_output(self, steering, speed):
ack_cmd = AckermannDriveStamped()
ack_cmd.header.stamp = rospy.Time.now()
drive = AckermannDrive()
drive.speed = speed
drive.steering_angle = steering
# 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 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 pursuit(self):
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
r = rospy.Rate(self.rate)
while not rospy.is_shutdown():
print 'robot_pose:', self.robot_pose, 'waypoints:', self.waypoints
self.destination_pose = self.circleIntersect(self.lookahead_distance)
print 'destination_pose:', self.destination_pose
print 'waypoint_index:', self.current_waypoint_index
if (self.destination_pose == None):
self.speed = 0
self.steering_angle = 0
else:
self.speed = self.default_speed
distance_to_destination= self.getDistance(self.robot_pose, self.destination_pose)
angle_to_destination = -self.getAngle(self.robot_pose, self.destination_pose)
self.steering_angle = np.arctan((2 * self.robot_length * np.sin(angle_to_destination)) / distance_to_destination)
drive_msg.speed = self.speed
drive_msg.steering_angle = self.steering_angle
drive_msg_stamped.drive = drive_msg
self.pub.publish(drive_msg_stamped)
r.sleep()
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)
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()
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
global lastPixelDetectionX
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)
