机器人编程趣味实践06-程序（节点）【图文】

推荐

与往事干杯 2021-08-16 10:02 2021/08/16 ©著作权读给你听

文章标签 机器人编程趣味实践节点原力计划 #define 机器人开发人工智能万象人工智能专栏 专栏分类人工智能 阅读数6874 评论数0 点赞数4 收藏数0

导读：分别输入如下命令：ros2 run examples_rclcpp_minimal_publisher publisher_lambdaros2 run turtlesim turtlesim_node..

分别输入如下命令：

ros2 run examples_rclcpp_minimal_publisher publisher_lambda
ros2 run turtlesim turtlesim_node

效果如下图所示：

机器人编程趣味实践06-程序（节点）_机器人编程

机器人编程趣味实践06-程序（节点）_#define_02

要想更酷炫的效果，需要三维仿真软件，如下：

机器人编程趣味实践06-程序（节点）_节点_03

必然也支持真实机器人哦。

迷你机器人案例

每敲击一行指令，开启一个或多个程序，具体介绍参考（ROS 2节点-nodes-）。

每个节点可以通过主题消息、服务消息、行动消息或参数相互之间传递数据哦，多台电脑手机之间也可以的。

ros2 run <package_name> <executable_name>

```
ros2 run turtlesim turtlesim_node
```
```
ros2 node list
```

机器人编程趣味实践06-程序（节点）_趣味实践_04

开启一个遥控节点：

```
ros2 run turtlesim turtle_teleop_key
```

机器人编程趣味实践06-程序（节点）_趣味实践_05

如果需要中文显示，需要修改代码如下：

//  puts("Reading from keyboard");
//  puts("---------------------------");
//  puts("Use arrow keys to move the turtle.");
//  puts("Use G|B|V|C|D|E|R|T keys to rotate to absolute orientations. 'F' to cancel a rotation.");
//  puts("'Q' to quit.");

  puts("读取键盘信息. ");
  puts("--------------------------- ");
  puts("使用方向键移动机器人. ");
  puts("使用 G|B|V|C|D|E|R|T 键使机器人转相应角度. 'F' 键取消旋转. ");
  puts("'Q' 键退出遥控. ");

这种方式也可以实现相应开源软件的汉化，但无技术难度……

此时，可以看到两个节点了哦：

机器人编程趣味实践06-程序（节点）_机器人编程_06

机器人程序的通用型，如何体现呢？比如相似功能的节点是否支持多种机器人遥控，而无需修改代码呢？

重映射（Remapping）

此时开启机器人turtlebot3！

通过映射可以同时遥控二维和三维环境中的机器人吗？试一试吧。

ros2 run turtlesim turtlesim_node --ros-args --remap __node:=turtlebot3

这显然不行，这只是一个重命名呢………………

机器人编程趣味实践06-程序（节点）_机器人编程_08

如果需要查看节点的信息使用如下命令：

```
ros2 node info <node_name>
```
```
ros2 node info /turtlebot3_diff_drive
```

机器人编程趣味实践06-程序（节点）_原力计划_09

机器人编程趣味实践06-程序（节点）_趣味实践_10

节点的全部功能如下：

机器人编程趣味实践06-程序（节点）_趣味实践_11

本节涉及的键盘遥控效果：

机器人编程趣味实践06-程序（节点）_趣味实践_12

有没有发现什么不对劲的地方？更多内容，下一节继续。

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <turtlesim/action/rotate_absolute.hpp>

#include <signal.h>
#include <stdio.h>
#ifndef _WIN32
# include <termios.h>
# include <unistd.h>
#else
# include <windows.h>
#endif

#define KEYCODE_RIGHT 0x43
#define KEYCODE_LEFT 0x44
#define KEYCODE_UP 0x41
#define KEYCODE_DOWN 0x42
#define KEYCODE_B 0x62
#define KEYCODE_C 0x63
#define KEYCODE_D 0x64
#define KEYCODE_E 0x65
#define KEYCODE_F 0x66
#define KEYCODE_G 0x67
#define KEYCODE_Q 0x71
#define KEYCODE_R 0x72
#define KEYCODE_T 0x74
#define KEYCODE_V 0x76

