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)