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

分别输入如下命令：

ros2 run examples_rclcpp_minimal_publisher publisher_lambda

ros2 run turtlesim turtlesim_node

效果如下图所示：

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

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

迷你机器人案例

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

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

ros2 run

ros2 run turtlesim turtlesim_node

ros2 node list

开启一个遥控节点：

ros2 run turtlesim turtle_teleop_key

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

// 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' 键退出遥控. ");

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

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

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

重映射（Remapping）

此时开启机器人turtlebot3！

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

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

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

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

ros2 node info

ros2 node info /turtlebot3_diff_drive

节点的全部功能如下：

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

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

#include

#include

#include

#include

#include

#include

#ifndef _WIN32

# include

# include

#else

# include

#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::SharedPtr> future);

void cancelGoal();

rclcpp::Node::SharedPtr nh_;

double linear_, angular_, l_scale_, a_scale_;

rclcpp::Publisher::SharedPtr twist_pub_;

rclcpp_action::Client::SharedPtr rotate_absolute_client_;

rclcpp_action::ClientGoalHandle::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("turtle1/cmd_vel", 1);

rotate_absolute_client_ = rclcpp_action::create_client(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::SendGoalOptions();

send_goal_options.goal_response_callback =

[this](std::shared_future::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::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;

}

机器人

版权声明：本文内容由网络用户投稿，版权归原作者所有，本站不拥有其著作权，亦不承担相应法律责任。如果您发现本站中有涉嫌抄袭或描述失实的内容，请联系我们jiasou666@gmail.com 处理，核实后本网站将在24小时内删除侵权内容。