class KeyboardReader
{
public:
  KeyboardReader()
#ifndef _WIN32
    : kfd(0)
#endif
  {
#ifndef _WIN32
    // get the console in raw mode
    tcgetattr(kfd, &cooked);
    struct termios raw;
    memcpy(&raw, &cooked, sizeof(struct termios));
    raw.c_lflag &=~ (ICANON | ECHO);
    // Setting a new line, then end of file
    raw.c_cc[VEOL] = 1;
    raw.c_cc[VEOF] = 2;
    tcsetattr(kfd, TCSANOW, &raw);
#endif
  }
  void readOne(char * c)
  {
#ifndef _WIN32
    int rc = read(kfd, c, 1);
    if (rc < 0)
    {
      throw std::runtime_error("read failed");
    }
#else
    for(;;)
    {
      HANDLE handle = GetStdHandle(STD_INPUT_HANDLE);
      INPUT_RECORD buffer;
      DWORD events;
      PeekConsoleInput(handle, &buffer, 1, &events);
      if(events > 0)
      {
        ReadConsoleInput(handle, &buffer, 1, &events);
        if (buffer.Event.KeyEvent.wVirtualKeyCode == VK_LEFT)
        {
          *c = KEYCODE_LEFT;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == VK_UP)
        {
          *c = KEYCODE_UP;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == VK_RIGHT)
        {
          *c = KEYCODE_RIGHT;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == VK_DOWN)
        {
          *c = KEYCODE_DOWN;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x42)
        {
          *c = KEYCODE_B;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x43)
        {
          *c = KEYCODE_C;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x44)
        {
          *c = KEYCODE_D;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x45)
        {
          *c = KEYCODE_E;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x46)
        {
          *c = KEYCODE_F;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x47)
        {
          *c = KEYCODE_G;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x51)
        {
          *c = KEYCODE_Q;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x52)
        {
          *c = KEYCODE_R;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x54)
        {
          *c = KEYCODE_T;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x56)
        {
          *c = KEYCODE_V;
          return;
        }
      }
    }
#endif
  }
  void shutdown()
  {
#ifndef _WIN32
    tcsetattr(kfd, TCSANOW, &cooked);
#endif
  }
private:
#ifndef _WIN32
  int kfd;
  struct termios cooked;
#endif
};

class TeleopTurtle
{
public:
  TeleopTurtle();
  int keyLoop();

private:
  void spin();
  void sendGoal(float theta);
  void goalResponseCallback(std::shared_future<rclcpp_action::ClientGoalHandle<turtlesim::action::RotateAbsolute>::SharedPtr> future);
  void cancelGoal();
  
  rclcpp::Node::SharedPtr nh_;
  double linear_, angular_, l_scale_, a_scale_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
  rclcpp_action::Client<turtlesim::action::RotateAbsolute>::SharedPtr rotate_absolute_client_;
  rclcpp_action::ClientGoalHandle<turtlesim::action::RotateAbsolute>::SharedPtr goal_handle_;
};

TeleopTurtle::TeleopTurtle():
  linear_(0),
  angular_(0),
  l_scale_(2.0),
  a_scale_(2.0)
{
  nh_ = rclcpp::Node::make_shared("teleop_turtle");
  nh_->declare_parameter("scale_angular", rclcpp::ParameterValue(2.0));
  nh_->declare_parameter("scale_linear", rclcpp::ParameterValue(2.0));
  nh_->get_parameter("scale_angular", a_scale_);
  nh_->get_parameter("scale_linear", l_scale_);

  twist_pub_ = nh_->create_publisher<geometry_msgs::msg::Twist>("turtle1/cmd_vel", 1);
  rotate_absolute_client_ = rclcpp_action::create_client<turtlesim::action::RotateAbsolute>(nh_, "turtle1/rotate_absolute");
}

void TeleopTurtle::sendGoal(float theta)
{
  auto goal = turtlesim::action::RotateAbsolute::Goal();
  goal.theta = theta;
  auto send_goal_options = rclcpp_action::Client<turtlesim::action::RotateAbsolute>::SendGoalOptions();
  send_goal_options.goal_response_callback =
    [this](std::shared_future<rclcpp_action::ClientGoalHandle<turtlesim::action::RotateAbsolute>::SharedPtr> future)
    {
      RCLCPP_DEBUG(nh_->get_logger(), "Goal response received");
      this->goal_handle_ = future.get();
    };
  rotate_absolute_client_->async_send_goal(goal, send_goal_options);
}

void TeleopTurtle::goalResponseCallback(std::shared_future<rclcpp_action::ClientGoalHandle<turtlesim::action::RotateAbsolute>::SharedPtr> future)
{
  RCLCPP_DEBUG(nh_->get_logger(), "Goal response received");
  this->goal_handle_ = future.get();
}

void TeleopTurtle::cancelGoal()
{
 if (goal_handle_)
 {
   RCLCPP_DEBUG(nh_->get_logger(), "Sending cancel request");
   try
   {
     rotate_absolute_client_->async_cancel_goal(goal_handle_);
   }
   catch (...)
   {
     // This can happen if the goal has already terminated and expired
   }
 }
}

KeyboardReader input;

void quit(int sig)
{
  (void)sig;
  input.shutdown();
  rclcpp::shutdown();
  exit(0);
}


