探索使用科大讯飞语音包控制机器狗（三）--控制小狗移动（控制脚本修改）

上一篇文章尝试了将语音识别SDK移植到机器狗树莓派主控上，最终也是出现了一些问题，但是验证了语音识别的可行性。本篇文章主要来改写机器狗的控制脚本，为接下来的语音控制作铺垫。

编写启动文件

因为原先的启动步骤比较繁琐，所以尝试编写一个launch文件启动所有节点。

<?xml version="1.0" encoding="utf-8"?>
<!-- Launch file -->
<launch>
    <!-- Defining the node and executable and publishing the output on terminal-->
	<node name="i2cpwm_board" pkg="i2cpwm_board" type="i2cpwm_board" output="screen" />
    <node name="spot_micro_motion_cmd_node" pkg="spot_micro_motion_cmd" type="spot_micro_motion_cmd_node" output="screen">
        <!-- loading the parameters from yaml file during th runtime -->
        <rosparam command="load" file="$(find spot_micro_motion_cmd)/config/spot_micro_motion_cmd.yaml"/>
    </node>
	<node name="spot_micro_keyboard_control" pkg="spot_micro_keyboard_command" type="spotMicroKeyboardMove.py" output="screen" launch-prefix="xterm -e" />
</launch>

源码研读

代码修改

我再初步修改了键盘控制部分脚本后，导入后执行时出现了一些问题，后来发现是因为脚本没有执行权限导致的，修改权限即可解决问题。

脚本修改有一点问题，现在的想法是站立状态然后我直接让其行走一定的时间然后自动切换回站立状态，因为狗的控制默认行走状态的时候是原地踏步的，很傻。但是在我进行状态切换的时候，虽然发布了速度指令但是狗还是在原地踏步。关于此问题先记录一下，探讨一下原因，自我感觉是因为控制状态的切换问题，我需要再多看看代码结构。

• 代码结构
  除了robot_voice是我自己创建的功能包之外，其余代码全部都是源码，比较重要的是ros-i2cpwmboard,spot_micro_keyboard_command和spot_micro_motion_cmd。
  

lcd_monitor：这部分代码是控制显示屏显示机器狗的状态信息，外接1602显示屏，不过并没有测试过功能的完整性。

i2cpwm_board：这部分代码是专门用来控制pca 9685舵机板子的，与舵机板进行通信，从而控制各个舵机的运动。

servo_move_keyboard：这部分代码是可以分别调整各个舵机的运动，主要用来进行舵机的初始姿态矫正，配置初始参数。

spot_micro_keyboard_command：这部分代码是通过键盘发送指令来控制机器狗的移动，调整机器狗的运动状态，也是目前我正在修改的代码。

spot_micro_motion_cmd：这部分代码是主节点，运行机器人控制软件。接收状态事件命令和运动命令，输出舵机控制命令。将yaml配置文件用于各种软件设置。由具有5个状态的状态机和以下模式图组成。
spot_micro_plot：通过matplotlib显示机器人的线框图，并从spot_micro_motion_cmd接收状态数据。如果spot_micro_motion_cmd yaml文件中的debug_mode参数设置为true，则可以使用该图节点代替实际的机器人来测试运动。

• 代码解读

• 1.spot_micro_keyboard_command：

由于代码比较长，所以只截取关键部分进行注释解读。spotMicroKeyboardMove.py
41-46行

valid_cmds = ('quit','Quit','walk','stand','idle', 'angle_cmd')# 有效的输入命令

# Global body motion increment values，定义增量的最小量
speed_inc = 0.02	#walk模式的增量，改变速度
yaw_rate_inc = 2*pi/180		#walk模式改变狗姿态的增量。
angle_inc = 2.5*pi/180		#angle_cmd模式改变狗姿态的增量。

52-78

		''' 有一个问题就是_x_speed_cmd_msg和_speed_cmd_msg.x的区别还没搞明白，要看一看'''
	  	self._x_speed_cmd_msg = Float32()	#为身体运动命令创建消息，并初始化为零
        self._x_speed_cmd_msg.data = 0

        self._y_speed_cmd_msg = Float32()
        self._y_speed_cmd_msg.data = 0

        self._yaw_rate_cmd_msg = Float32()
        self._yaw_rate_cmd_msg.data = 0

        self._angle_cmd_msg = Vector3()
        self._angle_cmd_msg.x = 0
        self._angle_cmd_msg.y = 0
        self._angle_cmd_msg.z = 0

        self._speed_cmd_msg = Vector3()
        self._speed_cmd_msg.x = 0
        self._speed_cmd_msg.y = 0
        self._speed_cmd_msg.z = 0
