base_controller.py

#! /usr/bin/env python

import math
import sys

import rospy
import serial
import tf
from geometry_msgs.msg import Twist, Pose2D
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Imu
from std_srvs.srv import Empty, EmptyResponse


class BaseControl(object):
    def __init__(self):
        self.port = rospy.get_param("~port", "/dev/ttyUSB0")
        self.baudrate = long(rospy.get_param("~baud", "115200"))

        self.wheelSep = float(rospy.get_param("~wheel_separation", "0.46"))
        self.wheelRad = float(rospy.get_param("~wheel_radius", "0.08255"))
        self.wheelDeadZone = float(rospy.get_param("~wheel_deadzone_rad", "0.0"))

        self.baseId = rospy.get_param("~base_id", "base_footprint")
        self.odomId = rospy.get_param("~odom_id", "odom")
        self.odomTopic = rospy.get_param("~odom_topic", "odom")
        self.odomFreq = float(rospy.get_param("~odom_freq", "20"))

        self.isPubOdomFrame = bool(
            rospy.get_param("~publish_odom_frame", "false"))

        self.cmdTopic = rospy.get_param("~cmd_topic", "cmd_vel")
        self.commandFreq = float(rospy.get_param("~command_freq", "20"))

        self.linear_max = float(rospy.get_param("~linear_max", "0.6"))
        self.linear_min = float(rospy.get_param("~linear_min", "-0.6"))
        self.angular_max = float(rospy.get_param("~angular_max", "0.6"))
        self.angular_min = float(rospy.get_param("~angular_min", "-0.6"))

        self.serial = None
        try:
            self.serial = serial.Serial(self.port, self.baudrate, timeout=10)
            rospy.loginfo("Connect to port: " + self.port + " success!!")
        except Exception as e:
            rospy.logerr("Cannot connect to port: " + self.port)
            rospy.logerr("Error: " + str(e))

        self.sub_cmd = rospy.Subscriber(
            self.cmdTopic, Twist, self.cmdvelCB, queue_size=1)
        self.pub_odom = rospy.Publisher(self.odomTopic, Odometry, queue_size=1)

        self.timer_command = rospy.Timer(rospy.Duration(
            1.0/self.commandFreq), self.timerCommandCB)
        self.timer_odom = rospy.Timer(rospy.Duration(
            1.0/self.odomFreq), self.timerOdomCB)

        # self.service_reset = rospy.Service("/reset", Empty, self.serviceReset)

        self.transform = tf.TransformBroadcaster()

        self.trans_x = 0.0
        self.rotat_z = 0.0

        self.goal_trans_x = 0.0
        self.goal_rotat_z = 0.0

        self.pose = Pose2D()
        self.previous_time = rospy.Time.now()

    @staticmethod
    def constrain(value, value_min, value_max):
        return max(min(value_max, value), value_min)

    def cmdvelCB(self, msg):
        self.goal_trans_x = self.constrain(
            msg.linear.x, self.linear_min, self.linear_max)
        self.goal_rotat_z = self.constrain(
            msg.angular.z, self.angular_min, self.angular_max)

    def timerCommandCB(self, event):

        vel_wheel_l = (self.goal_trans_x - self.wheelSep /
              2.0*self.goal_rotat_z)/self.wheelRad
        vel_wheel_r = -(self.goal_trans_x + self.wheelSep /
              2.0*self.goal_rotat_z)/self.wheelRad

        byteL = int(vel_wheel_l*10) & 0xFF
        byteR = int(vel_wheel_r*10) & 0xFF

        if vel_wheel_l > self.wheelDeadZone:
            byteL = self.constrain(byteL, 3, 127)
        elif vel_wheel_l < -self.wheelDeadZone:
            byteL = self.constrain(byteL, 128, 253)
        else:
            byteL = 0
        if vel_wheel_r > self.wheelDeadZone:
            byteR = self.constrain(byteR, 3, 127)
        elif vel_wheel_r < -self.wheelDeadZone:
            byteR = self.constrain(byteR, 128, 253)
        else:
            byteR = 0

        command = [255, byteL, byteR, 254]
        if self.serial != None:
            self.serial.write(command)
        # rospy.loginfo(command)

    def timerOdomCB(self, event):
        pose_quat = tf.transformations.quaternion_from_euler(
            0, 0, self.pose.theta)

        msg = Odometry()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = self.odomId
        msg.child_frame_id = self.baseId
        msg.pose.pose.position.x = self.pose.x
        msg.pose.pose.position.y = self.pose.y
        msg.pose.pose.orientation.x = pose_quat[0]
        msg.pose.pose.orientation.y = pose_quat[1]
        msg.pose.pose.orientation.z = pose_quat[2]
        msg.pose.pose.orientation.w = pose_quat[3]
        msg.twist.twist.linear.x = self.trans_x
        msg.twist.twist.angular.z = self.rotat_z

        self.pub_odom.publish(msg)

        if self.isPubOdomFrame:
            self.transform.sendTransform((self.pose.x, self.pose.y, 0), (
                pose_quat[0], pose_quat[1], pose_quat[2], pose_quat[3]), rospy.Time.now(), self.baseId, self.odomId)

    # def serviceReset(self, event):
    #     self.pose_x = 0.0
    #     self.pose_y = 0.0
    #     self.pose_th = 0.0
    #     return EmptyResponse()

    def spin(self):
        while not rospy.is_shutdown():
            try:
                if self.serial.readable():
                    try:
                        if self.serial.read() == b'\xff':
                            byteL = ord(self.serial.read())
                            byteR = ord(self.serial.read())
                            if self.serial.read() == b'\xfe':
                                vel_wheel_l = (256-byteL) * \
                                    (-1/10.0) if byteL > 127 else byteL/10.0
                                vel_wheel_r = (256-byteR) * \
                                    (-1/10.0) if byteR > 127 else byteR/10.0

                                # rospy.loginfo("{}, {}".format(vel_wheel_r,vel_wheel_l))

                                current_time = rospy.Time.now()
                                dt = (current_time - self.previous_time).to_sec()
                                self.previous_time = current_time

                                speedL = vel_wheel_l * self.wheelRad
                                speedR = vel_wheel_r * self.wheelRad

                                self.trans_x = float((speedL - speedR) / 2.0)
                                self.rotat_z = float(-(speedR +
                                                       speedL) / self.wheelSep)
                                # rospy.loginfo(" {}, {}".format(
                                #     self.trans_x, self.rotat_z))

                                self.pose.x += (self.trans_x *
                                                math.cos(self.pose.theta)*dt)
                                self.pose.y += (self.trans_x *
                                                math.sin(self.pose.theta)*dt)
                                self.pose.theta += (self.rotat_z * dt)

                    except Exception:
                        pass
            except KeyboardInterrupt:
                if robot.serial != None:
                    robot.serial.close()
                exit()
            except AttributeError:
                pass


if __name__ == '__main__':
    try:
        rospy.init_node("base_control")
        rospy.loginfo("Base control ...")
        robot = BaseControl()
        robot.spin()
    except rospy.ROSInterruptException:
        pass