def driveForwardDistance(): global positions, accTickDistance, gpsDistance i = 1 steering_cmd = NormalizedSteeringCommand() steering_cmd.value = 0.0 #left 1.0 , right -1.0 pub_steering.publish(steering_cmd) speed = SpeedCommand() speed.value = 0.2 #moeglichst langsam pub_speed.publish(speed) time.sleep(4) while i < len(positions): gpsDistance += math.sqrt( math.pow(positions[i][0] - positions[i - 1][0], 2) + math.pow(positions[i][1] - positions[i - 1][1], 2)) accTickDistance += accTick # print(gpsDistance) i = i + 1 rospy.loginfo("cumulativ gpsDistance (cm): " + str(gpsDistance)) rospy.loginfo("last - first position gps distance (cm) : " + str( math.sqrt( math.pow(positions[len(positions) - 1][0] - positions[0][0], 2) + math.pow(positions[len(positions) - 1][1] - positions[0][1], 2)))) rospy.loginfo("Ticks: " + str(accTickDistance)) rospy.loginfo("Distance per Tick (cumulativ gpsDistance (cm)/ ticks): " + str(gpsDistance / accTickDistance)) speed.value = 0.0 pub_speed.publish(speed)
def test1(): print("test 1") global x1, y1, position1, position2, endTicks rospy.sleep(3) position1 = [x1, y1] print("start position: " + str(position1)) steering_command = NormalizedSteeringCommand() steering_command.value = 1.0 publish_steering.publish(steering_command) rospy.sleep(1) speed_command = SpeedCommand() speed_command.value = 0.2 publish_speed.publish(speed_command) rospy.sleep(5) speed_command.value = 0.0 publish_speed.publish(speed_command) position2 = [x1, y1] print("end position: " + str(position2)) print("total ticks: " + str(endTicks)) euclideanDistance = euclidean_distance(position1[0], position1[1], position2[0], position2[1]) current_slope = calculate_slope(position1[0], position1[1], position2[0], position2[1]) current_angle = math.atan(current_slope) print("current angle: " + str(current_angle)) radius = (euclideanDistance / (2 * math.sin(current_angle / 2))) print("radius: " + str(radius)) distanceDriven = radius * current_angle print("total distance driven: " + str(distanceDriven)) ratio = distanceDriven / endTicks print("ratio between travelled distance and counted ticks: " + str(ratio))
def timer(): h = Header() h.stamp = rospy.Time.now() mensaje = NormalizedSteeringCommand() mensaje.header = h mensaje.value = -0.45 pub.publish(mensaje)
def PID(self): Kp = 5 Ki = 0 Kd = 0.1 error = (self.setpoint - self.yaw) self.acc_error = self.acc_error + error #accumulator for error current_time = rospy.Time.now() delta_time = (current_time - self.pre_time).to_sec() #Kp*error Ki*area Kd*slope PID = Kp * error + Ki * self.acc_error * delta_time + Kd * ( error - self.pre_error) / delta_time # PID = int(round(PID)) self.pre_time = current_time self.pre_error = error #pid value to steering message str_com = NormalizedSteeringCommand() str_com.value = PID self.str_pub.publish(str_com) #pub speed sp = SpeedCommand() sp.value = 0.3 self.speed_pub.publish(sp)
def publisher(): pub_steer = rospy.Publisher('/actuators/steering_normalized', NormalizedSteeringCommand, queue_size=10) pub_speed = rospy.Publisher('/actuators/speed', SpeedCommand, queue_size=10) rospy.init_node('publisher', anonymous=True) rate = rospy.Rate(10) while not rospy.is_shutdown(): ''' rospy.set_param('msg_steer', 1.0) msg_steer = rospy.get_param("msg_steer") rospy.loginfo(msg_steer) pub_steer.publish(msg_steer) rospy.set_param('msg_speed' , 0.3) msg_speed = rospy.get_param("msg_speed") rospy.loginfo(msg_speed) pub_speed.publish(msg_speed) rate.sleep() ''' msg_steer = NormalizedSteeringCommand() msg_steer.value = 1.0 rospy.loginfo(msg_steer) pub_steer.publish(msg_steer) msg_speed = SpeedCommand() msg_speed.value = 0.3 rospy.loginfo(msg_speed) pub_speed.publish(msg_speed)
def on_control(self, tmr): if tmr.last_duration is None: dt = 0.01 else: dt = (tmr.current_expected - tmr.last_expected).to_sec() # print dt quat = [ self.pose.pose.pose.orientation.x, self.pose.pose.pose.orientation.y, self.pose.pose.pose.orientation.z, self.pose.pose.pose.orientation.w ] roll, pitch, yaw = tf.transformations.euler_from_quaternion(quat) error = self.desired_angle self.integral_error += error * dt self.integral_error = max(self.min_i, self.integral_error) self.integral_error = min(self.max_i, self.integral_error) derivative_error = (error - self.last_error) / dt self.last_error = error pid_output = self.kp * error + self.kd * derivative_error + self.ki * self.integral_error steering_msg = NormalizedSteeringCommand() steering_msg.value = pid_output self.steering_pub.publish(steering_msg)
def main(): while curX == 0.0: pass pos1 = (curX,curY) time.sleep(1) steering_cmd = NormalizedSteeringCommand() steering_cmd.value = 1.0 pub_steering.publish(steering_cmd) time.sleep(1) speed_cmd = SpeedCommand() speed_cmd.value = 0.2 pub_speed.publish(speed_cmd) time.sleep(4) speed_cmd.value = 0.0 pub_speed.publish(speed_cmd) time.sleep(2) pos2 = (curX,curY) speed_cmd.value = 0.2 pub_speed.publish(speed_cmd) time.sleep(4) speed_cmd.value = 0.0 pub_speed.publish(speed_cmd) time.sleep(2) pos3 = (curX,curY) print "Radius:",berechneRadius(pos1,pos2,pos3),"Meter" print "Winkel:",berechneEinlenkWinkel(berechneRadius(pos1,pos2,pos3)),"Grad, weil niemand was mit Bogenmass anfangen kann"
def drive(distance, command, speed, angle): global is_active speed = speed * speed_mult rospy.loginfo("speed is %f", speed) rospy.loginfo("%s: Running %s(%f)", rospy.get_caller_id(), command, distance) if distance <= 0: rospy.logerr( "%s: Error, distance argument has to be > 0! %f given", rospy.get_caller_id(), distance) return pub_info.publish("BUSY") if is_active: rospy.logwarn( "%s: Warning, another command is still active! Please wait and try again.", rospy.get_caller_id()) return is_active = True # stop the car and set desired steering angle + speed speed_cmd = SpeedCommand() speed_cmd.value = 0 pub_speed.publish(speed_cmd) rospy.sleep(1) steering_cmd = NormalizedSteeringCommand() steering_cmd.value = angle pub_steering.publish(steering_cmd) rospy.sleep(1) speed_cmd.value = speed pub_speed.publish(speed_cmd) start_pos = last_odom.pose.pose.position current_distance = 0 while not rospy.is_shutdown() and current_distance < (distance - epsilon): current_pos = last_odom.pose.pose.position current_distance = sqrt( (current_pos.x - start_pos.x)**2 + (current_pos.y - start_pos.y)**2) # rospy.loginfo("current distance = %f", current_distance) rospy.sleep(0.1) speed_cmd.value = 0 pub_speed.publish(speed_cmd) is_active = False current_pos = last_odom.pose.pose.position current_distance = sqrt((current_pos.x - start_pos.x) ** 2 + (current_pos.y - start_pos.y)**2) pub_info.publish("FINISHED") rospy.loginfo( "%s: Finished %s(%f)\nActual travelled distance = %f", rospy.get_caller_id(), command, distance, current_distance)
def __init__(self): print("initializing Driver") rospy.init_node("assignment_6", anonymous = True) self.rate = rospy.Rate(10) self.steer_pub = rospy.Publisher("/actuators/steering_normalized", NormalizedSteeringCommand, queue_size=10) self.speed_pub = rospy.Publisher("/actuators/speed", SpeedCommand, queue_size=10) self.steer_cmd = NormalizedSteeringCommand() self.speed_cmd = SpeedCommand() self.speed_sub = rospy.Subscriber("/actuators/speed", Speed, self.print_speed_given, queue_size=10) print("initialized complete")
def driver(steer_pub, speed_pub): steer_msg = NormalizedSteeringCommand() steer_msg.value = -1.0 steer_pub.publish(steer_msg) speed_msg = SpeedCommand() speed_msg.value = 0.3 speed_pub.publish(speed_msg) rospy.sleep(2.5) speed_msg.value = 0.0 speed_pub.publish(speed_msg)
def on_localization(self, data): current_pos = np.array( [data.pose.pose.position.x, data.pose.pose.position.y]) look_ahead_point = self.map.lanes[self.lane].lookahead_point( current_pos, 0.5)[0] # [0] for lookahead point inner lane self.publish_looakhead(look_ahead_point) car_yaw = self.get_car_yaw(data) yaw = self.calculateYaw(current_pos, look_ahead_point, car_yaw) # yaw difference from two points print("I want this yaw:" + str(yaw)) steering_msg = NormalizedSteeringCommand() steering_msg.value = yaw self.pid_desired_angle.publish(steering_msg)
def talker(): pub = rospy.Publisher('/actuators/speed', SpeedCommand, queue_size=10) pub2 = rospy.Publisher('/actuators/steering_normalized', NormalizedSteeringCommand, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): msg = SpeedCommand() msg.value = 0.5 pub.publish(msg) msg2 = NormalizedSteeringCommand() msg2.value = 1.0 pub2.publish(msg2) rate.sleep()
def talker(): steer_pub = rospy.Publisher('/actuators/steering_normalized', NormalizedSteeringCommand, queue_size=10) speed_pub = rospy.Publisher('/actuators/speed', SpeedCommand, queue_size=10) rospy.init_node('publisher', anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): steer_msg = NormalizedSteeringCommand() steer_msg.value = 1.0 steer_pub.publish(steer_msg) speed_msg = SpeedCommand() speed_msg.value = 0.6 speed_pub.publish(speed_msg) rate.sleep()
def callback(self, raw_msgs): position = raw_msgs.pose.pose.position x = position.x y = position.y current_point = [x, y] # math transformation orientation = raw_msgs.pose.pose.orientation quaternion = [ orientation.x, orientation.y, orientation.z, orientation.w ] euler = tf.transformations.euler_from_quaternion(quaternion) self.yaw = euler[2] def rot(yaw): return np.array([[math.cos(yaw), -math.sin(yaw)], [math.sin(yaw), math.cos(yaw)]]) map = myMap.Map() lane = map.lanes[self.lane] goal_point = lane.lookahead_point(current_point, 0.5)[0] vector = goal_point - current_point map_point = np.matmul(rot(-self.yaw), vector) self.setpoint = np.arctan2(map_point[1], map_point[0]) # np.arccos(np.dot(current_point,goal_point)/(np.linalg.norm(current_point)*np.linalg.norm(goal_point))) Kp = 2.0 Ki = 0.0 Kd = 0.2 error = self.setpoint #- self.yaw self.acc_error = self.acc_error + error #accumulator for error current_time = rospy.Time.now() delta_time = (current_time - self.pre_time).to_sec() #Kp*error Ki*area Kd*slope PID = Kp * error + Ki * self.acc_error * delta_time + Kd * ( error - self.pre_error) / delta_time # PID = int(round(PID)) self.pre_time = current_time self.pre_error = error #pid value to steering message str_com = NormalizedSteeringCommand() str_com.value = PID self.str_pub.publish(str_com) #pub speed sp = SpeedCommand() sp.value = 0.3 self.speed_pub.publish(sp)
def callback(data): rospy.loginfo(rospy.get_caller_id() + "I heard %f", data.value) h = Header() h.stamp = Rospy.Time.now() mensaje = NormalizedSteeringCommand() mensaje.header = h mensaje.value = data.value if data.value < 0: mensaje.value = data.value * -1 if data.value > 0: mensaje.value = data.value * -1 pub.publish(mensaje)
def driver(): steer_pub = rospy.Publisher('/actuators/steering_normalized', NormalizedSteeringCommand, queue_size=10) speed_pub = rospy.Publisher('/actuators/speed', SpeedCommand, queue_size=10) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): steer_msg = NormalizedSteeringCommand() steer_msg.value = 1.0 steer_pub.publish(steer_msg) speed_msg = SpeedCommand() speed_msg.value = 0.3 speed_pub.publish(speed_msg)
def comienzo(): rospy.init_node('parking2', anonymous=True) pub = rospy.Publisher('/control/command/normalized_wanted_speed', NormalizedSpeedCommand, queue_size=10) steering = rospy.Publisher('/control/command/normalized_wanted_steering', NormalizedSteeringCommand, queue_size=10) h = std_msgs.msg.Header() h.stamp = rospy.Time.now() mensaje = mensaje2.value() mensaje.header = h mensaje.value = date.value j = std_msgs.msg.Header() j.stamp = rospy.Time.now() mensaje2 = NormalizedSteeringCommand() mensaje2.header = j mensaje2.value = 0.5
def callbackGPS(self, gps_data): # calculate how off steering is and give the error to PID Controler orientation = gps_data.pose.pose.orientation # print ("gps: " , gps_data) current_yaw = self.get_rotation(orientation) error = self.desired_angle - current_yaw if (abs(error) > self.accepted_error): update = self.PID_controller.get_PID(error) print("err:", error) print("yaw: ", current_yaw) print("UPD: ", update) steering = NormalizedSteeringCommand() steering.value = update self.pub_steering.publish(steering) self.pub_speed.publish(self.speed_cmd) rate.sleep()
def driver(): steer_pub = rospy.Publisher('/actuators/steering_normalized', NormalizedSteeringCommand, queue_size=10) speed_pub = rospy.Publisher('/actuators/speed', SpeedCommand, queue_size=10) steer_msg = NormalizedSteeringCommand() steer_msg.value = 1.0 steer_pub.publish(steer_msg) speed_msg = SpeedCommand() speed_msg.value = 0.3 speed_pub.publish(speed_msg) rospy.sleep(1) speed_msg = SpeedCommand() speed_msg.value = 0.0 speed_pub.publish(speed_msg)
def __init__(self): rospy.init_node("initNull") self.speed = SpeedCommand() self.steer = NormalizedSteeringCommand() self.position = Odometry() self.speed_pub = rospy.Publisher("/actuators/speed", SpeedCommand, queue_size=10) self.steer_pub = rospy.Publisher("/actuators/steering_normalized", NormalizedSteeringCommand, queue_size=10) self.position_sub = rospy.Subscriber("/communication/gps/9", Odometry, self.on_position, queue_size=10) self.rate = rospy.Rate(100) while not rospy.is_shutdown(): self.speed.value = 0.0 self.speed_pub.publish(self.speed) self.steer.value = 0.0 self.steer_pub.publish(self.steer) self.rate.sleep()
def __init__(self): rospy.init_node("basic_publisher") self.speed_publisher = rospy.Publisher("/actuators/speed", SpeedCommand, queue_size=1) self.steering_publisher = rospy.Publisher("/actuators/steering_normalized", NormalizedSteeringCommand, queue_size=1) self.r = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): steer_msg = NormalizedSteeringCommand() steer_msg.value = 1.0 self.steering_publisher.publish(steer_msg) speed_msg = SpeedCommand() speed_msg.value = 0.3 self.speed_publisher.publish(speed_msg) self.r.sleep()
def backwards_right(data, date): rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.value) pub = rospy.Publisher('/control/command/normalized_wanted_speed', NormalizedSpeedCommand, queue_size=10) steering = rospy.Publisher('/control/command/normalized_wanted_steering', NormalizedSteeringCommand, queue_size=10) h = std_msgs.msg.Header() h.stamp = rospy.Time.now() mensaje = NormalizedSpeedCommand() mensaje.header = h mensaje.value = data.value j = std_msgs.msg.Header() j.stamp = rospy.Time.now() mensaje2 = NormalizedSteeringCommand() mensaje2.header = j mensaje2.value = date.value if date.value > 0: mensaje.vlaue = date.value * -1 pub.publish(mensaje)
def callback(self,raw_msgs): #print("call") msg=raw_msgs.pose.pose.orientation msg_array=[msg.x,msg.y,msg.z,msg.w] #print(str(msg)) euler=tf.transformations.euler_from_quaternion(msg_array) #print(euler) steering=NormalizedSteeringCommand() steering.value=self.pid(euler[2]) speed=SpeedCommand() speed.value=0.5 #print(str(steering.value)) #cv2.imshow('image',img) #cv2.waitKey(3) self.x4.publish(speed) try: self.x3.publish(steering) except CvBridgeError as e: print(e)
def __init__(self): rospy.init_node("initSpeed", anonymous=True) self.speed = SpeedCommand() self.steer = NormalizedSteeringCommand() self.position = Odometry() self.speed_pub = rospy.Publisher("/actuators/speed", SpeedCommand, queue_size=10) self.steer_pub = rospy.Publisher("/actuators/steering_normalized", NormalizedSteeringCommand, queue_size=10) self.position_sub = rospy.Subscriber("/communication/gps/9", Odometry, self.on_position, queue_size=10) self.error_counter = 0 self.rate = rospy.Rate(10) self.Kp = 0.0 self.Ki = 0.0 self.Kd = 0.0 self.current_time = time.time() self.last_time = self.current_time self.error_time = 0.0 self.error = 0.0 self.P = 0.0 self.I = 0.0 self.D = 0.0 self.PID = 0.0 self.max_integrator = 5.0 self.min_integrator = -5.0 self.speed_value = 0.5 self.steering_value = 0.0 while not rospy.is_shutdown(): self.drive() pass rospy.spin()
def publish(): pub_speed = rospy.Publisher('/actuators/speed', SpeedCommand, queue_size=10) pub_steering = rospy.Publisher("/actuators/steering_normalized", NormalizedSteeringCommand, queue_size=10) steering_msg = NormalizedSteeringCommand() steering_msg.value = 1 speed_msg = SpeedCommand() speed_msg.value = 0.3 rospy.init_node('publisher', anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): pub_steering.publish(steering_msg) pub_speed.publish(speed_msg) rospy.loginfo('speed is ' + str(speed_msg.value)) print('speed is ' + str(speed_msg.value)) rate.sleep()
def talker(): # Publish to this existing topics pub_steering = rospy.Publisher('/actuators/steering_normalized', NormalizedSteeringCommand, queue_size=10) pub_speed = rospy.Publisher('/actuators/speed', SpeedCommand, queue_size=10) rospy.init_node('steering_speed_whisperer', anonymous=True) # steering norm_steerin_cmd = NormalizedSteeringCommand() # Command is a class create object norm_steerin_cmd.value = 0 #speed init speed_cmd = SpeedCommand() speed_cmd.value = 0.3 rate = rospy.Rate(5) # 5 hz while not rospy.is_shutdown(): info_str = "time {time} : speed: {speed} , steering: {steering}".format(time=rospy.get_time(),speed=speed_cmd.value,steering=norm_steerin_cmd.value) rospy.loginfo(info_str) pub_steering.publish(norm_steerin_cmd) pub_speed.publish(speed_cmd) rate.sleep()
def simple_parking(self): # here comes the values I will asign to the topics # definition of message for speed velocidad = NormalizedSpeedCommand() # definition of message for steering direccion = NormalizedSteeringCommand() rospy.sleep(1.0) #here are the values assigned to speed amd steering # first values velocidad.value = -0.2 direccion.value = 0.7 self.vel_pub.publish(velocidad) self.dir_pub.publish(direccion) # use this values for this period of time rospy.sleep(2.0) # second values velocidad.value = -0.2 direccion.value = -0.7 self.vel_pub.publish(velocidad) self.dir_pub.publish(direccion) # use this values for this period of time rospy.sleep(1.5) #third values velocidad.value = 0.2 direccion.value = 0.2 self.vel_pub.publish(velocidad) self.dir_pub.publish(direccion) rospy.sleep(1.0) # for stop, in this program the stop was by using ctrl+c velocidad.value = 0.0 direccion.value = 0.0 self.vel_pub.publish(velocidad) self.dir_pub.publish(direccion) rospy.sleep(10.0)
def on_control(self, tmr): if tmr.last_duration is None: dt = 0.01 else: dt = (tmr.current_expected - tmr.last_expected).to_sec() if self.desired_angle is None: return quat = [ self.pose.pose.pose.orientation.x, self.pose.pose.pose.orientation.y, self.pose.pose.pose.orientation.z, self.pose.pose.pose.orientation.w ] roll, pitch, yaw = tf.transformations.euler_from_quaternion(quat) diff = self.desired_angle - yaw # normalize steering angles (keep between -pi and pi) error = math.atan2(math.sin(diff), math.cos(diff)) self.integral_error += error * dt self.integral_error = max(self.min_i, self.integral_error) self.integral_error = min(self.max_i, self.integral_error) derivative_error = (error - self.last_error) / dt self.last_error = error pid_output = self.kp * error + self.kd * derivative_error + self.ki * self.integral_error steering_msg = NormalizedSteeringCommand() steering_msg.value = pid_output steering_msg.header.frame_id = "base_link" steering_msg.header.stamp = rospy.Time.now() self.steering_pub.publish(steering_msg)
def __init__(self): self.map = Map() rospy.init_node("map_visualization") self.steering = SteeringCommand() self.steering_normalized = NormalizedSteeringCommand() self.position = Odometry() self.lane_pub = rospy.Publisher("lane", Marker, queue_size=10) self.position_sub = rospy.Subscriber("/sensors/localization/filtered_map", Odometry, self.on_position, queue_size=1) self.pid_pub = rospy.Publisher("/control/steering", SteeringCommand, queue_size=1) self.lane_switch = rospy.Subscriber("/control/lane_switch", UInt8, self.lane_switch, queue_size=1) self.current_line = self.map.lanes[0] self.rate = rospy.Rate(1) while not rospy.is_shutdown(): i = 0 for lane in self.map.lanes: msg = Marker(type=Marker.LINE_STRIP, action=Marker.ADD) msg.header.frame_id = "map" msg.scale.x = 0.01 msg.scale.y = 0.01 msg.color.r = 1.0 msg.color.a = 1.0 msg.id = i for i in range(int(lane.length() * 100.0)): inter = lane.interpolate(i / 100.0) msg.points.append(Point(inter[0], inter[1], 0.0)) i += 1 self.lane_pub.publish(msg) self.rate.sleep()
def callback(self, data): dt = (data.header.stamp - self.last_time).to_sec() # 25hz if dt < 0.04: return self.last_time = data.header.stamp x = data.pose.pose.position.x y = data.pose.pose.position.y orientation_q = data.pose.pose.orientation orientation_list = [ orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w ] (roll, pitch, yaw) = euler_from_quaternion(orientation_list) x_index_floor = int(math.floor(x * (100.0 / self.resolution))) y_index_floor = int(math.floor(y * (100.0 / self.resolution))) x_index_ceil = x_index_floor + 1 y_index_ceil = y_index_floor + 1 ceil_ratio_x = x * (100.0 / self.resolution) - x_index_floor ceil_ratio_y = y * (100.0 / self.resolution) - y_index_floor if x_index_floor < 0: x_index_floor = 0 if x_index_floor > self.map_size_x / self.resolution - 1: x_index_floor = self.map_size_x / self.resolution - 1 if y_index_floor < 0: y_index_floor = 0 if y_index_floor > self.map_size_y / self.resolution - 1: y_index_floor = self.map_size_y / self.resolution - 1 if x_index_ceil < 0: x_index_ceil = 0 if x_index_ceil > self.map_size_x / self.resolution - 1: x_index_ceil = self.map_size_x / self.resolution - 1 if y_index_ceil < 0: y_index_ceil = 0 if y_index_ceil > self.map_size_y / self.resolution - 1: y_index_ceil = self.map_size_y / self.resolution - 1 x3_floor, y3_floor = self.matrix[x_index_floor, y_index_floor, :] x3_ceil, y3_ceil = self.matrix[x_index_ceil, y_index_ceil, :] x3 = x3_floor * (1.0 - ceil_ratio_x) + x3_ceil * ceil_ratio_x y3 = y3_floor * (1.0 - ceil_ratio_y) + y3_ceil * ceil_ratio_y f_x = np.cos(yaw) * x3 + np.sin(yaw) * y3 f_y = -np.sin(yaw) * x3 + np.cos(yaw) * y3 angle = np.arctan2(f_y, f_x) self.integral_error = self.integral_error + angle * dt steering = self.Kp * angle + self.Kd * ( (angle - self.last_angle) / dt) + self.Ki * self.integral_error self.last_angle = angle if f_x > 0: speed = -self.speed_value else: speed = self.speed_value if steering > 1: steering = 1 if steering < -1: steering = -1 if f_x > 0: speed = max(self.speed_value, speed * ((np.pi / 3) / (abs(steering) + 1))) steerMsg = NormalizedSteeringCommand() steerMsg.value = steering steerMsg.header.stamp = rospy.Time.now() self.pub.publish(steerMsg) if not self.shutdown_: msg = NormalizedSpeedCommand() msg.value = speed msg.header.stamp = rospy.Time.now() self.pub_speed.publish(msg)