#创建了三个状态，walk,stand,idle。
        self._walk_event_cmd_msg = Bool()
        self._walk_event_cmd_msg.data = True # Mostly acts as an event driven action on receipt of a true message

        self._stand_event_cmd_msg = Bool()
        self._stand_event_cmd_msg.data = True
       
        self._idle_event_cmd_msg = Bool()
        self._idle_event_cmd_msg.data = True

80-98行

		''' 这部分是在进行节点初始化并创建发布器 节点名称：spot_micro_keyboard_control'''
		
		rospy.loginfo("Setting Up the Spot Micro Keyboard Control Node...")#输出信息打印在终端

        # Set up and title the ros node for this code
        rospy.init_node('spot_micro_keyboard_control')

        # Create publishers for x,y speed command, body rate command, and state command
        self.ros_pub_x_speed_cmd    = rospy.Publisher('x_speed_cmd',Float32,queue_size=1)
        self.ros_pub_y_speed_cmd    = rospy.Publisher('/y_speed_cmd',Float32,queue_size=1)
        self.ros_pub_yaw_rate_cmd   = rospy.Publisher('/yaw_rate_cmd',Float32,queue_size=1)
        self.ros_pub_speed_cmd      = rospy.Publisher('/speed_cmd',Vector3,queue_size=1)
        self.ros_pub_angle_cmd      = rospy.Publisher('/angle_cmd',Vector3,queue_size=1)
        self.ros_pub_state_cmd      = rospy.Publisher('/state_cmd',Bool,queue_size=1)
        self.ros_pub_walk_cmd       = rospy.Publisher('/walk_cmd',Bool, queue_size=1)
        self.ros_pub_stand_cmd      = rospy.Publisher('/stand_cmd',Bool,queue_size=1)
        self.ros_pub_idle_cmd       = rospy.Publisher('/idle_cmd',Bool,queue_size=1)

        rospy.loginfo("> Publishers corrrectly initialized")

        rospy.loginfo("Initialization complete")

100-101

 	# Setup terminal input reading, taken from teleop_twist_keyboard
     self.settings = termios.tcgetattr(sys.stdin)#读取键盘输入

128-133

#定义getKey函数，此函数返回值为键盘输入的一个字母。
def getKey(self):
        tty.setraw(sys.stdin.fileno())
        select.select([sys.stdin], [], [], 0)
        key = sys.stdin.read(1)
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
        return key

135-163

#定义函数run，在这个函数中发布动作指令等消息。
 def run(self):
        # Publish all body motion commands to 0
        self.reset_all_motion_commands_to_zero()

        # Prompt user with keyboard command information
        
        while not rospy.is_shutdown():#当没接收到停止指令时一直执行，停止指令比如“ctrl+c”。
            print(msg)
            userInput = raw_input("Command?: ")#raw_input函数的作用是接受终端输入的字符串。

            if userInput not in valid_cmds:	#判断接收到的字符串是否符合规定
                print('Valid command not entered, try again...')
            else:
                if userInput == 'quit':
                    print("Ending program...")
                    break
                
                elif userInput == 'stand':
                    #Publish stand command event
                    self.ros_pub_stand_cmd.publish(self._stand_event_cmd_msg)#发布站立事件self.ros_pub_stand_cmd = rospy.Publisher('/stand_cmd',Bool,queue_size=1)，话题是stand_cmd
                
                elif userInput == 'idle':
                    #Publish idle command event
                    self.ros_pub_idle_cmd.publish(self._idle_event_cmd_msg)

                elif userInput == 'angle_cmd':
                    # Reset all angle commands
                    self.reset_all_angle_commands_to_zero()
                    

204-238行
这一部分代码是控制行走动作的脚本，也是主要进行修改的地方，所以分析细致一些。我目前启动的节点只有舵机板控制，spot_micro_motion_cmd和spot_micro_keyboard_commond。这个spotMicroKeyboardMove.py发布了很多的信息，光速度就有不同的两种，我本来还纠结都是干什么用的，后来发现运行基础功能的时候只订阅了其中的一种。

• speed_cmd
  这个话题被spot_micro_motion_cmd所订阅。

