gitProject/transbot_driver.py

#!/usr/bin/env python
# encoding: utf-8
import sys
sys.path.append("/home/jetson/Transbot/transbot")
import rospy
import random
import threading
from math import pi
from time import sleep
from transbot_msgs.msg import *
from transbot_msgs.srv import *
from sensor_msgs.msg import Imu
from Transbot_Lib import Transbot
from geometry_msgs.msg import Twist
from arm_transbot import Transbot_ARM
from dynamic_reconfigure.server import Server
from transbot_bringup.cfg import PIDparamConfig

class transbot_driver:
    def __init__(self):
        rospy.on_shutdown(self.cancel)
        self.bot_arm = Transbot_ARM()
        bot_arm_offset = self.bot_arm.get_arm_offset()
        self.bot = Transbot(arm_offset=bot_arm_offset)
        # 弧度转角度
	    # Radians turn angle
        self.RA2DE = 180 / pi
        imu = rospy.get_param("imu", "/transbot/imu")
        vel = rospy.get_param("vel", "/transbot/get_vel")
        self.CameraDevice = rospy.get_param("CameraDevice", "astra")
        self.linear_max = rospy.get_param('~linear_speed_limit', 0.4)
        self.linear_min = rospy.get_param('~linear_speed_limit', 0.0)
        self.angular_max = rospy.get_param('~angular_speed_limit', 2.0)
        self.angular_min = rospy.get_param('~angular_speed_limit', 0.0)
        self.sub_cmd_vel = rospy.Subscriber("/cmd_vel", Twist, self.cmd_vel_callback, queue_size=10)
        self.sub_TargetAngle = rospy.Subscriber("/TargetAngle", Arm, self.sub_armcallback, queue_size=10)
        self.sub_PWMServo = rospy.Subscriber("/PWMServo", PWMServo, self.sub_PWMServocallback, queue_size=10)
        self.sub_Adjust = rospy.Subscriber("/Adjust", Adjust, self.sub_Adjustcallback, queue_size=10)
        self.srv_CurrentAngle = rospy.Service("/CurrentAngle", RobotArm, self.RobotArmcallback)
        self.srv_RGBLight = rospy.Service("/RGBLight", RGBLight, self.RGBLightcallback)
        self.srv_Buzzer = rospy.Service("/Buzzer", Buzzer, self.Buzzercallback)
        self.srv_Headlight = rospy.Service("/Headlight", Headlight, self.Headlightcallback)
        self.ediPublisher = rospy.Publisher('/edition', Edition, queue_size=10)
        self.volPublisher = rospy.Publisher("/voltage", Battery, queue_size=10)
        self.velPublisher = rospy.Publisher(vel, Twist, queue_size=10)
        self.imuPublisher = rospy.Publisher(imu, Imu, queue_size=10)
        self.dyn_server = Server(PIDparamConfig, self.dynamic_reconfigure_callback)
        self.bot.create_receive_threading()
        self.bot.set_uart_servo_angle(9, 90)

    def cancel(self):
        self.srv_CurrentAngle.shutdown()
        self.srv_RGBLight.shutdown()
        self.srv_Buzzer.shutdown()
        self.srv_Headlight.shutdown()
        self.velPublisher.unregister()
        self.imuPublisher.unregister()
        self.volPublisher.unregister()
        self.sub_cmd_vel.unregister()
        self.sub_TargetAngle.unregister()
        self.sub_PWMServo.unregister()
        # Always stop the robot when shutting down the node
        rospy.loginfo("Close the robot...")
        rospy.sleep(1)

    def RobotArmcallback(self, request):
        # 服务端，机械臂当前关节角度
        # Server, the current joint angle of the robotic arm   
        if not isinstance(request, RobotArmRequest): return
        # print ("request: ",request)
        response = RobotArmResponse()
        joints = self.bot.get_uart_servo_angle_array()
        for i in range(3):
            joint = Joint()
            joint.id = i + 7
            joint.angle = joints[i]
            joint.run_time = 500
            if joints[i] < 0: continue
            else: response.RobotArm.joint.append(joint)
            # print ("id: {},angle: {}".format(joint.id, joint.angle))
        return response

    def pub_data(self):
        # 发布小车运动速度、陀螺仪数据、电池电压
	## Publish the speed of the car, gyroscope data, and battery voltage
        while not rospy.is_shutdown():
            sleep(0.05)
            ax, ay, az = self.bot.get_accelerometer_data()
            gx, gy, gz = self.bot.get_gyroscope_data()
            velocity, angular = self.bot.get_motion_data()
            voltage = self.bot.get_battery_voltage()
            battery = Battery()
            battery.Voltage = voltage
            self.volPublisher.publish(battery)
            robot_edition = self.bot.get_version()
            edition = Edition()
            edition.edition = robot_edition
            self.ediPublisher.publish(edition)
            # rospy.loginfo("battery: {}".format(battery))
            # 发布陀螺仪的数据
	        # Publish gyroscope data
            imu = Imu()
            imu.linear_acceleration.x = ax
            imu.linear_acceleration.y = ay
            imu.linear_acceleration.z = az
            imu.angular_velocity.x = gx
            imu.angular_velocity.y = gy
            imu.angular_velocity.z = gz
            self.imuPublisher.publish(imu)
            # 将小车当前的线速度和角速度发布出去
	        # Publish the current linear velocity and angular velocity of the car
            twist = Twist()
            twist.linear.x = velocity
            twist.angular.z = angular
            # print(ax, ay, az, gx, gy, gz)
            # rospy.loginfo("velocity: {}, angular: {}".format(twist.linear.x, twist.angular.z))
            self.velPublisher.publish(twist)

    def sub_Adjustcallback(self, msg):
        # 机械臂控制，订阅者回调函数
	    # Robotic arm control, subscriber callback function
        if not isinstance(msg, Adjust): return
        self.bot.set_imu_adjust(msg.adjust)

    def sub_armcallback(self, msg):
        # 机械臂控制，订阅者回调函数
	    # Robotic arm control, subscriber callback function
        if not isinstance(msg, Arm): return
        for joint in msg.joint:
            if not isinstance(joint, Joint): continue
            # print "joint: ", joint
            if joint.run_time != 0: self.bot.set_uart_servo_angle(joint.id, joint.angle, joint.run_time)

    def sub_PWMServocallback(self, msg):
        # 云台控制，订阅者回调函数
	    # PWMServo control, subscriber callback function
        PWMServo_angle = int(msg.angle)
        if self.CameraDevice == "astra":
            if msg.id == 1:
                if PWMServo_angle <= 60: PWMServo_angle = 60
                elif PWMServo_angle >= 120: PWMServo_angle = 120
        if msg.id == 2:
            if PWMServo_angle > 140: PWMServo_angle = 140
        self.bot.set_pwm_servo(msg.id, PWMServo_angle)

    def cmd_vel_callback(self, msg):
        # 小车运动控制，订阅者回调函数
	    # Car motion control, subscriber callback function
        if not isinstance(msg, Twist): return
        # 下发线速度和角速度
	    # Issue linear velocity and angular velocity
        velocity = msg.linear.x
        angular = msg.angular.z
        # 小车运动控制,velocity=[-0.45, 0.45], angular=[2, 2]
	    # Trolley motion control,velocity=[-0.45, 0.45], angular=[2, 2]
        if velocity > self.linear_max:
            velocity = self.linear_max
        elif velocity < -self.linear_max:
            velocity = -self.linear_max
        elif -self.linear_min < velocity < 0:
            velocity = -self.linear_min
        elif 0 < velocity < self.linear_min:
            velocity = self.linear_min
        if angular > self.angular_max:
            angular = self.angular_max
        elif angular < -self.angular_max:
            angular = -self.angular_max
        elif -self.angular_min < angular < 0:
            angular = -self.angular_min
        elif 0 < angular < self.angular_min:
            angular = self.angular_min
        # rospy.loginfo("cmd_vel: {}, cmd_ang: {}".format(velocity, angular))
        self.bot.set_car_motion(velocity, angular)

    def RGBLightcallback(self, request):
        # 流水灯控制，服务端回调函数
	    # RGBLight control, server callback function
        if not isinstance(request, RGBLightRequest): return
        # print (request.effect, request.speed)
        self.bot.set_colorful_effect(request.effect, request.speed, parm=1)
        response = RGBLightResponse()
        response.result = True
        return response

    def Buzzercallback(self, request):
        # 蜂鸣器控制，服务端回调函数
	    # Buzzer control, server callback function
        if not isinstance(request, BuzzerRequest): return
        self.bot.set_beep(request.buzzer)
        response = BuzzerResponse()
        response.result = True
        return response

    def Headlightcallback(self, request):
        # 探照灯控制，服务端回调函数
	    # Searchlight control, server callback function
        if not isinstance(request, HeadlightRequest): return
        self.bot.set_floodlight(request.Headlight)
        response = HeadlightResponse()
        response.result = True
        return response

    def dynamic_reconfigure_callback(self, config, level):
        # self.bot.set_pid_param(config['Kp'], config['Ki'], config['Kd'])
        print(config['Kp'], config['Ki'], config['Kd'])
        self.linear_max = config['linear_max']
        self.linear_min = config['linear_min']
        self.angular_max = config['angular_max']
        self.angular_min = config['angular_min']
        return config


if __name__ == '__main__':
    rospy.init_node("driver_node", anonymous=False)
    try:
        driver = transbot_driver()
        driver.pub_data()
        rospy.spin()
    except:
        rospy.loginfo("Final!!!")