コード例 #1
0
class EarthRoverChassis:
    def __init__(self):
        rospy.init_node("earth_rover_chassis",
                        # log_level=rospy.DEBUG
                        )

        # rospy.on_shutdown(self.shutdown)

        # robot dimensions
        self.wheel_radius = rospy.get_param("~wheel_radius_meters", 1.0)
        self.wheel_distance = rospy.get_param("~wheel_distance_meters", 1.0)
        self.ticks_per_rotation = rospy.get_param("~ticks_per_rotation", 1.0)

        # speed parameters
        self.left_min_speed_meters_per_s = rospy.get_param(
            "~left_min_speed_meters_per_s", -1.0)
        self.left_max_speed_meters_per_s = rospy.get_param(
            "~left_max_speed_meters_per_s", 1.0)
        self.right_min_speed_meters_per_s = rospy.get_param(
            "~right_min_speed_meters_per_s", -1.0)
        self.right_max_speed_meters_per_s = rospy.get_param(
            "~right_max_speed_meters_per_s", 1.0)

        self.left_min_usable_command = rospy.get_param(
            "~left_min_usable_command", 0.10)
        self.right_min_usable_command = rospy.get_param(
            "~right_min_usable_command", 0.10)

        self.min_measurable_speed = rospy.get_param("~min_measurable_speed",
                                                    0.0001)

        # cmd_vel parameters
        # self.min_cmd_lin_vel = rospy.get_param("~min_cmd_lin_vel", 0.0)
        # self.min_cmd_ang_vel = rospy.get_param("~min_cmd_lin_vel", 0.0)

        # PID parameters
        self.enable_pid = rospy.get_param("~enable_pid", True)
        self.min_command = rospy.get_param("~min_command", -1.0)
        self.max_command = rospy.get_param("~max_command", 1.0)
        self.kp = rospy.get_param("~kp", 1.0)
        self.ki = rospy.get_param("~ki", 0.0)
        self.kd = rospy.get_param("~kd", 0.0)
        self.speed_smooth_k = rospy.get_param("~speed_smooth_k", 1.0)
        # self.output_deadzone = rospy.get_param("~output_deadzone", 0.1)
        self.output_noise = rospy.get_param("~output_noise", 0.01)

        # TF parameters
        self.child_frame = rospy.get_param("~odom_child_frame", "base_link")
        self.parent_frame = rospy.get_param("~odom_parent_frame", "odom")
        self.tf_broadcaster = tf.TransformBroadcaster()

        # Ultrasonic parameters
        self.stopping_distances_cm = rospy.get_param("~stopping_distances_cm",
                                                     None)
        self.easing_offset_cm = rospy.get_param("~easing_offset_cm", 5.0)
        self.easing_offset_cm = rospy.get_param("~min_tracking_lin_vel", 0.07)
        self.easing_offset_cm = rospy.get_param("~min_tracking_ang_vel", 5.0)
        self.front_sensors = rospy.get_param("~front_sensors", None)
        self.right_sensors = rospy.get_param("~right_sensors", None)
        self.left_sensors = rospy.get_param("~left_sensors", None)
        self.back_sensors = rospy.get_param("~back_sensors", None)

        assert self.stopping_distances_cm is not None
        assert self.front_sensors is not None
        assert self.right_sensors is not None
        assert self.left_sensors is not None
        assert self.back_sensors is not None

        # Motor controller objects
        left_motor_info = MotorInfo(
            "left motor", self.kp, self.ki, self.kd, self.speed_smooth_k,
            self.wheel_radius, self.ticks_per_rotation,
            self.left_min_usable_command, self.left_min_speed_meters_per_s,
            self.left_max_speed_meters_per_s, self.min_command,
            self.max_command, self.output_noise, self.min_measurable_speed)

        right_motor_info = MotorInfo(
            "right motor", self.kp, self.ki, self.kd, self.speed_smooth_k,
            self.wheel_radius, self.ticks_per_rotation,
            self.right_min_usable_command, self.right_min_speed_meters_per_s,
            self.right_max_speed_meters_per_s, self.min_command,
            self.max_command, self.output_noise, self.min_measurable_speed)

        self.left_motor = MotorController(left_motor_info)
        self.right_motor = MotorController(right_motor_info)

        self.linear_speed_mps = 0.0
        self.rotational_speed_mps = 0.0

        # Ultrasonic state tracking objects
        self.num_sensors = len(self.stopping_distances_cm)

        self.sensor_directions = [[Direction.FRONT, self.front_sensors],
                                  [Direction.BACK, self.back_sensors],
                                  [Direction.LEFT, self.left_sensors],
                                  [Direction.RIGHT, self.right_sensors]]

        self.trackers_indexed = [None for _ in range(self.num_sensors)]
        self.trackers_directed = {
            Direction.FRONT: TrackerCollection(),
            Direction.BACK: TrackerCollection(),
            Direction.LEFT: TrackerCollection(),
            Direction.RIGHT: TrackerCollection(),
        }

        for direction, sensor_indices in self.sensor_directions:
            for sensor_index in sensor_indices:
                stop_dist = self.stopping_distances_cm[sensor_index]
                ease_dist = stop_dist + self.easing_offset_cm
                tracker = UltrasonicTracker(stop_dist, ease_dist)
                self.trackers_indexed[sensor_index] = tracker
                self.trackers_directed[direction].append(tracker)

        # prev state tracking
        self.prev_left_output = 0.0
        self.prev_right_output = 0.0

        # odometry state
        self.odom_x = 0.0
        self.odom_y = 0.0
        self.odom_t = 0.0

        self.odom_vx = 0.0
        self.odom_vy = 0.0
        self.odom_vt = 0.0

        # Odometry message
        self.odom_msg = Odometry()
        self.odom_msg.header.frame_id = self.parent_frame
        self.odom_msg.child_frame_id = self.child_frame

        # Subscribers
        self.twist_sub = rospy.Subscriber("cmd_vel",
                                          Twist,
                                          self.twist_callback,
                                          queue_size=5)
        self.left_encoder_sub = rospy.Subscriber("left/left_encoder/ticks",
                                                 Int64,
                                                 self.left_encoder_callback,
                                                 queue_size=50)
        self.right_encoder_sub = rospy.Subscriber("right/right_encoder/ticks",
                                                  Int64,
                                                  self.right_encoder_callback,
                                                  queue_size=50)
        self.ultrasonic_sub = rospy.Subscriber(
            "earth_rover_teensy_bridge/ultrasonic",
            Float32MultiArray,
            self.ultrasonic_callback,
            queue_size=15)

        # Publishers
        self.left_dist_pub = rospy.Publisher("left/left_encoder/distance",
                                             Float64,
                                             queue_size=5)
        self.right_dist_pub = rospy.Publisher("right/right_encoder/distance",
                                              Float64,
                                              queue_size=5)
        self.left_speed_pub = rospy.Publisher("left/left_encoder/speed",
                                              Float64,
                                              queue_size=5)
        self.right_speed_pub = rospy.Publisher("right/right_encoder/speed",
                                               Float64,
                                               queue_size=5)
        self.left_command_pub = rospy.Publisher("left/command_speed",
                                                Float64,
                                                queue_size=5)
        self.right_command_pub = rospy.Publisher("right/command_speed",
                                                 Float64,
                                                 queue_size=5)
        self.odom_pub = rospy.Publisher("odom", Odometry, queue_size=5)

        # Services
        self.tuning_service = rospy.Service("tune_motor_pid", TuneMotorPID,
                                            self.tune_motor_pid)

    def tune_motor_pid(self, request):
        kp = request.kp
        ki = request.ki
        kd = request.kd
        rospy.loginfo("Updated PID constants to kp=%s, ki=%s, kd=%s" %
                      (kp, ki, kd))

        self.left_motor.tune_pid(kp, ki, kd)
        self.right_motor.tune_pid(kp, ki, kd)

        return TuneMotorPIDResponse()

    def twist_callback(self, twist_msg):
        self.linear_speed_mps = twist_msg.linear.x  # m/s
        angular_speed_radps = twist_msg.angular.z  # rad/s

        # if abs(self.linear_speed_mps) < self.min_cmd_lin_vel and self.linear_speed_mps != 0.0:
        #     # assign linear_speed_mps the minimum speed with direction sign applied
        #     self.linear_speed_mps = math.copysign(self.min_cmd_lin_vel, self.linear_speed_mps)

        # if abs(angular_speed_radps) < self.min_cmd_ang_vel and angular_speed_radps != 0.0:
        #     angular_speed_radps = math.copysign(self.min_cmd_ang_vel, angular_speed_radps)

        # arc = angle * radius
        # rotation speed at the wheels
        self.rotational_speed_mps = angular_speed_radps * self.wheel_distance / 2

    def left_encoder_callback(self, enc_msg):
        self.left_motor.enc_tick = -enc_msg.data

        dt = self.left_motor.get_dt(rospy.Time.now())
        left_output = self.left_motor.update(dt)
        if left_output is not None:
            self.command_left_motor(left_output)

    def right_encoder_callback(self, enc_msg):
        self.right_motor.enc_tick = enc_msg.data

        dt = self.right_motor.get_dt(rospy.Time.now())
        right_output = self.right_motor.update(dt)
        if right_output is not None:
            self.command_right_motor(right_output)

    def ultrasonic_callback(self, ultrasonic_msg):
        for index, distance in enumerate(ultrasonic_msg.data):
            self.trackers_indexed[index].update(distance)

    def scale_targets(self, lin_vel, ang_vel):
        if lin_vel > 0:
            lin_vel = self.trackers_directed[Direction.FRONT].scale_v(lin_vel)
        elif lin_vel < 0:
            lin_vel = self.trackers_directed[Direction.BACK].scale_v(lin_vel)
        elif ang_vel < 0:  # if moving right, check the left side
            ang_vel = self.trackers_directed[Direction.LEFT].scale_v(ang_vel)
        elif ang_vel > 0:  # if moving left, check the right side
            ang_vel = self.trackers_directed[Direction.RIGHT].scale_v(ang_vel)

        return lin_vel, ang_vel

    def run(self):
        clock_rate = rospy.Rate(30)

        prev_ultrasonic_report_t = rospy.Time.now()
        while not rospy.is_shutdown():
            self.compute_odometry(self.left_motor.get_delta_dist(),
                                  self.right_motor.get_delta_dist(),
                                  self.left_motor.get_speed(),
                                  self.right_motor.get_speed())

            self.publish_chassis_data()

            linear_speed_mps, rotational_speed_mps = self.scale_targets(
                self.linear_speed_mps, self.rotational_speed_mps)
            if linear_speed_mps != self.linear_speed_mps or rotational_speed_mps != self.rotational_speed_mps:
                current_time = rospy.Time.now()
                if current_time - prev_ultrasonic_report_t > rospy.Duration(
                        0.5):
                    prev_ultrasonic_report_t = current_time

                    print "Scaling speed based on distance sensors"
                    for direction in self.trackers_directed:
                        print "\t%s, dist: %s, scale: %s" % (
                            Direction.name(direction),
                            self.trackers_directed[direction].get_dists(),
                            self.trackers_directed[direction].get_scale())

            self.left_motor.set_target(linear_speed_mps - rotational_speed_mps)
            self.right_motor.set_target(linear_speed_mps +
                                        rotational_speed_mps)

            clock_rate.sleep()

    def command_left_motor(self, command):
        if self.enable_pid and self.prev_left_output != command:
            self.left_command_pub.publish(command)
            self.prev_left_output = command

    def command_right_motor(self, command):
        if self.enable_pid and self.prev_right_output != command:
            self.right_command_pub.publish(command)
            self.prev_right_output = command

    def publish_chassis_data(self):
        self.left_dist_pub.publish(self.left_motor.get_dist())
        self.right_dist_pub.publish(self.right_motor.get_dist())
        self.left_speed_pub.publish(self.left_motor.get_speed())
        self.right_speed_pub.publish(self.right_motor.get_speed())

        odom_quaternion = tf.transformations.quaternion_from_euler(
            0.0, 0.0, self.odom_t)
        self.tf_broadcaster.sendTransform((self.odom_x, self.odom_y, 0.0),
                                          odom_quaternion, rospy.Time.now(),
                                          self.child_frame, self.parent_frame)

        self.odom_msg.header.stamp = rospy.Time.now()
        self.odom_msg.pose.pose.position.x = self.odom_x
        self.odom_msg.pose.pose.position.y = self.odom_y
        self.odom_msg.pose.pose.position.z = 0.0

        self.odom_msg.pose.pose.orientation.x = odom_quaternion[0]
        self.odom_msg.pose.pose.orientation.y = odom_quaternion[1]
        self.odom_msg.pose.pose.orientation.z = odom_quaternion[2]
        self.odom_msg.pose.pose.orientation.w = odom_quaternion[3]

        self.odom_msg.twist.twist.linear.x = self.odom_vx
        self.odom_msg.twist.twist.linear.y = self.odom_vy
        self.odom_msg.twist.twist.linear.z = 0.0

        self.odom_msg.twist.twist.angular.x = 0.0
        self.odom_msg.twist.twist.angular.y = 0.0
        self.odom_msg.twist.twist.angular.z = self.odom_vt

        self.odom_pub.publish(self.odom_msg)

    def compute_odometry(self, delta_left, delta_right, left_speed,
                         right_speed):
        delta_dist = (delta_right + delta_left) / 2

        # angle = arc / radius
        delta_angle = (delta_right - delta_left) / self.wheel_distance
        self.odom_t += delta_angle

        dx = delta_dist * math.cos(self.odom_t)
        dy = delta_dist * math.sin(self.odom_t)

        self.odom_x += dx
        self.odom_y += dy

        speed = (right_speed + left_speed) / 2
        self.odom_vx = speed * math.cos(self.odom_t)
        self.odom_vy = speed * math.sin(self.odom_t)
        self.odom_vt = (right_speed - left_speed) / (self.wheel_distance / 2)