• walk_cmd
  被订阅
  
• x_speed_cmd
  没有被订阅，经过代码查看这个消息是在spot_micro_walk节点被订阅。
  
• state_cmd
  同样被spot_micro_walk所订阅
  

			elif userInput == 'walk':
                    # Reset all body motion commands to zero
                    #同样的，进入行走状态首先置零
                    self.reset_all_motion_commands_to_zero()

                    # Publish walk event
                    #发布状态信息。
                    self.ros_pub_state_cmd.publish(self._walk_event_cmd_msg)#发送给spot_micro_walk节点，目前并未启用
                    self.ros_pub_walk_cmd.publish(self._walk_event_cmd_msg)#发送给spot_micro_motion_cmd,被其订阅使用。

                    # Enter loop to act on user command

                    print('Enter command, u to go back to stand mode: ')
	#循环执行
                    while (1):
                        print('Cmd Values: x speed: %1.3f m/s, y speed: %1.3f m/s, yaw rate: %1.3f deg/s '\
                                %(self._x_speed_cmd_msg.data,self._y_speed_cmd_msg.data,self._yaw_rate_cmd_msg.data*180/pi))
                       #打印速度信息
                        userInput = self.getKey()#从键盘接收一个字母

                        if userInput == 'u':
                            # Send walk event message, this will take robot back to rest mode
                            self.ros_pub_state_cmd.publish(self._walk_event_cmd_msg)#暂时自动忽略state

                            self.ros_pub_stand_cmd.publish(self._stand_event_cmd_msg)#返回站立模式
                            break	#结束循环

                        elif userInput not in ('w','a','s','d','q','e','u','f'):
                            print('Key not in valid key commands, try again')
                        else:
                            if userInput == 'w':
                                self._x_speed_cmd_msg.data = self._x_speed_cmd_msg.data + speed_inc
                                self.ros_pub_x_speed_cmd.publish(self._x_speed_cmd_msg)

                                self._speed_cmd_msg.x = self._speed_cmd_msg.x + speed_inc#速度增加，前进
                                self.ros_pub_speed_cmd.publish(self._speed_cmd_msg)#发布话题
                            

其他的指令的代码都相似，不再进行赘述。
278-282

 elif userInput == 'f':	#归零
                                self._speed_cmd_msg.x = 0
                                self._speed_cmd_msg.y = 0
                                self._speed_cmd_msg.z = 0
                                self._x_speed_cmd_msg.data = 0
                                self._y_speed_cmd_msg.data = 0
                                self._yaw_rate_cmd_msg.data = 0

                                self.ros_pub_speed_cmd.publish(self._speed_cmd_msg)

288-290行

#主程序运行
if __name__ == "__main__":
    smkc     = SpotMicroKeyboardControl()
    smkc.run()

• 2.spot_micro_motion_cmd
  spot_micro_motion_cmd.cpp代码：上一部分键盘控制代码主要是进行话题的发布，这一部分运动执行模块就是要接收到发布的话题进行进一步的处理。
  20-42

/* 定义回调函数 */
//站立状态
void SpotMicroMotionCmd::standCommandCallback(
    const std_msgs::Bool::ConstPtr& msg) {
  if (msg->data == true) {cmd_.stand_cmd_ = true;}
}

//休息状态
void SpotMicroMotionCmd::idleCommandCallback(
    const std_msgs::Bool::ConstPtr& msg) {
  if (msg->data == true) {cmd_.idle_cmd_ = true;}
}

//行走状态
void SpotMicroMotionCmd::walkCommandCallback(
    const std_msgs::Bool::ConstPtr& msg) {
  if (msg->data == true) {cmd_.walk_cmd_ = true;}
}
//传递速度
void SpotMicroMotionCmd::speedCommandCallback(
    const geometry_msgs::Vector3ConstPtr& msg) {
  cmd_.x_vel_cmd_mps_ = msg->x;
  cmd_.y_vel_cmd_mps_ = msg->y;
  cmd_.yaw_rate_cmd_rps_ = msg->z;
}

102-137

