def set_steer(data): car_ctl = AckermannDrive() car_msg = AckermannDriveStamped() steering_angle = rospy.Publisher('/vesc/racecar/ackermann_cmd_mux/input/teleop', AckermannDriveStamped, queue_size=1) k_distance = 6.0 k_angle = 2.0 if data.data[0] and data.data[3]: print "section 1" #error = k_distance * (data.data[4] - data.data[1]) error = k_angle * (90.0 - data.data[2]) print "k_distance", k_distance, "data.data[4]", data.data[4], "data_data[1]", data.data[1], "error", error elif data.data[0] and not data.data[3]: print "section 2" error = k_angle * (90.0 - data.data[2]) elif data.data[3] and not data.data[0]: print "section 3" error = k_angle * (-90.0 - data.data[5]) else: print "section 4" pass #car_ctl.acceleration = 0.0 car_ctl.speed = 0.8 car_ctl.steering_angle = error #steering_angle.publish('{drive: {steering_angle: ' + str(error) + ', speed: ' + str(prev_speed) + '}}') #rospy.Publisher('ackermann_cmd', AckermannDriveStamped) "publish pilot command message" car_msg.header.stamp = rospy.Time.now() car_msg.header.frame_id = "wall_follower" car_msg.drive = car_ctl print "right before publish, steering_angle: ", error steering_angle.publish(car_msg)
def filter_command(data): global flag_move global flag_stop pub_movement = rospy.Publisher('/vesc/ackermann_cmd_mux/input/navigation', AckermannDriveStamped, queue_size=1) newData = AckermannDriveStamped() newData.header = data.header #print "flag is ", flag_stop #if len(data.header.frame_id) > 0 or not flag_stop: newData.drive = data.drive pub_movement.publish(newData)
def control(self,data): car_ctl = AckermannDrive() car_msg = AckermannDriveStamped() object_angle=data.data[0]*math.pi/180 object_distance=data.data[1] if data.data[0] != -1: desired = -.3 #how wide to clear cone multiplier=1 if(not self.right): desired = .3 desired_distance=.1 distance_y = object_distance * math.cos(object_angle) if distance_y < .4: desired *= 2./3 steering_gain=.1 steering_max = .34 actual=multiplier * math.sin(object_angle)*object_distance #if actual < desired and object_distance > desired * 1.2: # actual = object_distance error=desired-actual print actual, " actual" print object_distance, " object_distance" print steering_gain * error, " steering" steering_angle = multiplier*steering_gain * error car_max_speed=1.0 #desired_distance=.1 #distance_y = object_distance * math.cos(object_angle) distance_error=distance_y-desired_distance print 'distance y ', distance_y speed_gain=.4 speed=distance_error*speed_gain print 'speed ', speed #speed=.3 car_ctl.speed = speed car_ctl.steering_angle = steering_angle car_msg.header.stamp = rospy.Time.now() car_msg.header.frame_id = "IPSController" car_msg.drive = car_ctl self.control_pub.publish(car_msg) else: car_ctl.steering_angle = 0 car_ctl.speed = 0 #change for, later crawl forward car_msg.header.frame_id = "IPSController" car_msg.drive = car_ctl self.control_pub.publish(car_msg)
def control(self,data): car_ctl = AckermannDrive() car_msg = AckermannDriveStamped() [centerX,centerY,width,height,actual_angle]=data.data if actual_angle != -1 and width < 180 and height < 250: desired_angle = 0 steering_gain=.4 steering_range = .34 error = desired_angle - actual_angle steering_angle = steering_gain * error car_ctl.speed = 0.3 car_ctl.steering_angle = steering_angle car_msg.header.stamp = rospy.Time.now() car_msg.header.frame_id = "IBSController" car_msg.drive = car_ctl self.control_pub.publish(car_msg) else: car_ctl.speed = 0.0 car_msg.header.frame_id = "IBSController" car_msg.drive = car_ctl self.control_pub.publish(car_msg)
#!/usr/bin/env python import random import rospy from ackermann_msgs.msg import AckermannDriveStamped from sensor_msgs.msg import LaserScan from nav_msgs.msg import Odometry drive_msg = AckermannDriveStamped() drive_msg.header.seq = 0 drive_msg.header.stamp = rospy.Time() drive_msg.header.frame_id = 'test' drive_msg.drive.steering_angle = 0 drive_msg.drive.steering_angle_velocity = 1 drive_msg.drive.speed = 3 drive_msg.drive.acceleration = 0 drive_msg.drive.jerk = 0 pub = rospy.Publisher('/drive', AckermannDriveStamped, queue_size=10) def driver_callback(data): global s_angle global car_state # rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.ranges) x = [min(xi, max_range) for xi in data.ranges] lx = len(x) N = 5 x_r = [] for i in range(N): x_r[i] = np.average(x[int(lx * i / N):int(lx * (i + 1) / N)])
def MoveToBlob(self, blob_detect): #Moves to the largest blob. global Wall_Follow, Turning drive_msg = AckermannDriveStamped( ) #Sets the drive message to the AckermannDriveStamped message type blob_Color = ColorRGBA() drive_msg.drive.speed = self.speed #Sets a field of the drivemsg message. drive_msg.drive.steering_angle = -.06 #Gets the position of the largest blob. The first will be the largest blob because the blobs are sorted by size. (index 0 is biggest size). if (len(blob_detect.colors) == 0 ): #Make sure the blob detect arrays are not empty self.pub_mov_func.publish(drive_msg) rospy.loginfo("No blobs detected") return #exit turning_start_time = rospy.get_time() #if there are blobs largest_blob_area = blob_detect.sizes[0].data largest_blob_pos = blob_detect.locations[0] blob_x_pos = largest_blob_pos.x #This will be in between 0 and 1 (inclusive) (0 is the left and 1 is the right) blob_y_pos = largest_blob_pos.y #Same thing here blob_color = blob_detect.colors[0] #In rgba if (Wall_Follow == True and Turning == False): #Stage 4. Stop subscriptions from this node. rospy.loginfo( "Stopping control system node and publishing to wall follower." ) #Intiate wall follow by publishing the color to the wall follow's topic. color = "r" if (blob_color.r == 255) else "g" self.pub_blob_color_func(color) #stop further subscriptions and basically end the node self.blob_detect.unregister() elif (Wall_Follow == False and Turning == False): if (largest_blob_area > 8000 ): #Stage 2. We are close to the blob, so we initiate turning #PUT TURN HERE rospy.loginfo("Intializing Turning.") Turning = True turning_start_time = rospy.get_time() else: #Stage 1 #We are far from the blob, so we continue to navigate towards the blob. #While we are relatively far away from the blob (ie blob size is small relative to screen), we drive towards the center of the blob. rospy.loginfo("Navigating to blob") if (blob_x_pos > 0.60): #If Blob is On the right side drive_msg.drive.steering_angle = .3 #Turn left elif (blob_x_pos < 0.40): #If Blob is on the left side. drive_msg.drive.steering_angle = -.3 #Turn right else: #If blob_x_pos (the x component of he blob's centroid) is in the middle 200 pixels, just move forward drive_msg.drive.steering_angle = 0 if (Turning == True): #Stage 3. Start turning rospy.loginfo("Turning") if (turning_start_time + 4 > rospy.get_time() ): #Keeps turning for three seconds (approx) if (blob_color.r == 255): drive_msg.drive.steering_angle = 1.5 elif (blob_color.g == 255): drive_msg.drive.steering_angle = -1.5 #If the blob color is red, turn left, else if it is green, turn right. else: Turning = False #After turning process finishes, change Turning to False and Wall_Follow to true Wall_Follow = True #publish the message self.pub_mov_func.publish(drive_msg)
def pub_callback(self, event): ackermann_cmd_msg = AckermannDriveStamped() ackermann_cmd_msg.drive.speed = self.speed ackermann_cmd_msg.drive.steering_angle = self.steering_angle self.motors_pub.publish(ackermann_cmd_msg)
def shutdown(self): rospy.loginfo("Stopping the robot...") # always make sure to leave the robot stopped self.drive.publish(AckermannDriveStamped()) rospy.sleep(1)
if(msg.ranges[215] > msg.ranges[135]): ackMsg.drive.steering_angle = -90 else: ackMsg.drive.steering_angle = 90 ackMsg.drive.speed = 2 if(msg.ranges[170]<0.5 or msg.ranges[180]<0.5 or msg.ranges[190]<0.5): ackMsg.drive.speed = -4 ackMsg.drive.steering_angle = 90 pub.publish(ackMsg) if __name__ =='__main__': ackMsg = AckermannDriveStamped() rospy.init_node('proj', anonymous=True) #subscribe to listener commands sub = rospy.Subscriber('chatter',String,listenerCallback) #subscribe to lidar sub2 = rospy.Subscriber('/agent1/scan',LaserScan,wanderCallback) #publish to hamster wheels(hamster movement control) pub = rospy.Publisher('/agent1/ackermann_cmd',AckermannDriveStamped,queue_size=1) while not rospy.is_shutdown(): rospy.spin()
def calculate_pure_pursuit(self): L = 0.3302 # self.speed2 = self.speed # default: 2.0 # look-ahead distance ld based on the speed of the vehicle # ld = self.coefficient * self.v + 0.5 # default: 1.0 ld = self.v * 0.15 + 0.1 diff_min = ld pp = self.path_position self.length2 = len(pp) while self.length2 == 0: pp = self.path_position self.length2 = len(pp) #print "duljina:", self.length2 self.velocity_controller(pp) rob_x = self.robot_x rob_y = self.robot_y rob_qua_x = self.robot_qua_x rob_qua_y = self.robot_qua_y rob_qua_z = self.robot_qua_z rob_qua_w = self.robot_qua_w # find position(x,y) on the path that is ld away from the robot. for i in range(0, self.length2): path_x = pp[i][0] path_y = pp[i][1] path_base_link = self.calculate_pose(path_x, path_y) if path_base_link.pose.position.x < -0.05: continue dist_rp = get_distance(rob_x, rob_y, path_x, path_y) diff = abs(dist_rp - ld) if diff < diff_min: diff_min = diff self.goal_x = path_x self.goal_y = path_y #show a point in Rviz on the Pure Pursuit goal position goal = PointStamped() goal.header.stamp = rospy.Time.now() goal.header.frame_id = "/map" goal.point.x = self.goal_x goal.point.y = self.goal_y goal.point.z = 0 self.pub2.publish(goal) # calculate alpha1, alpha2 to get steering angle of the robot # alpha1 - angle between robot and goal position in radians. alpha1 = get_angle_rad(rob_x, self.goal_x, rob_y, self.goal_y) quaternion = [rob_qua_x, rob_qua_y, rob_qua_z, rob_qua_w] rpy = tf.transformations.euler_from_quaternion(quaternion) # alpha2 - robots orientation with respect to map cordinate frame alpha2 = rpy[2] alpha = alpha1 - alpha2 self.delta = math.atan((2 * L * math.sin(alpha)) / ld) # publish speed and steering angle of the robot whereto = AckermannDriveStamped() whereto.header.stamp = rospy.Time.now() whereto.drive.steering_angle = self.delta whereto.drive.steering_angle_velocity = 0 whereto.drive.speed = self.v whereto.drive.acceleration = 0 whereto.drive.jerk = 0 self.pub.publish(whereto)
def drive(steer_val, car_run_speed): global ack_publisher ack_msg = AckermannDriveStamped() ack_msg.drive.steering_angle = steer_val ack_msg.drive.speed = car_run_speed ack_publisher.publish(ack_msg)
def send_drive_command(self, speed, steering_angle): ack_msg = AckermannDriveStamped() ack_msg.drive.speed = speed ack_msg.drive.steering_angle = steering_angle self.ack_msg = ack_msg
def pursuit_callback(self, msg): """ given pose: 1) find closest line segment from trajectory 2) pure pursuit 3) ??? 4) profit (set angle of ackermann message and publish) """ if not self.initialpose: return if not self.num_pts: return #get pose np_pose_now = np.array([[msg.pose.pose.position.x, msg.pose.pose.position.y]]) theta_now = np.arctan2(msg.pose.pose.orientation.z, msg.pose.pose.orientation.w)*2 # now we have x,y,theta in world frame seg, close_pt = self.get_closest_seg(np_pose_now) self.prev_seg = seg if self.do_rviz: marker = Marker() marker.header.frame_id = "/map" marker.type = marker.POINTS marker.action = marker.ADD marker.points = [] marker.colors = [] marker.scale.x = 0.5 marker.scale.y = 0.5 target_viz = Point() target_viz.x = close_pt[0] target_viz.y = close_pt[1] target_viz.z = .05 marker.colors.append(ColorRGBA(r=0, g=0, b=1, a=1)) marker.points.append(target_viz) self.closest_pub.publish(marker) if not self.wrap and seg == self.num_pts-2: self.speed = 0 self.stop = True ## FINDING TARGET POINT ## ## circle intersecting line segment stuff using quadratics ## # everything so far in the world frame def next_seg(seg): return (seg+1)%self.num_pts q = np_pose_now.reshape((2,)) # initial line seg to check bw # p1 = pt.reshape((2,)) # dont check behind car p1 = self.trajectory.np_points[seg] p2 = self.trajectory.np_points[next_seg(seg)] self.kdd = MAX_KDD self.speed = MAX_SPEED scale = ANGLE_SCALE_MAX while not self.stop: seg_len = np.sqrt(self.trajectory_dists[seg]) #if seg_len >= 2: # self.kdd = 1.5 if seg_len <= MAX_SPEED*SMALL_SEG: self.kdd = MIN_KDD self.speed = SLOWDOWN_SPEED scale = ANGLE_SCALE_MIN self.lookahead = self.speed*self.kdd vec = p2-p1 A = np.dot(vec, vec) B = 2*(np.dot(vec, p1 - q)) C = np.dot(p1, p1) + np.dot(q,q) - np.dot(2*p1,q) - self.lookahead**2 disc = B**2 - 4*A*C while disc <= 0: # no intersection # extend lookahead distance disc = self.adjust_kdd(A, B, p1, q) #rospy.loginfo("Point: " + str(p1) + str(p2)) #rospy.loginfo(str(disc)) t1 = (-B + np.sqrt(disc))/(2*A) t2 = (-B - np.sqrt(disc))/(2*A) #rospy.loginfo("Found expanded disc") while t1 <= 0: # if segment extended enough backwards it would touch, so we shouldn't move to # next segment, increase lookahead again disc = self.adjust_kdd(A, B, p1, q) t1 = (-B + np.sqrt(disc))/(2* A) t2 = (-B - np.sqrt(disc))/(2*A) if t1 >= 1: # also no intersection, but bc segment is not long enough # here we should probably do the moving on to next line segment # start point of next segment is endpoint of this segment seg = (seg+1)%self.num_pts p1 = p2 p2 = self.trajectory.np_points[next_seg(seg)] continue #else: # we have intersection! yay target_pos = p1 + t1*vec # position on line segment if 0<=t1<=1 target_theta = np.arctan2(p1[1]-p2[1], p1[0]-p2[0]) # direction of line segment break if not self.stop: # convert to robot frame target_coord = get_robot_frame_coord((target_pos, target_theta), (np_pose_now, theta_now)) target_coord = self.adjust_target_coord_for_obstacles(target_coord, STEERING_ANGLE) if self.do_rviz: marker = Marker() marker.header.frame_id = "/base_link" marker.type = marker.POINTS marker.action = marker.ADD marker.points = [] marker.colors = [] marker.scale.x = 0.5 marker.scale.y = 0.5 target_viz = Point() target_viz.x = target_coord[0] target_viz.y = target_coord[1] target_viz.z = .05 marker.colors.append(ColorRGBA(r=1, g=0, b=0, a=1)) marker.points.append(target_viz) self.nearest_pub.publish(marker) rospy.loginfo("TARGET: " + str(target_coord)) target_th = np.arctan2(target_coord[1],target_coord[0]) if abs(target_th) > SHARP_TURN: self.kdd = MIN_KDD self.speed = SLOWDOWN_SPEED scale = ANGLE_SCALE_MIN # find turning radius using method in that paper linked in piazza R_track = self.lookahead**2/(2*abs(target_coord[1])) # now get desired turning angle! delta = np.sign(target_coord[1])*np.arctan2(self.wheelbase_length, R_track) if self.stop: delta = 0 drive_msg = AckermannDriveStamped() drive_msg.header.stamp = msg.header.stamp drive_msg.drive.speed = self.speed drive_msg.drive.steering_angle = -1*scale*delta #NEEDS TO BE SET self.drive_pub.publish(drive_msg)
def drive(self): new_msg = AckermannDriveStamped() new_msg.drive.speed = self.speed new_msg.drive.steering_angle = self.steering_angle self.pub.publish(new_msg)
def drive_cb(event): global init_flag, waypoint if not init_flag: x = pos[0] y = pos[1] path.append((x,y)) init_flag = True return x = pos[0] y = pos[1] path.append((x,y)) curr_loc = path[-1] prev_loc = path[-2] next_loc = get_next_loc(curr_loc, prev_loc) #d_target = get_distance(waypoint, curr_loc) a_error = get_angle_between_3_pts(curr_loc, waypoint, next_loc) if a_error > math.pi: a_error = a_error - 2*math.pi elif a_error < -math.pi: a_error = a_error + 2*math.pi d_target = get_distance(waypoint, curr_loc) #print(d_target, reached[0]) if d_target < EPSILON_RADIUS and single_ack_entry_flag[0]: #print ("WAYPOINT REACHED, d_target = {}".format(d_target)) #waypoint = points.pop(0) reached[0] = True a_integral = 0.0 d_integral = 0.0 a_prev = 0.0 d_prev = 0.0 #continue #print("STOPPING") #stop() stop_cmd = True; print("ACK SENT") ack_msg = String(data="TRUE") ack_publisher.publish(ack_msg) single_ack_entry_flag[0] = False #stop() #continue if stop_cmd: print("STOPPING") speed = STOP stop_cmd = False; msg = AckermannDriveStamped() msg.drive.speed = speed msg.drive.steering_angle = 0 pub.publish(msg) if single_ack_entry_flag[0]: print("Driving") d_prev = d_target d_integral += d_target a_prev = a_error a_integral += a_error if a_error < -EPSILON_ANGLE: kLeft = 0 kRight = 1 elif a_error > EPSILON_ANGLE: kRight = 0 kLeft = 1 else: kLeft = 0 kRight = 0 x_prev = x y_prev = y msg = AckermannDriveStamped() # reset speed & direction accordingly. speed += DELTA_SPEED_DRIVE * (kUp - kDown) direction += -DELTA_DIRECTION * (kRight - kLeft) speed = max(min(speed, 2), -2) direction = max(min(direction, 0.35), -0.35) msg.drive.speed = speed msg.drive.steering_angle = direction # print(kUp, kDown, kLeft, kRight) #print(speed, direction) # sys.stdout.flush() pub.publish(msg)
def stop(): #print("STOPPING") msg = AckermannDriveStamped() msg.drive.speed = STOP msg.drive.steering_angle = 0 pub.publish(msg)
def JoyCB(data): #Use global/static vars global controllerInitializationStarted global controllerInitialized global lastSteeringState global steeringState global lastThrottleState global throttleState global lastFineSteeringState global fineSteeringState #steering direction if (data.buttons[2] != lastSteeringState and lastSteeringState == 0): steeringState = -steeringState rospy.logdebug("Changing steering direction") lastSteeringState = data.buttons[2] #driving mode if (data.buttons[0] != lastThrottleState and lastThrottleState == 0): throttleState = not throttleState rospy.logdebug("Changing driving mode") lastThrottleState = data.buttons[0] #fine steering mode if (data.buttons[3] != lastFineSteeringState and lastFineSteeringState == 0): fineSteeringState = not fineSteeringState rospy.logdebug("Changing steering mode") lastFineSteeringState = data.buttons[3] #apply modificaitons to throttle based on drive mode selection #acceptible values for physical jetson car are +-5 (not in m/s) for now if not controllerInitialized: throttleVal = 0 if (controllerInitializationStarted and data.axes[2] == 1 and data.axes[5] == 1): #controller should be corrected by now controllerInitialized = True elif (data.axes[2] == -1 and data.axes[5] == -1): controllerInitializationStarted = True elif throttleState: throttleVal = map(data.axes[5], 1, -1, 0, 2) + map( data.axes[2], 1, -1, 0, 2) #range is 0 to 4 else: throttleVal = map(data.axes[5], 1, -1, 0, 2) - map( data.axes[2], 1, -1, 0, 2) #range is -2 to 2 # #handle braking if applied (doesn't correlate to anything in simulation) # if(data.buttons[4] == 1 or data.buttons[5] == 1): # throttleVal = 6 #out of range value triggers active brake in ARC1 firmware #apply modifications to steering based on steering mode selection if fineSteeringState: steeringVal = data.axes[0] #range is -1 to 1 else: steeringVal = data.axes[0] / 2 #range is -0.5 to 0.5 msg = AckermannDriveStamped() msg.header.stamp = rospy.Time.now() msg.header.frame_id = "base_link" msg.drive.acceleration = 0 #drive acceleration unknown msg.drive.speed = throttleVal msg.drive.jerk = 0 #jerk unknown msg.drive.steering_angle = steeringVal * steeringState #steering state is 1 or -1 to flip steering if needed msg.drive.steering_angle_velocity = 0 #angle velocity unknown pub.publish(msg)
def perform_retrace(waypoint, i): print waypoint, ODOM_INF global pix_bot_center, pix_bot_theta, pix_bot_velocity, cmd_pub rospy.wait_for_message(ODOM_INF, Odometry, 5) dx = waypoint[0] - pix_bot_center.position.x dy = waypoint[1] - pix_bot_center.position.y target_distance = math.sqrt(dx*dx + dy*dy) cmd = AckermannDriveStamped() cmd.header.stamp = rospy.Time.now() cmd.header.frame_id = "base_link" cmd.drive.speed = pix_bot_velocity.linear.x cmd.drive.acceleration = max_acc[i] delta_error = 0.0 last_error = 0.0 while target_distance > retrace_waypoint_tol: dx = pix_bot_center.position.x - waypoint[0] dy = pix_bot_center.position.y - waypoint[1] lookahead_dist = np.sqrt(dx * dx + dy * dy) lookahead_theta = math.atan2(abs(dy), abs(dx)) # lookahead_theta = waypoint[2] alpha = shortest_angular_distance(lookahead_theta, pix_bot_theta) cmd.header.stamp = rospy.Time.now() cmd.header.frame_id = "base_link" # Publishing constant speed of 1m/s cmd.drive.speed = 1 # Reactive steering # print(pix_bot_theta, lookahead_theta ) # print("ALPHA") # print(alpha) if alpha < 0: st_ang = max(-max_steering_angle_ret, alpha) else: st_ang = min(max_steering_angle_ret, alpha) cmd.drive.steering_angle = st_ang target_distance = math.sqrt(dx * dx + dy * dy) error_speed = target_distance if last_error == 0: pass else: delta_error = error_speed - last_error velocity = Kp * error_speed + Kd * delta_error MIN_VEL = 0.1 MAX_VEL = 0.75 velocity = max(MIN_VEL, min(MAX_VEL , velocity)) cmd.drive.speed = -velocity cmd_pub.publish(cmd) rospy.wait_for_message(ODOM_INF, Odometry, 5)
def lidar_callback(data): global prev_threshold_angle, integral_prior gauss_range = np.arange(0, 1080, 1) gaussian_weights = gaussian(gauss_range, 540, 150) gaussian_weights = gaussian_weights + (1 - gaussian_weights[0]) #limited_ranges = limited_ranges*gaussian_weights limited_ranges = np.asarray(data.ranges) limited_ranges[0:right_extreme] = 0.0 limited_ranges[(left_extreme + 1):] = 0.0 limited_ranges[(left_extreme + 1)] = limited_ranges[left_extreme] indices = np.where(limited_ranges >= lidar_max_range)[0] limited_ranges[indices] = lidar_max_range - 0.1 #data.range_m - 0.1 disparities = find_disparities(limited_ranges, disparity_threshold) # Experimental section right = [] left = [] front = [] # Handling Multiple Disparities a = 14 for i in disparities: if i > 540 - a and i < 540 + a: front.append(i) elif i < 540: right.append(i) #print(right) else: left.append(i) if len(right) > len(left) and len(right) > len(front): disparities = right elif len(left) > len(right) and len(left) > len(front): disparities = left else: disparities = front new_ranges = extend_disparities(limited_ranges, disparities, car_width) new_ranges = new_ranges * gaussian_weights max_value = max(new_ranges) target_distances = np.where( new_ranges >= (max_value - 1))[0] #index values driving_distance_index, chosen_index = calculate_target_distance( target_distances) driving_angle = calculate_angle(driving_distance_index) thresholded_angle = threshold_angle(driving_angle) behind_car = np.asarray(data.ranges) behind_car_right = behind_car[0:right_extreme] behind_car_left = behind_car[(left_extreme + 1):] #change the steering angle based on whether we are safe thresholded_angle = adjust_turning_for_safety(behind_car_left, behind_car_right, thresholded_angle) velocity = calculate_min_turning_radius(thresholded_angle) velocity = threshold_speed(velocity, new_ranges[driving_distance_index], new_ranges[540]) # Publish drive and Steer msg = AckermannDriveStamped() Kd = 0.02 #0.055 Ki = 0.075 Kp = 0.75 integral = integral_prior + (thresholded_angle * 0.004) msg.drive.steering_angle = Kp * thresholded_angle + ( Kd * (thresholded_angle - prev_threshold_angle) / 0.004) + Ki * integral prev_threshold_angle = thresholded_angle integral_prior = integral msg.drive.speed = velocity pub_drive_param.publish(msg) current_time = rospy.Time.now() # Publish Modified Scan scan = LaserScan() scan.header.stamp = current_time scan.header.frame_id = 'ego_racecar/laser' scan.angle_min = -1.57 scan.angle_max = 1.57 scan.angle_increment = 3.14 / 1080 scan.time_increment = (1.0 / 250) / (1080) scan.range_min = 0.0 scan.range_max = 270.0 scan.ranges = new_ranges scan.intensities = [] scan_pub.publish(scan) # Publish marker markermsg = Marker() markermsg.header.frame_id = "ego_racecar/laser" markermsg.header.stamp = current_time markermsg.id = 0 markermsg.type = 2 markermsg.action = 0 markermsg.pose.position.x = np.sin(math.pi - np.deg2rad(( (driving_distance_index - 180) / 4))) * new_ranges[driving_distance_index] markermsg.pose.position.y = np.cos(math.pi - np.deg2rad(( (driving_distance_index - 180) / 4))) * new_ranges[driving_distance_index] markermsg.pose.position.z = 0 markermsg.scale.x = 0.5 markermsg.scale.y = 0.5 markermsg.scale.z = 0.5 markermsg.pose.orientation.x = 0 markermsg.pose.orientation.y = 0 markermsg.pose.orientation.z = 0 markermsg.pose.orientation.w = 1.0 markermsg.color.r = 0.0 markermsg.color.g = 1.0 markermsg.color.b = 0.0 markermsg.color.a = 1.0 markermsg.lifetime = rospy.Duration() marker_pub.publish(markermsg) """Scale the speed in accordance to the forward distance"""
def generate_drive_stamp(self, steering): stamp = AckermannDriveStamped() stamp.drive.steering_angle = steering stamp.drive.speed = -2. rospy.logwarn("steering: %s", steering) return stamp
import sys from ackermann_msgs.msg import AckermannDrive from ackermann_msgs.msg import AckermannDriveStamped __author__ = "Ilyas M Abbas ([email protected])" if __name__ == '__main__': if len(sys.argv) != 3: print sys.argv sys.exit(1) rospy.init_node('ackermann_test_publisher', anonymous=True) pub = rospy.Publisher('auto_cmd', AckermannDriveStamped, queue_size=1) ack_msg = AckermannDriveStamped() ack_msg.drive = AckermannDrive() ack_msg.drive.speed = float(sys.argv[1]) ack_msg.drive.steering_angle = float(sys.argv[2]) then = rospy.Time.now() r = rospy.Rate(10) while not rospy.is_shutdown(): if ((rospy.Time.now() - then).to_sec() > 10 and (rospy.Time.now() - then).to_sec() < 14): ack_msg.drive.speed = 0 ack_msg.drive.steering_angle = 0 pub.publish(ack_msg) r.sleep()
#!/usr/bin/env python3 import rospy, math, serial import time import os from collections import deque # ROS Msgs from std_msgs.msg import Int16 from ackermann_msgs.msg import AckermannDriveStamped steering_pub = rospy.Publisher('pot_angle', AckermannDriveStamped, queue_size=10) steering_msg = AckermannDriveStamped() #REGION SerialInit. command ='udevadm info -e' p=os.popen(command,"r") data = p.read() lines= data.split('\n\n') default_port='/dev/ttyACM9' for i in range(len(lines)): lines[i].replace("P","\nP") # if 'Uno' in lines[i] and '855313036' in lines[i]: if 'Uno' in lines[i] and 'dev/ttyACM' in lines[i]: start = lines[i].find('/dev/ttyACM') if (start != -1): default_port = lines[i][start:start+12] print("Arduino port was found after search: ", default_port, "Srart was: ", start) ard = serial.Serial(default_port,9600,timeout=0.0001) ard.flushInput() #ENDREGION SerialInit.
def send_action(self, steering_angle, throttle): ack_msg = AckermannDriveStamped() ack_msg.header.stamp = rospy.Time.now() ack_msg.drive.steering_angle = steering_angle ack_msg.drive.speed = throttle self.ack_publisher.publish(ack_msg)
rospy.Subscriber('/sim_car_pose/pose', PoseStamped, cb_heading) # Publish initial position init_pose = PoseWithCovarianceStamped() init_pose.header.stamp = rospy.Time.now() init_pose.header.frame_id = 'map' init_pose.pose.pose.position.x = 0.0 init_pose.pose.pose.position.y = 0.0 init_pose.pose.pose.orientation = utils.angle_to_quaternion(0.0) rospy.sleep(1) # publisher needs some time before it can publish rospy.loginfo('Publish initialpose') p_initpose.publish(init_pose) rospy.sleep(1) # Publish drive info cmd = AckermannDriveStamped() cmd.header.frame_id = 'map' cmd.drive.steering_angle = 0.1 cmd.drive.speed = 2 r = rospy.Rate(20) while not rospy.is_shutdown(): if heading < -0.5 and circle == 0: circle = 1 elif heading > -0.1 and circle == 1: cmd.drive.steering_angle = -cmd.drive.steering_angle circle = 2 elif heading > 0.5 and circle == 2: circle = 3 elif heading < 0.1 and circle == 3: break p_nav0.publish(cmd)