def cbLanePoses(self, input_pose_msg):
"""Callback receiving pose messages
Args:
input_pose_msg (:obj:`LanePose`): Line segments in the ground plane relative to the robot origin.
"""
self.pose_msg = input_pose_msg
car_control_msg = Twist2DStamped()
car_control_msg.header = self.pose_msg.header
v, omega = self.pp_controller.pure_pursuit()
if omega:
car_control_msg.omega = omega
else:
v = 0.25
w = np.sin(self.pose_msg.phi + np.pi)
a = 6
b = 6
w = a * w + np.sign(w) * b * self.pose_msg.d
car_control_msg.omega = w
car_control_msg.v = v
self.publishCmd(car_control_msg)
def publishControl(self):
car_cmd_msg = Twist2DStamped()
car_cmd_msg.header.stamp = self.joy.header.stamp
car_cmd_msg.v = self.joy.axes[
1] * self.v_gain #Left stick V-axis. Up is positive
if self.bicycle_kinematics:
# Implements Bicycle Kinematics - Nonholonomic Kinematics
# see https://inst.eecs.berkeley.edu/~ee192/sp13/pdf/steer-control.pdf
steering_angle = self.joy.axes[3] * self.steer_angle_gain
car_cmd_msg.omega = car_cmd_msg.v / self.simulated_vehicle_length * math.tan(
steering_angle)
else:
# Holonomic Kinematics for Normal Driving
car_cmd_msg.omega = self.joy.axes[3] * self.omega_gain
if self.allrb == True:
self.AllRobot(1, car_cmd_msg)
#self.AllRobot(self.pub_car_cmd, car_cmd_msg)
else:
self.MultiRobot()
self.pub_car_cmd.publish(car_cmd_msg)
car_twist_msg = Twist()
car_twist_msg.linear.x = car_cmd_msg.v * 1.5
car_twist_msg.angular.z = car_cmd_msg.omega * 1.5
self.pub_car_twist.publish(car_twist_msg)
def __init__(self):
self.node_name = "Lane Filter"
self.active = True
self.filter = None
self.mode = 'histogram'
#self.mode = 'particle'
self.updateParams(None)
self.t_last_update = rospy.get_time()
self.velocity = Twist2DStamped()
self.d_median = []
self.phi_median = []
self.latencyArray = []
self.pub_in_lane = rospy.Publisher("~in_lane",BoolStamped, queue_size=1)
# Subscribers
self.sub = rospy.Subscriber("~segment_list", SegmentList, self.processSegments, queue_size=1)
self.sub_velocity = rospy.Subscriber("~car_cmd", Twist2DStamped, self.updateVelocity)
self.sub_change_params = rospy.Subscriber("~change_params", String, self.cbChangeParams)
# Publishers
self.pub_lane_pose = rospy.Publisher("~lane_pose", LanePose, queue_size=1)
self.pub_belief_img = rospy.Publisher("~belief_img", Image, queue_size=1)
self.pub_seglist_filtered = rospy.Publisher("~seglist_filtered",SegmentList, queue_size=1)
# FSM
self.sub_switch = rospy.Subscriber("~switch",BoolStamped, self.cbSwitch, queue_size=1)
self.sub_fsm_mode = rospy.Subscriber("~fsm_mode", FSMState, self.cbMode, queue_size=1)
self.active = True
# timer for updating the params
self.timer = rospy.Timer(rospy.Duration.from_sec(1.0), self.updateParams)
def cbPose(self, lane_pose_msg):
self.current_v = self.v_bar
self.lane_reading = lane_pose_msg
cross_track_err = lane_pose_msg.d - self.d_offset
heading_err = lane_pose_msg.phi
self.last_phi = heading_err
car_control_msg = Twist2DStamped()
car_control_msg.header = lane_pose_msg.header
self.last_header = lane_pose_msg.header
car_control_msg.v = self.current_v #*self.speed_gain #Left stick V-axis. Up is positive
if math.fabs(cross_track_err) > self.d_thres:
cross_track_err = cross_track_err / math.fabs(
cross_track_err) * self.d_thres
#self.found_obstacle = True
if not self.found_obstacle and not self.at_stop_line:
#rospy.loginfo("Cross track / phi %f %f, w: %f" %(cross_track_err, heading_err, self.k_d * cross_track_err + self.k_theta * heading_err))
car_control_msg.omega = self.k_d * cross_track_err + self.k_theta * heading_err #*self.steer_gain #Right stick H-axis. Right is negative
self.publishCmd(car_control_msg)
def getControlAction(self, pose_msg):
"""Callback that receives a pose message and updates the related control command.
Using a controller object, computes the control action using the current pose estimate.
Args:
pose_msg (:obj:`LanePose`): Message containing information about the current lane pose.
"""
current_s = rospy.Time.now().to_sec()
dt = None
if self.last_s is not None:
dt = (current_s - self.last_s)
if self.at_stop_line or self.at_obstacle_stop_line:
v = 0
omega = 0
else:
# Compute errors
d_err = pose_msg.d - self.params['~d_offset']
phi_err = pose_msg.phi
K = LRAGain()
omega = np.dot(-K,np.array([d_err,phi_err]))
# Initialize car control msg, add header from input message
car_control_msg = Twist2DStamped()
car_control_msg.header = pose_msg.header
# Add commands to car message
car_control_msg.v = v
car_control_msg.omega = omega
self.publishCmd(car_control_msg)
self.last_s = current_s
def line_sensor_callback(self, msg):
current_time = time.time()
duration = current_time - self.last_iteration_time
detections = self.threshold_measurement(msg)
# This dict maps line detections to a number representing the current state
states = {
# (Black, Black): Too far left
(False, False): -1,
# (Black, White): Perfectly on the line
(False, True): 0,
# (White, White): Too far right
(True, True): 1,
# (White, Black): Way too far right (past the line)
(True, False): 2
}
current_state = states[(detections.inner_right, detections.outer_right)]
self.integral = self.parameters['~I_decay'] * (self.integral + current_state * duration)
# Constrain the integral to within +/- max_I
self.integral = min(self.parameters['~max_I'], max(-self.parameters['~max_I'], self.integral))
omega = self.parameters['~steering_gain'] * current_state + self.parameters['~k_I'] * self.integral
# self.log("State: {}, Integral: {}, Omega: {}, Raw measurements: ({}, {})".format(
# current_state, self.integral, omega, msg.inner_right, msg.outer_right))
car_cmd = Twist2DStamped()
car_cmd.header.stamp = rospy.get_rostime()
car_cmd.v = self.parameters['~drive_speed']
car_cmd.omega = omega
self.car_cmd_pub.publish(car_cmd)
self.prevState = current_state
self.last_iteration_time = current_time
def cbPose(self, lane_pose_msg):
self.lane_reading = lane_pose_msg
cross_track_err = lane_pose_msg.d - self.d_offset
heading_err = lane_pose_msg.phi
car_control_msg = Twist2DStamped()
car_control_msg.header = lane_pose_msg.header
car_control_msg.v = self.v_bar #*self.speed_gain #Left stick V-axis. Up is positive
if math.fabs(cross_track_err) > self.d_thres:
cross_track_err = cross_track_err / math.fabs(
cross_track_err) * self.d_thres
if (car_control_msg.v == 0.0): # ADDED
car_control_msg.omega = 0.0 # ADDED
else: # ADDED
car_control_msg.omega = self.k_d * cross_track_err + self.k_theta * heading_err #*self.steer_gain #Right stick H-axis. Right is negative
# controller mapping issue
# car_control_msg.steering = -car_control_msg.steering
# print "controls: speed %f, steering %f" % (car_control_msg.speed, car_control_msg.steering)
# self.pub_.publish(car_control_msg)
self.publishCmd(car_control_msg)
def cbBool(self, bool_msg):
stop = Twist2DStamped()
stop.header = bool_msg.header
stop.v = 0.0
stop.omega = 0.0
self.pub_car_cmd.publish(stop)
def stop(self):
rospy.sleep(0.2)
cmd = Twist2DStamped()
cmd.v = 0
cmd.omega = 0
self.pub_car_cmd.publish(cmd)
#!/usr/bin/env python
# Author: PCH 2017
"""
Description: Drive straight the distance of 1 mat.
"""
import rospy
from duckietown_msgs.msg import Twist2DStamped
if __name__ == '__main__':
rospy.init_node('drive_straight_test', anonymous=False)
vel = 0.38 # m/s
mat_len = 23.0 + 5.0 / 8.0 # inch
dist = 1 * mat_len / 39.3701 # m
cmd_go = Twist2DStamped(v=vel, omega=0.0)
cmd_stop = Twist2DStamped(v=0.0, omega=0.0)
pub = rospy.Publisher("/pi/dagu_car/vel_cmd", Twist2DStamped, queue_size=1)
rospy.sleep(0.2)
cmd_go.header.stamp = rospy.Time.now()
pub.publish(cmd_go)
rospy.sleep(1.2 * dist / vel) # fudge factor
cmd_stop.header.stamp = rospy.Time.now()
pub.publish(cmd_stop)
def JoyAxes(self, joy_msg): carcmd = Twist2DStamped() carcmd.v = joy_msg.axes[4]*0.3 self.pub_carcmd.publish(carcmd)
def toSegmentMsg(self, lines, normals, color):
arr_cutoff = np.array((0, self.top_cutoff, 0, self.top_cutoff))
arr_ratio = np.array(
(1. / self.image_size[1], 1. / self.image_size[0],
1. / self.image_size[1], 1. / self.image_size[0]))
segmentMsgList = []
for x1, y1, x2, y2, norm_x, norm_y in np.hstack((lines, normals)):
ox = int((x1 + x2) / 2)
oy = int((y1 + y2) / 2)
cx = (ox + 3 * norm_x).astype('int')
cy = (oy + 3 * norm_y).astype('int')
ccx = (ox - 3 * norm_x).astype('int')
ccy = (oy - 3 * norm_y).astype('int')
if cx > 158:
cx = 158
elif cx < 1:
cx = 1
if ccx > 158:
ccx = 158
elif ccx < 1:
ccx = 1
if cy >= 79:
cy = 79
elif cy < 1:
cy = 1
if ccy >= 79:
ccy = 79
elif ccy < 1:
ccy = 1
if (self.blue[cy, cx] == 255 and self.yellow[ccy, ccx] == 255) or (
self.yellow[cy, cx] == 255 and self.blue[ccy, ccx] == 255):
[x1, y1, x2,
y2] = (([x1, y1, x2, y2] + arr_cutoff) * arr_ratio)
segment = Segment()
segment.color = color
segment.pixels_normalized[0].x = x1
segment.pixels_normalized[0].y = y1
segment.pixels_normalized[1].x = x2
segment.pixels_normalized[1].y = y2
segment.normal.x = norm_x
segment.normal.y = norm_y
segmentMsgList.append(segment)
if self.time_switch == True:
msgg = BoolStamped()
msgg.data = True
self.pub_lane.publish(msgg)
self.time_switch = False
self.count = 0
else:
self.count += 1
if self.count > 5:
print "****************count = ", self.count, " *******************"
if self.time_switch == False:
msgg = BoolStamped()
msgg.data = False
self.pub_lane.publish(msgg)
self.time_switch = True
car_control_msg = Twist2DStamped()
car_control_msg.v = 0.0
car_control_msg.omega = 0.0
self.pub_car_cmd.publish(car_control_msg)
return segmentMsgList
def sendStop(self):
# Send stop command
car_control_msg = Twist2DStamped()
car_control_msg.v = 0.0
car_control_msg.omega = 0.0
self.publishCmd(car_control_msg)
def pubStop(self):
msg = Twist2DStamped()
msg.v = 0
msg.omega = 0
self.car_cmd_pub.publish(msg)
def __init__(self):
super(controller, self).__init__()
self.publisher = rospy.Publisher("/duckiebot/wheels_driver_node/car_cmd", Twist2DStamped, queue_size = 1)
self.subscriber = rospy.Subscriber("/duckiebot/joy", Joy, self.callback)
self.twist = Twist2DStamped()
def cbPose(self, lane_pose_msg):
self.lane_reading = lane_pose_msg
# self.d_target_pub = rospy.Publisher(self.pub_topic, Float32, queue_size=1)
#if self.object_detected: # if object is detected (TRUE)
#self.d_ref = 0.02 # set d_ref here, LanePose message from saviors
# negetive value: moves towards right line
# positive value: moves towards left line
###END TODO
# Calculating the delay image processing took
timestamp_now = rospy.Time.now()
image_delay_stamp = timestamp_now - self.lane_reading.header.stamp
# delay from taking the image until now in seconds
image_delay = image_delay_stamp.secs + image_delay_stamp.nsecs / 1e9
prev_cross_track_err = self.cross_track_err
prev_heading_err = self.heading_err
self.cross_track_err = lane_pose_msg.d - self.d_offset - self.d_ref
self.heading_err = lane_pose_msg.phi - self.phi_ref
car_control_msg = Twist2DStamped()
car_control_msg.header = lane_pose_msg.header
car_control_msg.v = self.v_bar #*self.speed_gain #Left stick V-axis. Up is positive
if math.fabs(self.cross_track_err) > self.d_thres:
rospy.logerr("inside threshold ")
self.cross_track_err = self.cross_track_err / math.fabs(
self.cross_track_err) * self.d_thres
currentMillis = int(round(time.time() * 1000))
if self.last_ms is not None:
dt = (currentMillis - self.last_ms) / 1000.0
self.cross_track_integral += self.cross_track_err * dt
self.heading_integral += self.heading_err * dt
if self.cross_track_integral > 0.3:
rospy.loginfo("you're greater 0.3")
self.cross_track_integral = 0.3
if self.cross_track_integral < -0.3:
rospy.loginfo("youre smaller -0.3")
self.cross_track_integral = -0.3
if self.heading_integral < -1.2:
self.heading_integral = -1.2
if self.heading_integral > 1.2:
self.heading_integral = 1.2
if abs(
self.cross_track_err
) <= 0.011: # TODO: replace '<= 0.011' by '< delta_d' (but delta_d might need to be sent by the lane_filter_node.py or even lane_filter.py)
self.cross_track_integral = 0
if abs(
self.heading_err
) <= 0.051: # TODO: replace '<= 0.051' by '< delta_phi' (but delta_phi might need to be sent by the lane_filter_node.py or even lane_filter.py)
self.heading_integral = 0
if np.sign(self.cross_track_err) != np.sign(prev_cross_track_err):
self.cross_track_integral = 0
if np.sign(self.heading_err) != np.sign(prev_heading_err):
self.heading_integral = 0
if self.wheels_cmd_executed.vel_right == 0 and self.wheels_cmd_executed.vel_left == 0:
self.cross_track_integral = 0
self.heading_integral = 0
# if velocity_of_actual_motor_comand == 0: # TODO: get this velocity that is actually sent to the motors and plug in here
# self.cross_track_integral = 0
# self.heading_integral = 0
# if self.curve_inner:
# self.curvature = self.curvature_inner
# else:
# self.curvature = self.curvature_outer
omega_feedforward = self.v_bar * self.velocity_to_m_per_s * lane_pose_msg.curvature * 2 * math.pi
if self.turn_off_feedforward_part:
omega_feedforward = 0
omega = self.k_d * self.cross_track_err + self.k_theta * self.heading_err
# rospy.loginfo("P-Control: " + str(car_control_msg.omega))
# rospy.loginfo("Adjustment: " + str(-self.k_Id * self.cross_track_integral))
omega -= self.k_Id * self.cross_track_integral
omega -= self.k_Iphi * self.heading_integral
omega += (omega_feedforward) * self.omega_to_rad_per_s
### omega_max_actuator_params = ..... # TODO: complete (based on parameters from self.actuator_params)
### omega_max_radius_limitation = ..... # TODO: complete (based on radius limitation)
### self.omega_max = min(omega_max_actuator_params, omega_max_radius_limitation)
if omega > self.omega_max:
self.cross_track_integral -= self.cross_track_err * dt
self.heading_integral -= self.heading_err * dt
### if omega > omega_max_radius_limitation:
### car_control_msg.omega = omega_max_radius_limitation
else:
car_control_msg.omega = omega
# if not self.incurvature:
# if self.heading_err > 0.3:
# self.incurvature = True
# rospy.set_param('~incurvature',True)
# car_control_msg.omega -= self.k_Id * self.cross_track_integral
# car_control_msg.omega -= self.k_Iphi * self.heading_integral #*self.steer_gain #Right stick H-axis. Right is negative
# else:
# if self.curve_inner:
# time_incurve = 1
# else:
# time_incurve = 3
# if (rospy.Time.now().secs - self.time_start_curve) > time_incurve: #TODO fix 5 to a time in curvature with v and d
# rospy.set_param('~incurvature',False)
# self.incurvature = False
# rospy.loginfo("incurvature : ")
# car_control_msg.omega += ( omega_feedforward) * self.omega_to_rad_per_s
# rospy.loginfo("kid : " + str(self.k_Id))
# rospy.loginfo("Kd : " + str(self.k_d))
#rospy.loginfo("k_Iphi * heading : " + str(self.k_Iphi * self.heading_integral))
# rospy.loginfo("k_Iphi :" + str(self.k_Iphi))
# rospy.loginfo("Ktheta : " + str(self.k_theta))
# rospy.loginfo("incurvature : " + str(self.incurvature))
# rospy.loginfo("cross_track_err : " + str(self.cross_track_err))
# rospy.loginfo("heading_err : " + str(self.heading_err))
#rospy.loginfo("Ktheta : Versicherung")
rospy.loginfo("lane_pose_msg.curvature: " +
str(lane_pose_msg.curvature))
rospy.loginfo("heading_err: " + str(self.heading_err))
rospy.loginfo("heading_integral: " + str(self.heading_integral))
rospy.loginfo("cross_track_err: " + str(self.cross_track_err))
rospy.loginfo("cross_track_integral: " +
str(self.cross_track_integral))
rospy.loginfo("turn_off_feedforward_part: " +
str(self.turn_off_feedforward_part))
# rospy.loginfo("actuator_params.gain: " + str(self.actuator_params.gain))
# rospy.loginfo("actuator_params.trim: " + str(self.actuator_params.trim))
# rospy.loginfo("actuator_params.baseline: " + str(self.actuator_params.baseline))
# rospy.loginfo("actuator_params.radius: " + str(self.actuator_params.radius))
# rospy.loginfo("actuator_params.k: " + str(self.actuator_params.k))
# rospy.loginfo("actuator_params.limit: " + str(self.actuator_params.limit))
# controller mapping issue
# car_control_msg.steering = -car_control_msg.steering
# print "controls: speed %f, steering %f" % (car_control_msg.speed, car_control_msg.steering)
# self.pub_.publish(car_control_msg)
self.publishCmd(car_control_msg)
self.last_ms = currentMillis
def setGains(self):
self.v_bar_gain_ref = 0.5
v_bar_fallback = 0.25 # nominal speed, 0.25m/s
self.v_max = 1
k_theta_fallback = -2.0
k_d_fallback = -(k_theta_fallback**2) / (4.0 * self.v_bar_gain_ref)
theta_thres_fallback = math.pi / 6.0
d_thres_fallback = math.fabs(
k_theta_fallback / k_d_fallback) * theta_thres_fallback
d_offset_fallback = 0.0
k_theta_fallback = k_theta_fallback
k_d_fallback = k_d_fallback
k_Id_fallback = 2.5
k_Iphi_fallback = 1.25
self.fsm_state = None
self.cross_track_err = 0
self.heading_err = 0
self.cross_track_integral = 0
self.heading_integral = 0
self.cross_track_integral_top_cutoff = 0.3
self.cross_track_integral_bottom_cutoff = -0.3
self.heading_integral_top_cutoff = 1.2
self.heading_integral_bottom_cutoff = -1.2
#-1.2
self.time_start_curve = 0
use_feedforward_part_fallback = False
self.wheels_cmd_executed = WheelsCmdStamped()
self.actuator_limits = Twist2DStamped()
self.actuator_limits.v = 999.0 # to make sure the limit is not hit before the message is received
self.actuator_limits.omega = 999.0 # to make sure the limit is not hit before the message is received
self.omega_max = 999.0 # considering radius limitation and actuator limits # to make sure the limit is not hit before the message is received
self.use_radius_limit_fallback = True
self.flag_dict = {
"obstacle_detected": False,
"parking_stop": False,
"fleet_planning_lane_following_override_active": False,
"implicit_coord_velocity_limit_active": False
}
self.pose_msg = LanePose()
self.pose_initialized = False
self.pose_msg_dict = dict()
self.v_ref_possible = dict()
self.main_pose_source = None
self.active = True
self.sleepMaintenance = False
# overwrites some of the above set default values (the ones that are already defined in the corresponding yaml-file (see launch-file of this node))
self.v_bar = self.setupParameter("~v_bar",
v_bar_fallback) # Linear velocity
self.k_d = self.setupParameter("~k_d", k_d_fallback) # P gain for d
self.k_theta = self.setupParameter(
"~k_theta", k_theta_fallback) # P gain for theta
self.d_thres = self.setupParameter(
"~d_thres", d_thres_fallback) # Cap for error in d
self.theta_thres = self.setupParameter(
"~theta_thres", theta_thres_fallback) # Maximum desire theta
self.d_offset = self.setupParameter(
"~d_offset",
d_offset_fallback) # a configurable offset from the lane position
self.k_Id = self.setupParameter(
"~k_Id", k_Id_fallback) # gain for integrator of d
self.k_Iphi = self.setupParameter(
"~k_Iphi",
k_Iphi_fallback) # gain for integrator of phi (phi = theta)
#TODO: Feedforward was not working, go away with this error source! (Julien)
self.use_feedforward_part = self.setupParameter(
"~use_feedforward_part", use_feedforward_part_fallback)
self.omega_ff = self.setupParameter("~omega_ff", 0)
self.omega_max = self.setupParameter("~omega_max", 999)
self.omega_min = self.setupParameter("~omega_min", -999)
self.use_radius_limit = self.setupParameter(
"~use_radius_limit", self.use_radius_limit_fallback)
self.min_radius = self.setupParameter("~min_rad", 0.0)
self.d_ref = self.setupParameter("~d_ref", 0)
self.phi_ref = self.setupParameter("~phi_ref", 0)
self.object_detected = self.setupParameter("~object_detected", 0)
self.v_ref_possible["default"] = self.v_max
def pubStop(self):
msg = Twist2DStamped()
msg.v = 0
msg.omega = 0
rospy.loginfo('[%s] pubStop stop', self.name)
self.pub_cmd.publish(msg)
def updatePose(self, pose_msg):
self.lane_reading = pose_msg
# Calculating the delay image processing took
timestamp_now = rospy.Time.now()
image_delay_stamp = timestamp_now - self.lane_reading.header.stamp
# delay from taking the image until now in seconds
image_delay = image_delay_stamp.secs + image_delay_stamp.nsecs / 1e9
prev_cross_track_err = self.cross_track_err
prev_heading_err = self.heading_err
self.cross_track_err = pose_msg.d - self.d_offset
self.heading_err = pose_msg.phi
car_control_msg = Twist2DStamped()
car_control_msg.header = pose_msg.header
car_control_msg.v = pose_msg.v_ref
if car_control_msg.v > self.actuator_limits.v:
car_control_msg.v = self.actuator_limits.v
if math.fabs(self.cross_track_err) > self.d_thres:
rospy.logerr("inside threshold ")
self.cross_track_err = self.cross_track_err / math.fabs(
self.cross_track_err) * self.d_thres
currentMillis = int(round(time.time() * 1000))
if self.last_ms is not None:
dt = (currentMillis - self.last_ms) / 1000.0
self.cross_track_integral += self.cross_track_err * dt
self.heading_integral += self.heading_err * dt
if self.cross_track_integral > self.cross_track_integral_top_cutoff:
self.cross_track_integral = self.cross_track_integral_top_cutoff
if self.cross_track_integral < self.cross_track_integral_bottom_cutoff:
self.cross_track_integral = self.cross_track_integral_bottom_cutoff
if self.heading_integral > self.heading_integral_top_cutoff:
self.heading_integral = self.heading_integral_top_cutoff
if self.heading_integral < self.heading_integral_bottom_cutoff:
self.heading_integral = self.heading_integral_bottom_cutoff
if abs(
self.cross_track_err
) <= 0.011: # TODO: replace '<= 0.011' by '< delta_d' (but delta_d might need to be sent by the lane_filter_node.py or even lane_filter.py)
self.cross_track_integral = 0
if abs(
self.heading_err
) <= 0.051: # TODO: replace '<= 0.051' by '< delta_phi' (but delta_phi might need to be sent by the lane_filter_node.py or even lane_filter.py)
self.heading_integral = 0
if np.sign(self.cross_track_err) != np.sign(
prev_cross_track_err
): # sign of error changed => error passed zero
self.cross_track_integral = 0
if np.sign(self.heading_err) != np.sign(
prev_heading_err
): # sign of error changed => error passed zero
self.heading_integral = 0
if self.wheels_cmd_executed.vel_right == 0 and self.wheels_cmd_executed.vel_left == 0: # if actual velocity sent to the motors is zero
self.cross_track_integral = 0
self.heading_integral = 0
omega_feedforward = car_control_msg.v * pose_msg.curvature_ref
if self.main_pose_source == "lane_filter" and not self.use_feedforward_part:
omega_feedforward = 0
# Scale the parameters linear such that their real value is at 0.22m/s TODO do this nice that * (0.22/self.v_bar)
omega = self.k_d * (
0.22 / self.v_bar) * self.cross_track_err + self.k_theta * (
0.22 / self.v_bar) * self.heading_err
omega += (omega_feedforward)
# check if nominal omega satisfies min radius, otherwise constrain it to minimal radius
if math.fabs(omega) > car_control_msg.v / self.min_radius:
if self.last_ms is not None:
self.cross_track_integral -= self.cross_track_err * dt
self.heading_integral -= self.heading_err * dt
omega = math.copysign(car_control_msg.v / self.min_radius, omega)
if not self.fsm_state == "SAFE_JOYSTICK_CONTROL":
# apply integral correction (these should not affect radius, hence checked afterwards)
omega -= self.k_Id * (0.22 /
self.v_bar) * self.cross_track_integral
omega -= self.k_Iphi * (0.22 / self.v_bar) * self.heading_integral
if car_control_msg.v == 0:
omega = 0
else:
# check if velocity is large enough such that car can actually execute desired omega
if car_control_msg.v - 0.5 * math.fabs(omega) * 0.1 < 0.065:
car_control_msg.v = 0.065 + 0.5 * math.fabs(omega) * 0.1
# apply magic conversion factors
car_control_msg.v = car_control_msg.v * self.velocity_to_m_per_s
car_control_msg.omega = omega * self.omega_to_rad_per_s
omega = car_control_msg.omega
if omega > self.omega_max: omega = self.omega_max
if omega < self.omega_min: omega = self.omega_min
omega += self.omega_ff
car_control_msg.omega = omega
self.publishCmd(car_control_msg)
self.last_ms = currentMillis
def publishCmd(self,stamp): cmd_msg = Twist2DStamped()
def publishControl(self):
car_cmd_msg = Twist2DStamped()
#car_cmd_msg.header.stamp = self.joy.header.stamp
car_cmd_msg.v = self.cmd.linear.x * self.v_gain #Left stick V-axis. Up is positive
car_cmd_msg.omega = self.cmd.angular.z * self.omega_gain
self.pub_car_cmd.publish(car_cmd_msg)
def setGains(self):
self.v_bar_gain_ref = 0.5
self.v_max = 1
v_bar_fallback = 0.25 # nominal speed, 0.25m/s
k_theta_fallback = -2.0
k_d_fallback = -(k_theta_fallback**2) / (4.0 * self.v_bar_gain_ref)
theta_thres_fallback = math.pi / 6.0
d_thres_fallback = math.fabs(
k_theta_fallback / k_d_fallback) * theta_thres_fallback
d_offset_fallback = 0.0
k_Id_fallback = 2.5
k_Iphi_fallback = 1.25
self.fsm_state = None
self.cross_track_err = 0
self.heading_err = 0
self.cross_track_integral = 0
self.heading_integral = 0
self.cross_track_integral_top_cutoff = 0.3
self.cross_track_integral_bottom_cutoff = -0.3
self.heading_integral_top_cutoff = 1.2
self.heading_integral_bottom_cutoff = -1.2
self.time_start_curve = 0
self.wheels_cmd_executed = WheelsCmdStamped()
self.actuator_limits = Twist2DStamped()
self.actuator_limits.v = 999.0 # to make sure the limit is not hit before the message is received
self.actuator_limits.omega = 999.0 # to make sure the limit is not hit before the message is received
self.omega_max = 999.0 # considering radius limitation and actuator limits
self.use_radius_limit_fallback = True
self.pose_msg = LanePose()
self.pose_initialized = False
self.pose_msg_dict = dict()
self.v_ref_possible = {
'default': self.v_max,
'main_pose': v_bar_fallback
}
self.main_pose_source = None
self.active = True
# overwrites some of the above set default values (the ones that are already defined in the corresponding yaml-file (see launch-file of this node))
# Linear velocity
self.v_bar = self.setupParameter("~v_bar", v_bar_fallback)
# P gain for d
self.k_d = self.setupParameter("~k_d", k_d_fallback)
# P gain for theta
self.k_theta = self.setupParameter("~k_theta", k_theta_fallback)
# Cap for error in d
self.d_thres = self.setupParameter("~d_thres", d_thres_fallback)
# Maximum desired theta
self.theta_thres = self.setupParameter("~theta_thres",
theta_thres_fallback)
# A configurable offset from the lane position
self.d_offset = self.setupParameter("~d_offset", d_offset_fallback)
# Gain for integrator of d
self.k_Id = self.setupParameter("~k_Id", k_Id_fallback)
# Gain for integrator of phi (phi = theta)
self.k_Iphi = self.setupParameter("~k_Iphi", k_Iphi_fallback)
# setup backward parameters
self.k_d_back = self.setupParameter("~k_d_back", 3.0)
self.k_theta_back = self.setupParameter("~k_theta_back", 1.0)
self.k_Id_back = self.setupParameter("~k_Id_back", 1.0)
self.k_Itheta_back = self.setupParameter("~k_Itheta_back", -1.0)
self.omega_ff = self.setupParameter("~omega_ff", 0)
self.omega_max = self.setupParameter("~omega_max", 4.7)
self.omega_min = self.setupParameter("~omega_min", -4.7)
self.use_radius_limit = self.setupParameter(
"~use_radius_limit", self.use_radius_limit_fallback)
self.min_radius = self.setupParameter("~min_rad", 0.0)
self.d_ref = self.setupParameter("~d_ref", 0)
self.phi_ref = self.setupParameter("~phi_ref", 0)
self.object_detected = self.setupParameter("~object_detected", 0)
def publish_cmd(self, v, omega):
car_control_msg = Twist2DStamped()
car_control_msg.v = v
car_control_msg.omega = omega
self.pub_car_cmd.publish(car_control_msg)
def publish_car_cmd(self, event):
self.pub_coord_cmd.publish(Twist2DStamped(v=0, omega=0))
def __init__(self):
self.node_name = "DT Project Estimator"
self.active = True
self.filter = None
self.updateParams(None)
self.t_last_update = rospy.get_time()
self.velocity = Twist2DStamped()
self.d_median = []
self.phi_median = []
self.latencyArray = []
# #####################
# DT project add
# #####################
self.buffer_white = []
self.buffer_yellow = []
# #####################
# Define Constants
self.curvature_res = self.filter.curvature_res
# Set parameters to server
rospy.set_param(
'~curvature_res',
self.curvature_res) #Write to parameter server for transparancy
self.pub_in_lane = rospy.Publisher("~in_lane",
BoolStamped,
queue_size=1)
# Subscribers
self.sub = rospy.Subscriber("~segment_list",
SegmentList,
self.processSegments,
queue_size=1)
self.sub_velocity = rospy.Subscriber("~car_cmd", Twist2DStamped,
self.updateVelocity)
self.sub_change_params = rospy.Subscriber("~change_params", String,
self.cbChangeParams)
# Publishers
self.pub_lane_pose = rospy.Publisher("~lane_pose",
LanePose,
queue_size=1)
self.pub_belief_img = rospy.Publisher("~belief_img",
Image,
queue_size=1)
self.pub_seglist_filtered = rospy.Publisher("~seglist_filtered",
SegmentList,
queue_size=1)
self.pub_ml_img = rospy.Publisher("~ml_img", Image, queue_size=1)
self.pub_entropy = rospy.Publisher("~entropy", Float32, queue_size=1)
# ################################
# DT project add
# ################################
self.veh_name = rospy.get_namespace().strip("/")
self.pub_lane = rospy.Publisher("~gp_lanes", MarkerArray, queue_size=1)
# ################################
# FSM
self.sub_switch = rospy.Subscriber("~switch",
BoolStamped,
self.cbSwitch,
queue_size=1)
self.sub_fsm_mode = rospy.Subscriber("~fsm_mode",
FSMState,
self.cbMode,
queue_size=1)
self.active = True
# timer for updating the params
self.timer = rospy.Timer(rospy.Duration.from_sec(1.0),
self.updateParams)
def car_cmd_callback(self, msg_car_cmd):
"""
A callback that reposponds to received `car_cmd` messages by calculating the
corresponding wheel commands, taking into account the robot geometry, gain and trim
factors, and the set limits. These wheel commands are then published for the motors to use.
The resulting linear and angular velocities are also calculated and published.
Args:
msg_car_cmd (:obj:`Twist2DStamped`): desired car command
"""
# INVERSE KINEMATICS PART
# trim the desired commands such that they are within the limits:
msg_car_cmd.v = self.trim(
msg_car_cmd.v,
low=-self._v_max,
high=self._v_max
)
msg_car_cmd.omega = self.trim(
msg_car_cmd.omega,
low=-self._omega_max,
high=self._omega_max
)
# assuming same motor constants k for both motors
k_r = self._k
k_l = self._k
# adjusting k by gain and trim
k_r_inv = (self._gain.value + self._trim.value) / k_r
k_l_inv = (self._gain.value - self._trim.value) / k_l
omega_r = (msg_car_cmd.v + 0.5 * msg_car_cmd.omega * self._baseline) / self._radius
omega_l = (msg_car_cmd.v - 0.5 * msg_car_cmd.omega * self._baseline) / self._radius
# conversion from motor rotation rate to duty cycle
# u_r = (gain + trim) (v + 0.5 * omega * b) / (r * k_r)
u_r = omega_r * k_r_inv
# u_l = (gain - trim) (v - 0.5 * omega * b) / (r * k_l)
u_l = omega_l * k_l_inv
# limiting output to limit, which is 1.0 for the duckiebot
u_r_limited = self.trim(u_r, -self._limit.value, self._limit.value)
u_l_limited = self.trim(u_l, -self._limit.value, self._limit.value)
# Put the wheel commands in a message and publish
msg_wheels_cmd = WheelsCmdStamped()
msg_wheels_cmd.header.stamp = msg_car_cmd.header.stamp
if u_r_limited > 0 and u_r_limited < 0.1:
u_r_limited = 0.1
if u_r_limited < 0 and u_r_limited > -0.1:
u_r_limited = -0.1
if u_l_limited > 0 and u_l_limited < 0.1:
u_l_limited = 0.1
if u_l_limited < 0 and u_l_limited > -0.1:
u_l_limited = -0.1
msg_wheels_cmd.vel_right = u_r_limited
msg_wheels_cmd.vel_left = u_l_limited
self.pub_wheels_cmd.publish(msg_wheels_cmd)
# FORWARD KINEMATICS PART
# Conversion from motor duty to motor rotation rate
omega_r = msg_wheels_cmd.vel_right / k_r_inv
omega_l = msg_wheels_cmd.vel_left / k_l_inv
# Compute linear and angular velocity of the platform
v = (self._radius * omega_r + self._radius * omega_l) / 2.0
omega = (self._radius * omega_r - self._radius * omega_l) / self._baseline
# Put the v and omega into a velocity message and publish
msg_velocity = Twist2DStamped()
msg_velocity.header = msg_wheels_cmd.header
msg_velocity.v = v
msg_velocity.omega = omega
self.pub_velocity.publish(msg_velocity)
def __init__(self):
self.node_name = "Slim Parking"
self.active = False
self.filter = None
self.updateParams(None)
self.active_mode = False
self.t_last_update = rospy.get_time()
self.velocity = Twist2DStamped()
self.d_median = []
self.phi_median = []
self.latencyArray = []
self.park_timeout = 10
rospy.set_param("~park_timeout", self.park_timeout)
# Define Constants
self.curvature_res = self.filter.curvature_res
# Set parameters to server
rospy.set_param(
'~curvature_res',
self.curvature_res) #Write to parameter server for transparancy
self.pub_in_lane = rospy.Publisher("~in_lane_parking",
BoolStamped,
queue_size=1)
# Subscribers
self.sub = rospy.Subscriber(
"/articuno/ground_projection/lineseglist_out",
SegmentList,
self.processSegments,
queue_size=1)
self.sub_velocity = rospy.Subscriber(
"/articuno/car_cmd_switch_node/cmd", Twist2DStamped,
self.updateVelocity)
self.sub_change_params = rospy.Subscriber(
"/articuno/lane_filter_node/change_params", String,
self.cbChangeParams)
# Publishers
self.pub_lane_pose = rospy.Publisher(
"/articuno/lane_filter_node/lane_pose", LanePose, queue_size=1)
self.pub_belief_img = rospy.Publisher(
"/articuno/lane_filter_node/belief_img", Image, queue_size=1)
self.pub_seglist_filtered = rospy.Publisher(
"/articuno/lane_filter_node/seglist_filtered",
SegmentList,
queue_size=1)
self.pub_ml_img = rospy.Publisher("/articuno/lane_filter_node/ml_img",
Image,
queue_size=1)
self.pub_entropy = rospy.Publisher(
"/articuno/lane_filter_node/entropy", Float32, queue_size=1)
self.pub_parking_detection = rospy.Publisher("~parking_line",
BoolStamped,
queue_size=1)
self.pub_parking_on = rospy.Publisher("/articuno/parking_on",
BoolStamped,
queue_size=1)
self.pub_exit_from_parking = rospy.Publisher("~exit_from_parking",
BoolStamped,
queue_size=1)
# FSM
self.sub_switch = rospy.Subscriber("~switch",
BoolStamped,
self.cbSwitch,
queue_size=1)
self.sub_fsm_mode = rospy.Subscriber("/articuno/fsm_node/mode",
FSMState,
self.cbMode,
queue_size=1)
# timer for updating the params
self.timer = rospy.Timer(rospy.Duration.from_sec(1.0),
self.updateParams)
def __init__(self):
self.node_name = "Lane Filter"
self.active = True
self.filter = None
self.matrix_delta_d = 0.2
self.matrix_delta_phi = 0.1
self.matrix_mesh_size = 1
self.updateParams(None)
self.t_last_update = rospy.get_time()
self.velocity = Twist2DStamped()
self.d_median = []
self.phi_median = []
self.latencyArray = []
# Since we have knowledge of maximum linear and angular velocity, we can have some idea
# of when the enstimate are too far from what we may expect, and correct them
# (basically applying a low pass filter)
self.last_d = []
self.last_phi = []
self.veh_name = rospy.get_namespace().strip("/")
self.omega_max = rospy.get_param("/" + self.veh_name +
"/lane_controller_node/omega_max")
self.omega_min = rospy.get_param("/" + self.veh_name +
"/lane_controller_node/omega_min")
self.v_ref = rospy.get_param("/" + self.veh_name +
"/lane_controller_node/v_bar")
self.gain = rospy.get_param("/" + self.veh_name +
"/kinematics_node/gain")
# Define Constants
self.curvature_res = self.filter.curvature_res
# Set parameters to server
rospy.set_param(
'~curvature_res',
self.curvature_res) #Write to parameter server for transparancy
self.pub_in_lane = rospy.Publisher("~in_lane",
BoolStamped,
queue_size=1)
# Subscribers
self.sub = rospy.Subscriber("~segment_list",
SegmentList,
self.processSegments,
queue_size=1)
self.sub_velocity = rospy.Subscriber("~car_cmd", Twist2DStamped,
self.updateVelocity)
self.sub_change_params = rospy.Subscriber("~change_params", String,
self.cbChangeParams)
# Publishers
self.pub_lane_pose = rospy.Publisher("~lane_pose",
LanePose,
queue_size=1)
self.pub_belief_img = rospy.Publisher("~belief_img",
Image,
queue_size=1)
self.pub_seglist_filtered = rospy.Publisher("~seglist_filtered",
SegmentList,
queue_size=1)
self.pub_ml_img = rospy.Publisher("~ml_img", Image, queue_size=1)
self.pub_entropy = rospy.Publisher("~entropy", Float32, queue_size=1)
# FSM
self.sub_switch = rospy.Subscriber("~switch",
BoolStamped,
self.cbSwitch,
queue_size=1)
self.sub_fsm_mode = rospy.Subscriber("~fsm_mode",
FSMState,
self.cbMode,
queue_size=1)
self.active = True
# timer for updating the params
self.timer = rospy.Timer(rospy.Duration.from_sec(1.0),
self.updateParams)
def processStick(self, msg): car_cmd_msg = Twist2DStamped() car_cmd_msg.v = msg.axes[1] * self.v_gain car_cmd_msg.omega = msg.axes[3] * self.omega_gain self.pub_car_cmd.publish(car_cmd_msg)
def drive_curve(self):
car_control_msg = Twist2DStamped()
car_control_msg.v = self.defaultvelocity
car_control_msg.omega = 1.0
self.pub_move.publish(car_control_msg)
rospy.sleep(1)