// Initialize publishers and subscribers
//初始化发布器和订阅器
//订阅器
  // stand cmd event subscriber 站立
  stand_sub_ = nh.subscribe("/stand_cmd", 1, &SpotMicroMotionCmd::standCommandCallback, this);
    
  // idle cmd event subscriber 休息
  idle_sub_ = nh.subscribe("/idle_cmd", 1, &SpotMicroMotionCmd::idleCommandCallback, this);

  // walk cmd event subscriber 行走
  walk_sub_ = nh.subscribe("/walk_cmd", 1, &SpotMicroMotionCmd::walkCommandCallback, this);
 
  // speed command subscriber 行走速度
  speed_cmd_sub_ = nh.subscribe("/speed_cmd", 1, &SpotMicroMotionCmd::speedCommandCallback, this);  

  // body angle command subscriber 身体角度
  body_angle_cmd_sub_ = nh.subscribe("/angle_cmd", 1, &SpotMicroMotionCmd::angleCommandCallback, this);  
//发布器
  // servos_absolute publisher 
  servos_absolute_pub_ = nh.advertise<i2cpwm_board::ServoArray>("servos_absolute", 1);

  // Servos proportional publisher
  servos_proportional_pub_ = nh.advertise<i2cpwm_board::ServoArray>("servos_proportional",1);  
  
  // Servos configuration publisher
  servos_config_client_ = nh.serviceClient<i2cpwm_board::ServosConfig>("config_servos");

  // Body state publisher for plotting
  body_state_pub_ = nh.advertise<std_msgs::Float32MultiArray>("body_state",1);

  // State string publisher for lcd monitor
  sm_state_pub_ = nh.advertise<std_msgs::String>("sm_state",1);

  // Speed command state publisher for lcd monitor
  sm_speed_cmd_pub_ = nh.advertise<geometry_msgs::Vector3>("sm_speed_cmd",1);

  // Angle command state publisher for lcd monitor
  sm_angle_cmd_pub_ = nh.advertise<geometry_msgs::Vector3>("sm_angle_cmd",1);

• walk_test.py
  这个是我修改的代码，主要修改了指令动作。
  主要的代码结构是这样的，直接发布速度指令，然后延时一段时间后停止，回到站立状态。
  但是这个代码有点问题，机器狗没有接收到速度指令，不会前进，只会原地踏步，但是先是机器狗进入walk状态之后再发布前进指令就可以正常前进，所以我怀疑是消息发生了阻塞。

								self._x_speed_cmd_msg.data = -0.05
                                self.ros_pub_x_speed_cmd.publish(self._x_speed_cmd_msg)

                                self._speed_cmd_msg.x = -0.05
                                self.ros_pub_speed_cmd.publish(self._speed_cmd_msg)

                                time.sleep( 10 )//目前直接使用sleep函数有点问题，这时候机器狗是不受控制的，考虑之后使用其他方法进行延时操作。

                                self._speed_cmd_msg.x = 0
                                self._speed_cmd_msg.y = 0
                                self._speed_cmd_msg.z = 0
                                self._x_speed_cmd_msg.data = 0
                                self._y_speed_cmd_msg.data = 0
                                self._yaw_rate_cmd_msg.data = 0

                                self.ros_pub_speed_cmd.publish(self._speed_cmd_msg)

解决办法：

                                self.ros_pub_state_cmd.publish(self._walk_event_cmd_msg)
                                self.ros_pub_walk_cmd.publish(self._walk_event_cmd_msg)

                                time.sleep( 0.1 )	//在发布了walk状态之后做一个小延时，避免消息阻塞。

                                self._x_speed_cmd_msg.data = 0.05
                                self.ros_pub_x_speed_cmd.publish(self._x_speed_cmd_msg)

                                self._speed_cmd_msg.x = 0.05
                                self.ros_pub_speed_cmd.publish(self._speed_cmd_msg)

                                print('Cmd Values: x speed: %1.3f m/s, y speed: %1.3f m/s, yaw rate: %1.3f deg/s '\
                                %(self._speed_cmd_msg.x,self._speed_cmd_msg.y,self._yaw_rate_cmd_msg.data*180/pi)) 

                                time.sleep( 10 )

                                self._speed_cmd_msg.x = 0
                                self._speed_cmd_msg.y = 0
                                self._speed_cmd_msg.z = 0
                                self._x_speed_cmd_msg.data = 0
                                self._y_speed_cmd_msg.data = 0
                                self._yaw_rate_cmd_msg.data = 0

                                self.ros_pub_speed_cmd.publish(self._speed_cmd_msg)
                                self.ros_pub_stand_cmd.publish(self._stand_event_cmd_msg)

经过检验此部分代码可以达到预想的移动效果。