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