机器人编程趣味实践06-程序（节点）
【摘要】 分别输入如下命令：
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
效果如下图所示：
要想更酷炫的效果，需要三维仿真软件，如下：
必然也支持真实机器人哦。
每敲击一行指令，开启一个或多个程序，具体介绍参考（ROS 2节点-nodes-）。
每个节点可以通过主题消息、服务消息、行动消息或参数相互之间传递数据哦，多台电脑手机之间也可以的。
-
ros2 run <package_name> <executable_name>
-
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 <node_name>
-
ros2 node info /turtlebot3_diff_drive
节点的全部功能如下：
本节涉及的键盘遥控效果：
有没有发现什么不对劲的地方？更多内容，下一节继续。
-
#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;
-
}
-
-
文章来源: zhangrelay.blog.csdn.net，作者：zhangrelay，版权归原作者所有，如需转载，请联系作者。
原文链接：zhangrelay.blog.csdn.net/article/details/116804376
【版权声明】本文为华为云社区用户转载文章，如果您发现本社区中有涉嫌抄袭的内容，欢迎发送邮件进行举报，并提供相关证据，一经查实，本社区将立刻删除涉嫌侵权内容，举报邮箱：
cloudbbs@huaweicloud.com
- 点赞
- 收藏
- 关注作者
评论（0）