int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  TeleopTurtle teleop_turtle;

  signal(SIGINT,quit);

  int rc = teleop_turtle.keyLoop();
  input.shutdown();
  rclcpp::shutdown();
  
  return rc;
}

void TeleopTurtle::spin()
{
  while (rclcpp::ok())
  {
    rclcpp::spin_some(nh_);
  }
}

int TeleopTurtle::keyLoop()
{
  char c;
  bool dirty=false;

  std::thread{std::bind(&TeleopTurtle::spin, this)}.detach();

//  puts("Reading from keyboard");
//  puts("---------------------------");
//  puts("Use arrow keys to move the turtle.");
//  puts("Use G|B|V|C|D|E|R|T keys to rotate to absolute orientations. 'F' to cancel a rotation.");
//  puts("'Q' to quit.");

  puts("读取键盘信息. ");
  puts("--------------------------- ");
  puts("使用方向键移动机器人. ");
  puts("使用 G|B|V|C|D|E|R|T 键使机器人转相应角度. 'F' 键取消旋转. ");
  puts("'Q' 键退出遥控. ");

  for(;;)
  {
    // get the next event from the keyboard  
    try
    {
      input.readOne(&c);
    }
    catch (const std::runtime_error &)
    {
      perror("read():");
      return -1;
    }

    linear_=angular_=0;
    RCLCPP_DEBUG(nh_->get_logger(), "value: 0x%02X\n", c);
  
    switch(c)
    {
      case KEYCODE_LEFT:
        RCLCPP_DEBUG(nh_->get_logger(), "LEFT");
        angular_ = 1.0;
        dirty = true;
        break;
      case KEYCODE_RIGHT:
        RCLCPP_DEBUG(nh_->get_logger(), "RIGHT");
        angular_ = -1.0;
        dirty = true;
        break;
      case KEYCODE_UP:
        RCLCPP_DEBUG(nh_->get_logger(), "UP");
        linear_ = 1.0;
        dirty = true;
        break;
      case KEYCODE_DOWN:
        RCLCPP_DEBUG(nh_->get_logger(), "DOWN");
        linear_ = -1.0;
        dirty = true;
        break;
      case KEYCODE_G:
        RCLCPP_DEBUG(nh_->get_logger(), "G");
        sendGoal(0.0f);
        break;
      case KEYCODE_T:
        RCLCPP_DEBUG(nh_->get_logger(), "T");
        sendGoal(0.7854f);
        break;
      case KEYCODE_R:
        RCLCPP_DEBUG(nh_->get_logger(), "R");
        sendGoal(1.5708f);
        break;
      case KEYCODE_E:
        RCLCPP_DEBUG(nh_->get_logger(), "E");
        sendGoal(2.3562f);
        break;
      case KEYCODE_D:
        RCLCPP_DEBUG(nh_->get_logger(), "D");
        sendGoal(3.1416f);
        break;
      case KEYCODE_C:
        RCLCPP_DEBUG(nh_->get_logger(), "C");
        sendGoal(-2.3562f);
        break;
      case KEYCODE_V:
        RCLCPP_DEBUG(nh_->get_logger(), "V");
        sendGoal(-1.5708f);
        break;
      case KEYCODE_B:
        RCLCPP_DEBUG(nh_->get_logger(), "B");
        sendGoal(-0.7854f);
        break;
      case KEYCODE_F:
        RCLCPP_DEBUG(nh_->get_logger(), "F");
        cancelGoal();
        break;
      case KEYCODE_Q:
        RCLCPP_DEBUG(nh_->get_logger(), "quit");
        return 0;
    }
   

    geometry_msgs::msg::Twist twist;
    twist.angular.z = a_scale_*angular_;
    twist.linear.x = l_scale_*linear_;
    if(dirty ==true)
    {
      twist_pub_->publish(twist);    
      dirty=false;
    }
  }


  return 0;
}

更多文章请关注《万象专栏》

转载请注明出处：https://www.wanxiangsucai.com/read/cv12160

话题推荐： #机器人编程# #趣味实践# #节点# #原力计划# ##define# #机器人开发# #人工智能# #万象人工智能专栏#

打赏
4 赞
收藏
评论
举报

下一篇：人工智能技术及其赋能的含义解析

发布评论

全部评论(0)

模板

HTML模板

织梦模板

帝国模板

discuz模板

PHPWind

Wordpress

源码

CMS源码

PHP源码

ASP源码

GO源码

python源码

整站源码

特效代码

幻灯片代码

菜单导航

客服代码

素材

图标

元素

背景

ppt

字体

动图

模板

模板

海报

插画

UI

网页

淘宝

图库

高清壁纸

手机壁纸

万象图床