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 timerCallBack(self,event): state=self.path[self.state] steer_angle = -1.*state[3] vel = self.EXT_VEL newControl = AckermannDriveStamped() newDrive = AckermannDrive() newDrive.steering_angle = steer_angle newDrive.speed = vel self.newControl.drive = newDrive self.state+=1
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)
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.control_pub.publish(drive_msg_stamped)
def otherCode(self, data): pubAcker = rospy.Publisher('/autodrive', AckermannDrive, queue_size=10) # init publisher ack_msg = AckermannDrive() # initialize ackermann message i = 0 speed = 1 #setting the speed totalDist = [] #Setting arrays angs = [] #for the angles that are road while i < len(data.ranges): totalDist.append(data.ranges[i]) if totalDist[i] > 1000000: totalDist[i] = 100000 if totalDist[i] > 5 and totalDist[ i] < 8: # is the point in the road? angs.append(math.radians( (i - 380.0) * (190.0 / 760.0))) #log the angle (radians) i += 1 ack_msg.speed = speed # set speed to the ackermann message ack_msg.steering_angle = median( angs) # set the angle to the ackermann message pubAcker.publish(ack_msg) rospy.loginfo(median(angs))
def callback(data): global speed global steering_angle global distance_set global acceleration distance_set = [get_range(data, i) for i in range(90, -91, -1)] steering_angle = 0.0 acceleration = 0.0 perception(data) if simulator_mode == True: drive_msg = AckermannDrive(steering_angle=steering_angle, speed=speed, acceleration=acceleration) drive_st_msg = AckermannDriveStamped(drive=drive_msg) drive_pub_simulator.publish(drive_st_msg) else: drive_msg = AckermannDrive(steering_angle=steering_angle, speed=speed, acceleration=acceleration) drive_pub.publish(drive_msg)
def callback(self,data): #dict with key = button, value = input try: #raises gamepad map format error buttons = getButtons(data, self.gamepad) #convert button input to driving commands, etc commands = self.converter.getDriveCommands(buttons) js_ret = commands['journey_start'] newSpeed = commands['speed'] newAngle = commands['angle'] deadMansSwitch = commands['dms'] autoCtrl = commands['auto_mode'] if js_ret: js_msg = Bool() js_msg.data = True self.journeyStartPublisher.publish(js_msg) dms = Bool() dms.data = deadMansSwitch self.dmsPublisher.publish(dms) ac = Bool() ac.data = autoCtrl self.autoCtrlPublisher.publish(ac) #only publish if needed if deadMansSwitch and (not autoCtrl): ack = AckermannDrive() ack.steering_angle = newAngle ack.speed = newSpeed self.manualDrivePublisher.publish(ack) except GamepadMapFormatError: rospy.logfatal("%s, shutting down", GamepadMapFormatError.message) exit(0)
def stop(self): print "Stopping" 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.control_pub.publish(drive_msg_stamped)
def send_command(pub_controls, c): cmd = c.split(',') assert len(cmd) == 2 v, delta = [float(d) for d in cmd] dur = rospy.Duration(1.0) rate = rospy.Rate(10) start = rospy.Time.now() drive = AckermannDrive(steering_angle=delta, speed=v) while (rospy.Time.now() - start) < dur: pub_controls.publish(AckermannDriveStamped(drive=drive)) rate.sleep()
def pursuit(self): drive_msg_stamped = AckermannDriveStamped() drive_msg = AckermannDrive() r = rospy.Rate(self.rate) while not rospy.is_shutdown(): print 'robot_pose:', self.robot_pose, 'waypoints:', self.waypoints self.destination_pose = self.circleIntersect(self.lookahead_distance) print 'destination_pose:', self.destination_pose print 'waypoint_index:', self.current_waypoint_index if (self.destination_pose == None): self.speed = 0 self.steering_angle = 0 else: self.speed = self.default_speed distance_to_destination= self.getDistance(self.robot_pose, self.destination_pose) angle_to_destination = -self.getAngle(self.robot_pose, self.destination_pose) self.steering_angle = np.arctan((2 * self.robot_length * np.sin(angle_to_destination)) / distance_to_destination) drive_msg.speed = self.speed drive_msg.steering_angle = self.steering_angle drive_msg_stamped.drive = drive_msg self.pub.publish(drive_msg_stamped) r.sleep()
def run(self, hz): cmd_publisher = rospy.Publisher("/ackermann_cmd", AckermannDrive, queue_size=1) rate = rospy.Rate(hz) # 30 Hz screen = pygame.display.set_mode((1, 1)) # Mapping for PS4 Controller # Axis 0 - Left Stick X # Axis 1 - Left Stick Y # Axis 2 - Left "Trigger" # Axis 3 - Right Stick X # Axis 4 - Right Stick Y # Axis 5 - Right "Trigger" while not rospy.is_shutdown(): # This has to be called at the start of each loop in order for pygame # to update the axis status pygame.event.get() # Create a new "empty" message msg = AckermannDrive() print(msg.steering_angle) print(msg.speed) msg.steering_angle = -self.joy.get_axis(0) l_t = self.joy.get_axis(2) r_t = self.joy.get_axis(5) msg.speed = 2.0 * ((r_t + 1.0) - (l_t + 1.0)) print("steering: {} speed: {}".format(msg.steering_angle, msg.speed)) cmd_publisher.publish(msg) rate.sleep()
def update(self): """ Processes gps data and waypoint and publishes new steering angle. """ # Double check there is data from fix and from the Hemisphere. if nav_data.longitude is None or nav_data.direction is None: return # Update class variables. self.current_longitude = nav_data.longitude self.current_latitude = nav_data.latitude self.current_angle = nav_data.direciton # Figure out how to proceed if the tractorhas not reached. if (isclose(self.current_latitude, self.waypoint_latitude) and \ isclose(self.current_longitude, self.waypoint_longitude)): desired_angle = deg_calculate_desired_angle(self.current_latitude,\ self.current_longitude, self.waypoint_latitude, \ self.waypoint_longitude) current_error = deg_calculate_steering_error(self.current_angle, \ desired_angle) new_steering_angle = deg_calculate_steering_angle(current_error, \ self.kp1) new_velocity = deg_calculate_forward_velocity(new_steering_angle, \ self.kp3) # Create and publish a drive message. drive_msg = AckermannDrive() drive_msg.header.stamp = rospy.Time.now() drive_msg.steering_angle = new_steering_angle drive_msg.speed = abs(new_velocity) self.pub.publish(drive_msg) # Otherwise, we're ready to move on to the next waypoint. else: self.waypoint_reached = True return
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 publish_(self): self._controller() ai = self.throttle di = self.steering # print "throttle and steering angle :: ",di print "speed and steering angle :: ", ai, di print "\n\n" ackermann_cmd_msg = AckermannDrive() ackermann_cmd_msg.steering_angle = di ackermann_cmd_msg.speed = ai self.cmd_pub.publish(ackermann_cmd_msg) # throttle_msg = make_throttle_msg(ai) # steering_msg = make_steering_msg(di) # brake_msg = make_brake_msg() # self.throttle_pub.publish(throttle_msg) # self.steering_pub.publish(steering_msg) # self.brake_pub.publish(brake_msg) rospy.on_shutdown(self.stopOnShutdown)
def step(self, action): if rospy.is_shutdown(): raise (KeyboardInterrupt) self._resumeGazebo() ackermann_cmd = AckermannDrive() ackermann_cmd.speed = action[0] * (MAX_SPEED - MIN_SPEED) ackermann_cmd.steering_angle = action[1] * MAX_ANGLE self.ackermann_pub.publish(ackermann_cmd) rospy.sleep(self.step_size) data = None obs = None done = False while data is None and not rospy.is_shutdown(): try: # data = rospy.wait_for_message('/ackermann_vehicle/camera1/image_raw', Image, timeout=5) while self.image_msg is None: rospy.logwarn_throttle(5, 'No camera updates in step') data = self.image_msg obs = self._imageMsgToCv2(data) obs = cv2.cvtColor(obs, cv2.COLOR_BGR2RGB) obs = np.float32(obs) / 255 self.image_msg = None except: rospy.logerr_throttle(1, 'No data found') self._pauseGazebo() state = obs reward, done = self._rewardCalc() return state, reward, done, {}
def control(data): global prev_error global vel_input global kp global kd global angle error = data.pid_error angle_correction = kp * error + (kd * (error - prev_error)) angle = - angle_correction print angle if angle > 100: angle = 100 elif angle < -100: angle = -100 angle = angle / 100 vel_input = 1 - abs(angle) if vel_input < 0.5: vel_input = 0.5 msg = AckermannDrive(); msg.speed = vel_input msg.steering_angle = angle pub.publish(msg)
def __init__(self, joyListener): smach.State.__init__(self, outcomes=['playback', 'stop'], input_keys=['input'], output_keys=['output']) self.joyListener = joyListener # Create an event to block on until we transition to the next state self.stateChangeEvent = threading.Event() self.car_ctl = AckermannDrive() self.car_msg = AckermannDriveStamped() self.cmdPub = rospy.Publisher('ackermann_cmd', AckermannDriveStamped, queue_size=1)
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 callback(data): global speed global steering_angle global distance_set distance_set = [get_range(data, i) for i in range(90, -91, -1)] set_speedmode(4) wall_following(distance_apart=1.0) distant_narrow_front_perception() near_wide_front_perception() wheel() # steering_angle = 0 radian in range [-0.4189, 0.4189] drive_msg = AckermannDrive(steering_angle=steering_angle, speed=speed) drive_st_msg = AckermannDriveStamped(drive=drive_msg) drive_pub.publish(drive_st_msg)
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 callback(data): rospy.loginfo(str(data.steer) + " - " + str(data.throttle)) ackermsg = AckermannDriveStamped() ackermsg.header = Header() ackermsg.header.stamp = rospy.Time.now() ackermsg.drive = AckermannDrive() ackermsg.drive.steering_angle = data.steer ackermsg.drive.steering_angle_velocity = steering_angle_velocity ackermsg.drive.speed = data.throttle ackermsg.drive.acceleration = acceleration ackermsg.drive.jerk = jerk publisher.publish(ackermsg)
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 execute(self, currentPose, referencePose): # Compute the control input to the vehicle according to the # current and reference pose of the vehicle # Input: # currentPose: ModelState, the current state of the vehicle # referencePose: list, the reference state of the vehicle, # the element in the list are [ref_x, ref_y, ref_theta, ref_v] # Output: None # TODO: Implement this function #Pack computed velocity and steering angle into Ackermann command newAckermannCmd = AckermannDrive() # Publish the computed control input to vehicle model self.controlPub.publish(newAckermannCmd)
def main(): model = AckermannMotionModel() pathgen = AckermannPathGen(model) rospy.init_node('ackermann_test') freq = 16. rate = rospy.Rate(1 / freq) cmd = AckermannDrive(speed=1.0, steering_angle=0.3) pose_sub = rospy.Subscriber('/car/particle_filter/inferred_pose', PoseStamped, pathgen.handle_pose) path_pub = rospy.Publisher('/car/planner/path', PoseArray, queue_size=1) while not rospy.is_shutdown(): path_pub.publish(pathgen.get_path()) rate.sleep()
def __init__(self, data, pilotMode, vesc): drive_msg_stamped = AckermannDriveStamped() drive_msg = AckermannDrive() #self.drive_msg = AckermannDrive() if data[1]!='zed' and data[1]!='lidar': #self.both(data[0], data[1]) logicData = [self.zedData, self.lidarData] elif data[1]=='zed': self.zed(data[0][0], data[0][1], data[0][2], data[0][3]) logicData = [self.zedData, 'zed'] elif data[1]=='lidar': self.lidar(data[0][0], data[0][1], data[0][2], data[0][3], data[0][4], data[0][5]) logicData = [self.lidarData, 'lidar'] Logic(logicData, drive_msg, pilotMode) drive_msg_stamped.drive = drive_msg vesc.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 __init__(self): self.ackermann_cmd = AckermannDrive() self.ackermann_cmd.speed = 1 self.ackermann_cmd.steering_angle = 0.0 self.ackermann_pub = rospy.Publisher('/ackermann_cmd', AckermannDrive, queue_size=1) self.time_sub = rospy.Subscriber('/clock', Clock, self._clock_callback) self.bumper_sub = rospy.Subscriber('/bumper_sensor', ContactsState, self._bumper_callback) self.drive_cur_time = 0 self.drive_init_time = 0 self.rate = rospy.Rate(10)
def tape_present(self, msg): frame = self.bridge.imgmsg_to_cv2(msg, "mono8") white_pixels = cv2.countNonZero(frame) if white_pixels < MIN_PIXELS_TO_DRIVE: drive_msg_stamped = AckermannDriveStamped() drive_msg = AckermannDrive() drive_msg.speed = 0.0 drive_msg.steering_angle = 0.0 drive_msg.jerk = 0 drive_msg.steering_angle_velocity = 0 drive_msg_stamped.drive = drive_msg self.pub.publish(drive_msg_stamped)
def __init__(self): # node setup rospy.init_node("person_follower", anonymous=True) # state from subscribers self.tracker = PersonTracker() self.right_bumper = False self.last_bounding_box_timestamp = 0 self.last_speed = 0 self.last_angle = 0 # subscriber setup sub = message_filters.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes) depth_sub = rospy.Subscriber('/camera/depth/image_rect_raw', Image, self.tracker.depth_cb) joy_sub = rospy.Subscriber('/vesc/joy', Joy, self.right_bump) img_sub = message_filters.Subscriber('/camera/color/image_raw', Image) ts = message_filters.ApproximateTimeSynchronizer([sub, img_sub], 10, 0.1) sub.registerCallback(self.tracker.bounding_box_cb) ts.registerCallback(self.tracker.img_cb) # publisher setup pub = rospy.Publisher('/vesc/low_level/ackermann_cmd_mux/input/teleop', AckermannDriveStamped, queue_size=10) seq = 0 rate = rospy.Rate(30) while not rospy.is_shutdown(): seq += 1 header = Header(seq=seq, stamp=rospy.Time.now()) if self.tracker.predicted_bounding_box is not None: self.tracker.tracker_pub.publish( self.tracker.predicted_bounding_box) if self.tracker.average_tracked is not None: self.tracker.average_img_pub.publish( br.cv2_to_imgmsg(self.tracker.average_tracked)) angle, speed = self.get_steering_direction() if self.right_bumper and angle and speed: pub.publish( AckermannDriveStamped( header, AckermannDrive(steering_angle=angle, speed=speed))) rate.sleep() rospy.spin()
def __init__(self): IMAGE_WIDTH = 640 IMAGE_HEIGHT = 480 SERVO_RANGE = 0.34 MAX_PERCENT_OF_FRAME = 0.27 MIN_PERCENT_OF_FRAME = 0.18 self.last_box = None self.person_bounding_box = None self.right_bumper = False # node setup rospy.init_node("person_follower", anonymous=True) sub = rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes, lambda x: self.cb(x)) joy_sub = rospy.Subscriber('/vesc/joy', Joy, self.right_bump) pub = rospy.Publisher('/vesc/low_level/ackermann_cmd_mux/input/teleop', AckermannDriveStamped, queue_size=10) rate = rospy.Rate(30) seq = 0 while not rospy.is_shutdown(): seq += 1 header = Header(seq=seq, stamp=rospy.Time.now()) bb = self.person_bounding_box if bb is not None: boxCenter = (bb.xmin + bb.xmax) / 2.0 ratio = boxCenter / 640.0 angle = -1 * (-SERVO_RANGE + ratio * 2 * SERVO_RANGE) percent_of_frame = (abs(bb.xmax - bb.xmin) * abs(bb.ymax - bb.ymin)) / (640.0 * 480.0) speed = 0 if percent_of_frame > MAX_PERCENT_OF_FRAME: speed = -0.5 angle = -angle elif percent_of_frame < MIN_PERCENT_OF_FRAME: speed = 0.5 #print("angle is", angle, "speed is", speed, "percent of frame is", percent_of_frame) if self.right_bumper: pub.publish( AckermannDriveStamped( header, AckermannDrive(steering_angle=angle, speed=speed))) rate.sleep() rospy.spin()
def publish_stop_message(self): header = Header( stamp=Time.now(), frame_id="base_link", ) drive = AckermannDrive( steering_angle=self.last_steering_angle, steering_angle_velocity=0, speed=0, # STOP! acceleration=0, jerk=0, ) msg = AckermannDriveStamped( header=header, drive=drive, ) rospy.logwarn("STOP: safety controller ordered a stop!") self.safety_pub.publish(msg)
def __init__(self): super(RosThreaddedNode, self).__init__() # Initialize the ros node self.rosNode = rospy.init_node("autocar_keyop") # Publisher self.ackermannPub = rospy.Publisher("ackermann_cmd", AckermannDrive, queue_size=10) self.currentAckermannMsg = AckermannDrive() # self.jointStateSub = rospy.Subscriber("joint_states", JointState, self.onJointStateReceived) # Joint states self.currentSpeed = 0.0 self.currentSteerAngle = 0.0 # The scale value that decided how much the velocity will be increased on each keypress self.speedScale = 0.1 self.angleScale = 0.01 # Collect parameters for parameter server self.getParameters()
def __init__(self): self.path_points = [] self._i = 0 self.final_goal = 0 self._sub = rospy.Subscriber("/ada/fix", NavSatFix, self.call_gps) self._path = rospy.wait_for_message("route_points", Path) self.pub = rospy.Publisher('ackermann_cmd', AckermannDrive, queue_size=10, latch=True) self.ackermann_msg = AckermannDrive() self.state_pub = rospy.Publisher('state', Float64, queue_size=10, latch=True) self.control_effort = rospy.Subscriber('control_effort', Float64, self.control_callback) self.goal_sub = rospy.Subscriber("ada/goal_status", GoalStatusArray, self.call_goal) self.lateral_offset = 0.0 self.prev_lateral_offset = 0.0 self.yaw_error = 0.0 self.Ld = 0.0 self.Rd = 0.0 self.dt = 0.01 self.error = 0.0 self.yaw_angle_vehicle = 0.0 self.distance = 0.0 self.i = 0 self.current_pose = [] self.prev_pose = [] self.road_slope = None self.direction = 0.0 self.prev_error = 0.0 self.path_points = [] self.Velocity_x = 10