def drive_straight(self): while not rospy.is_shutdown(): drive_msg_stamped = AckermannDriveStamped() drive_msg = AckermannDrive() drive_msg.speed = 2.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) if (SPEED == 0): 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(5000) # don't spin too fast rospy.sleep(1.0 / PUBLISH_RATE)
def __on_camera_image(self, message): img = message.data img = np.frombuffer(img, dtype=np.uint8).reshape((message.height, message.width, 4)) img = img[380:420, :] # Segment the image by color in HSV color space img = cv2.cvtColor(img, cv2.COLOR_RGB2HSV) mask = cv2.inRange(img, np.array([50, 110, 150]), np.array([120, 255, 255])) # Find the largest segmented contour contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) command_message = AckermannDrive() command_message.speed = 50.0 command_message.steering_angle = 0.0 if contours: largest_contour = max(contours, key=cv2.contourArea) largest_contour_center = cv2.moments(largest_contour) if largest_contour_center['m00'] != 0: center_x = int(largest_contour_center['m10'] / largest_contour_center['m00']) # Find error (the lane distance from the target distance) error = center_x - 190 command_message.steering_angle = error*CONTROL_COEFFICIENT self.__ackermann_publisher.publish(command_message)
def twist_ackermann_node(data): command = AckermannDrive() if data.linear.x == 0.0 or data.angular.z == 0.0: command.steering_angle = 0.0 else: radius = data.linear.x / data.angular.z command.steering_angle = math.atan(wheelbase / radius) command.speed = data.linear.x command_pub.publish(command)
def call_back2(self, data): ack_cmd = AckermannDriveStamped() ack_cmd.header.stamp = rospy.Time.now() drive = AckermannDrive() drive.speed = data.linear.x drive.steering_angle = data.angular.z # impose limits on commanded angle # datetime object containing current date and time now = datetime.now() print(self.i) drive.steering_angle = self.i ack_cmd.drive = drive self.publisher.publish(ack_cmd)
def processError(self, error, dist): if dist == 0: steering_angle_cmd = 0 speed_cmd = 0 else: error = error / 1000.0 steering_angle_cmd = self.pid.update(error) if dist < SLOWDOWN_DISTANCE: speed_cmd = self.speed_slow else: speed_cmd = self.speed if not self.sim: speed_cmd += abs( self.latest_trailer_angle ) * SPEED_INCREASE_COEFFICIENT / MAX_TRAILER_ANGLE ack = AckermannDrive() ack.steering_angle = steering_angle_cmd ack.speed = speed_cmd if self.lock_stop: ack.speed = 0 self.drive_publisher.publish(ack)
def odom_callback(msg): ack_msg = AckermannDrive() ack_msg_stmp = AckermannDriveStamped() #Speed: Set constant half max speed ack_msg.speed = max_speed / 2.0 #Steering: Random btwn 0 and 1 rand = random() steering_range = max_steering_angle / 2.0 rand_angle = steering_range * rand - steering_range / 2.0 # sometimes change sign so it turns more (basically add bias to continue turning in current direction) rand = random() if rand > 0.8 and prev_angle != 0.0: sign_rand = rand_angle / abs(rand_angle) sign_prev = prev_angle / abs(prev_angle) rand_angle *= sign_rand * sign_prev #set angle ack_msg.steering_angle = min(max(prev_angle + rand_ang, -max_steering_angle), max_steering_angle)) #reset prev_angle prev_angle = ack_msg.steering_angle ack_msg_stmp = ack_msg drive_pub.publish(ack_msg_stmp)
def joy_command_callback(data): global current_command_topic global offboard_command passthrough_command = AckermannDrive() # listen to control transfer commands if data.buttons[ctl_offboard_button] and not data.buttons[ctl_teleop_button]: if not message_display[0]: rospy.loginfo(log_message.format(control_priority[0])) message_display[0] = True message_display[1] = False if current_command_topic != command_topic[0]: current_command_topic = command_topic[0] if not data.buttons[ctl_offboard_button] and data.buttons[ctl_teleop_button]: if not message_display[1]: rospy.loginfo(log_message.format(control_priority[1])) message_display[0] = False message_display[1] = True if current_command_topic != command_topic[1]: current_command_topic = command_topic[1] if current_command_topic == command_topic[1]: # identify and scale raw command data passthrough_command.steering_angle = data.axes[joy_angle_axis] * joy_angle_scaler passthrough_command.speed = data.axes[joy_speed_axis] * joy_speed_scaler elif current_command_topic == command_topic[0] and listen_offboard == 'true': passthrough_command = offboard_command multiplexer_pub.publish(passthrough_command)
def rotate_leg(self, speed, radius, arc, update_rate): # Initialize the movement command move_cmd = AckermannDrive() # Set the forward speed # Setting no acceleration or jerk will accelerate at maximum rate move_cmd.speed = speed # Set the steering angle - Need to derive from CLMR Kinematics # TODO: Check to see that it is within the geometric limits of the robot angle = math.atan(self.wheelbase / radius) move_cmd.steering_angle = angle # Move forward for a time to go the desired distance distance = arc * radius duration = distance / speed ticks = int(duration * update_rate) for t in range(ticks): self.cmd_ack.publish(move_cmd) self.rate.sleep() # Stop the robot before the next leg self.cmd_ack.publish(AckermannDrive()) rospy.sleep(1)
def cmd_callback(data): global wheelbase global ackermann_cmd_topic global frame_id global pub global message_type if message_type == 'ackermann_drive': v = data.linear.x steering = convert_trans_rot_vel_to_steering_angle( v, data.angular.z, wheelbase) msg = AckermannDrive() msg.steering_angle = steering msg.speed = v pub.publish(msg) else: v = data.linear.x steering = convert_trans_rot_vel_to_steering_angle( v, data.angular.z, wheelbase) msg = AckermannDriveStamped() msg.header.stamp = rospy.Time.now() msg.header.frame_id = frame_id msg.drive.steering_angle = steering msg.drive.speed = v pub.publish(msg)
def call_back2(self,data): ack_cmd = AckermannDriveStamped() ack_cmd.header.stamp = rospy.Time.now() drive = AckermannDrive() drive.speed = data.linear.x drive.steering_angle = data.angular.z # impose limits on commanded angle # datetime object containing current date and time now = datetime.now() self.i = now.strftime("%H:%M:%S") my_name="youssf" # dd/mm/YY H:M:S image =self.overlay()[210:,:] if(self.temp != str(self.i)): with open("data_index.txt", "a") as myfile: cv2.imwrite("data/"+my_name+self.i+'.jpg',image) myfile.write("data/"+my_name+self.i+'.jpg,'+str(drive.steering_angle)+"\n") print("now =", str(self.number) + " with id = "+str(self.i)+" shape"+str(image.shape)) self.temp=str(self.i) self.number+=1 ack_cmd.drive = drive self.publisher.publish(ack_cmd)
def keyboard_command_callback(data): global current_command_topic global offboard_command passthrough_command = AckermannDrive() # listen to control transfer commands if rospy.get_param( '/{}/command_priority'.format(car_name)) == control_priority[0]: if not message_display[0]: rospy.loginfo(log_message.format(control_priority[0])) message_display[0] = True message_display[1] = False if current_command_topic != command_topic[0]: current_command_topic = command_topic[0] if rospy.get_param( '/{}/command_priority'.format(car_name)) == control_priority[1]: if not message_display[1]: rospy.loginfo(log_message.format(control_priority[1])) message_display[0] = False message_display[1] = True if current_command_topic != command_topic[1]: current_command_topic = command_topic[1] if current_command_topic == command_topic[1]: # identify and scale raw command data passthrough_command = data elif current_command_topic == command_topic[ 0] and listen_offboard == 'true': passthrough_command = offboard_command if rospy.get_param('/{}/suspend_control'.format(car_name)) in [ 'True', 'true', '1', '1.0' ]: passthrough_command.steering_angle = 0.0 passthrough_command.speed = 0.0 multiplexer_pub.publish(passthrough_command) else: multiplexer_pub.publish(passthrough_command)
def cmd_callback(data): global wheelbase global ackermann_cmd_topic global frame_id global pub global message_type # Hard simulation limit if data.angular.z > 1.30: data.angular.z = 1.30 elif data.angular.z < -1.30: data.angular.z = -1.30 if message_type == 'ackermann_drive': v = data.linear.x steering = data.angular.z msg = AckermannDrive() msg.steering_angle = steering msg.speed = v pub.publish(msg) else: v = data.linear.x steering = data.angular.z msg = AckermannDriveStamped() msg.header.stamp = rospy.Time.now() msg.header.frame_id = frame_id msg.drive.steering_angle = steering msg.drive.speed = v pub.publish(msg)
def control(data): global prev_error global vel_input global kp global kd global angle = 0.0 print("PID Control Node is Listening to error") ## Your PID code goes here #TODO: Use kp, ki & kd to implement a PID controller # 1. Scale the error # 2. Apply the PID equation on error to compute steering # An empty AckermannDrive message is created. You will populate the steering_angle and the speed fields. command = AckermannDrive() # TODO: Make sure the steering value is within bounds [-100,100] command.steering_angle = angle # TODO: Make sure the velocity is within bounds [0,100] command.speed = vel_input # Move the car autonomously command_pub.publish(command)
def __init__(self): rospy.init_node("self_driving_system_bridge_node") self.steering_cmd = None self.speed_cmd = None rospy.Subscriber('/vehicle/dbw/steering_cmds/', Float32, callback=self.steering_cmd_callback) rospy.Subscriber('/vehicle/dbw/cruise_cmds', Float32, callback=self.speed_cmd_callback) ackermann_pub = rospy.Publisher('/carla/ego_vehicle/ackermann_cmd', AckermannDrive) rate = rospy.Rate(30) rospy.loginfo("self-driving system bridge started") while not rospy.is_shutdown(): if self.steering_cmd is not None and self.speed_cmd is not None: ackermann_msg = AckermannDrive() ackermann_msg.steering_angle = -1.0 * float(self.steering_cmd) ackermann_msg.speed = 5 + float(self.speed_cmd) ackermann_pub.publish(ackermann_msg) rate.sleep()
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 finalize(self): rospy.loginfo('Halting motors, aligning wheels and exiting...') ackermann_cmd_msg = AckermannDrive() ackermann_cmd_msg.speed = 0 ackermann_cmd_msg.steering_angle = 0 self.drive_pub.publish(ackermann_cmd_msg) sys.exit()
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 finalize(self): rospy.loginfo('Halting motors, aligning wheels and exiting...') self.settings = termios.tcgetattr(sys.stdin) ackermann_cmd_msg = AckermannDrive() ackermann_cmd_msg.speed = 0 ackermann_cmd_msg.steering_angle = 0 self.motors_pub.publish(ackermann_cmd_msg) sys.exit()
def pub_callback(self, event): ackermann_cmd_msg = AckermannDrive() ackermann_cmd_msg.speed = self.speed ackermann_cmd_msg.steering_angle = self.steering_angle # 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 stop_vehicle(self): msg = AckermannDrive() msg.speed = 0 msg.acceleration = 0 msg.jerk = 0 msg.steering_angle = 0 msg.steering_angle_velocity = 0 self.pub_ackermann_cmd.publish(msg) rospy.signal_shutdown('Made it to goal')
def set_vehicle_command(self, velocity, steering_angle): ''' Publishes the calculated steering angle ''' drive = AckermannDrive() drive.speed = velocity drive.steering_angle = steering_angle drive.steering_angle_velocity = 0.0 self.tracker_pub.publish(drive)
def drive(self, speed, angle): drive_msg_stamped = AckermannDriveStamped() drive_msg = AckermannDrive() drive_msg.speed = speed drive_msg.steering_angle = angle 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)
def control_vehicle(speed, steering_angle): publisher = rospy.Publisher('/carla/ego_vehicle/ackermann_cmd', AckermannDrive, queue_size=5) rospy.init_node('vehicle_controller', anonymous=True) rate = rospy.Rate(10) while not rospy.is_shutdown(): ackermann_cmd = AckermannDrive() ackermann_cmd.steering_angle = steering_angle ackermann_cmd.speed = speed publisher.publish(ackermann_cmd) rate.sleep()
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 callback(self, data): #rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.buttons[self.enable_button]) if data.buttons[self.enable_button] is 1: ackermann_msg = AckermannDrive() if abs(data.axes[self.axis_linear]) > self.axis_dead_zone: ackermann_msg.speed = data.axes[ self.axis_linear] * self.scale_linear #left up-down stick else: ackermann_msg.speed = 0.0 if abs(data.axes[self.axis_front_steering]) > self.axis_dead_zone: ackermann_msg.steering_angle = data.axes[ self. axis_front_steering] * self.scale_front_steering #right left-right stick else: ackermann_msg.steering_angle = 0.0 self.pub.publish(ackermann_msg)
def callback(data): #rospy.loginfo(rospy.get_caller_id() + 'I heard %s', data.axes[0]) ack_msg = AckermannDrive() #ack_msg.steering_angle=90+data.axes[0]*50 ack_msg.steering_angle=maprange( (-1, 1), (-100, 100), data.axes[0]) ack_publisher.publish(ack_msg) steer_msg = JointState() steer_msg.position = maprange( (-1, 1), (-100, 100), data.axes[0]) steer_publisher.publish(steer_msg)
def callback(data): rospy.loginfo(rospy.get_caller_id() + 'I heard %s', data.axes[0]) ack_cmd = AckermannDriveStamped() ack_cmd.header.stamp = rospy.Time.now() drive = AckermannDrive() drive.speed = (data.axes[5]) drive.steering_angle = (data.axes[0]) ack_cmd.drive = drive ack_publisher.publish(ack_cmd)
def stop(self): 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.drive_pub.publish(drive_msg_stamped)
def joy_callback(self, joy): global temp data = AckermannDrive() # x button pressed behaves as a deadman, switch R2 is forward L2 is reverse # if x is engadged and L2 and not active if joy.buttons[1] and not joy.buttons[6]: if not joy.buttons[7]: temp = 1 elif joy.buttons[7] and temp: # if rising edge on R2 drive forward # joy.axes[4] moves from 1 to -1 data.speed = self.MAX_FORWARD_VELOCITY*0.5*(-1*(joy.axes[4])+1) # also send steering angle # joy.axes[0] is left +1 to right -1 # right turn should correspond to a negative steering angle # left turn should correspond to positive steering angle data.steering_angle = joy.axes[0]*self.MAX_STEERING_ANGLE self.ackermann_publisher.publish(data) # if x is engadged and R2 and not active elif joy.buttons[1] and not joy.buttons[7]: if not joy.buttons[6]: temp = 1 elif joy.buttons[6] and temp: # if rising edge on L2 drive reverse # joy.axes[3] from 1 to -1 data.speed = self.MAX_REVERSE_VELOCITY*0.5*(joy.axes[3]-1) # also send steering angle # joy.axes[0] is left +1 to right -1 # right turn should correspond to a negative steering angle # left turn should correspond to a positivev steering angle data.steering_angle = joy.axes[0]*self.MAX_STEERING_ANGLE self.ackermann_publisher.publish(data) # deadman switch activated (i think i was trying to make a state-machine) elif (not joy.buttons[7] and temp) or not joy.buttons[1] or joy.buttons[6]: temp = 0
def callback(self, data): ack_cmd = AckermannDriveStamped() ack_cmd.header.stamp = rospy.Time.now() drive = AckermannDrive() drive.speed = -1 * data.axes[5] drive.steering_angle = data.axes[0] # impose limits on commanded angle if drive.steering_angle > self.max_steering: drive.steering_angle = self.max_steering if drive.steering_angle < self.min_steering: drive.steering_angle = self.min_steering # clean up angle if it is very close to zero if math.fabs(drive.steering_angle) < self.epsilon_steering: drive.steering_angle = 0.0 ack_cmd.drive = drive self.publisher.publish(ack_cmd)
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 = 1.5 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 i = 0 while True: pub.publish(drive_msg_stamped) rate.sleep() drive_msg.steering_angle = math.sin(i) i = i + 0.1
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 stop(self): print "Stopping" drive_msg_stamped = AckermannDriveStamped() drive_msg_stamped.header = utils.make_header("/base_link") 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 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)