ROS学习——通过键盘控制机器人运动

ROS学习

通过键盘实现机器人的运动

C++ 代码

• 1. 在软件包中创建节点
     代码及其解析如下

  #include 
  #include 
  #include 
  #include 
  
  using namespace std;
  
  // 设定键位映射 (线速度, 角速度)
  map<char, pair<double, double>> key_mapping = 
  {
      {'w', {1.0, 0.0}},  // 前进
      {'s', {-1.0, 0.0}}, // 后退
      {'a', {0.0, 1.0}},  // 左转
      {'d', {0.0, -1.0}}, // 右转
      {'q', {0.0, 0.0}}   // 停止
  };
  
  void setTerminalMode()
  {
      struct termios new_settings;
      tcgetattr(0, &new_settings);
      new_settings.c_lflag &= ~ICANON; // 规定按键输入后不需要回车立即生效
      new_settings.c_lflag &= ~ECHO;   // 规定输入不显示在终端上
      new_settings.c_cc[VMIN] = 1;     // 规定按键必须输入一个字符 
      new_settings.c_cc[VTIME] = 0;    // 规定输入进行不等待，即立即响应
      tcsetattr(0, TCSANOW, &new_settings); // 进行设置
  }
  
  char getKey()
  {
      char c;
      if (read(0, &c, 1) < 0) return '\0';
      return c;
  }
  
  int main(int argc, char **argv)
  {
      ros::init(argc, argv, "keyboard_control"); // 初始化节点
      ros::NodeHandle nh;
      ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10); // 规定话题参数
      ros::Rate rate(10); // 控制发布话题频率
  
      setTerminalMode();
      cout << "控制小车移动：\n---------------------------\n   w    \n a  q  d\n   s    \n\nw/s: 前进/后退\na/d: 左转/右转\nq: 停止\nCTRL+C 退出\n";
      geometry_msgs::Twist twist;
      while (ros::ok()) 
      {
          char key = getKey();
          if (key_mapping.count(key) && key != 'q') //判断
          {
              twist.linear.x += key_mapping[key].first * 0.5;
              twist.angular.z += key_mapping[key].second * 0.5;
              pub.publish(twist);
          }
          if (key == 'q') 
          {
              twist.linear.x = 0;
              twist.angular.z = 0;
              pub.publish(twist);
          }
          rate.sleep(); // 设置频率
      }
      
      geometry_msgs::Twist empty_twist; 
      pub.publish(empty_twist); // 发布一个空话题，使小车停止移动
      return 0;
  }
  

• 2. 在该节点的软件包中的CMakelist.txt文件添加节点声明如下

  add_executable(keyboard_control src/keyboard_control.cpp)
  target_link_libraries(keyboard_control
  ${catkin_LIBRARIES}
  )
  add_dependencies(keyboard_control ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
  

• 3. catkin_make编译工作空间

---

Python 代码

• 软件包中新建节点，编写代码

  #!/usr/bin/env python3
  #coding = utf-8
  import rospy
  import sys
  import termios
  import tty
  from geometry_msgs.msg import Twist
  
  # 设定键位映射 (线速度, 角速度)
  key_mapping = {
      'w': (0.1, 0.0),  # 前进
      's': (-0.1, 0.0),  # 后退
      'a': (0.0, 0.1),   # 左转
      'd': (0.0, -0.1),  # 右转
      'q': (0.0, 0.0)    # 停止
  }
  
  """ 获取键盘输入 """
  def get_key():
      fd = sys.stdin.fileno() 
      old_settings = termios.tcgetattr(fd)
      try:
          tty.setraw(sys.stdin.fileno())  # 设置终端不回显和缓冲输入
          key = sys.stdin.read(1)  # 读取一个字符
      finally:
          termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)  # 还原终端设置
      return key
  
  def main():
      rospy.init_node("keyboard_control")  # 初始化 ROS 节点
      pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)  # 发布 /cmd_vel 话题
      rate = rospy.Rate(10)  # 设置发布话题频率
  
      twist = Twist()
      print("控制小车移动：\n---------------------------\n   w    \n a  q  d\n   s    \n\nw/s: 前进/后退\na/d: 左转/右转\nq: 停止\nCTRL+C 退出\n")
  
      while not rospy.is_shutdown():
          key = get_key()
          if key in key_mapping:
              twist.linear.x += key_mapping[key][0] 
              twist.angular.z += key_mapping[key][1]
  
          if key == 'q':  # 按 'q' 停止
              twist.linear.x = 0
              twist.angular.z = 0
          
          pub.publish(twist)
          rate.sleep()
  
      empty_twist = Twist()
      pub.publish(empty_twist)
  
  if __name__ == "__main__":
      main()
  

• 2. 打开文件路径下的终端，通过chmod +x keyboard_control给予节点可执行权限

---

通过仿真环境观察代码现象

• 确保已经下载wpr_simulation软件包，没下载的可以看我的上一篇文章
• 1. 打开终端执行roslaunch wpr_simulation wpb_simple.launch
• 2. 打开一个新的终端执行rosrun vel_pkg keyboard_control节点

代码现象如下

键盘控制机器人运动