在学习这一节前,需简要复习一下前面3节内容:
这3节,基本都是在终端看到交互形式,输出字符(1),运算结果(2)或斐波那契数列(3)。
本节开启图形化之旅,这种人机交互模式更容易被接受和使用,当然消耗资源也更大。
先从简单的二维环境仿真入手,逐渐过渡到三维物理引擎仿真,再过渡到可穿戴嵌入式设备、真实机器人以及虚拟现实设备。
环境配置:
本文使用部分环境变量如下:
先检查turtlesim:
q
启动turtlesim:
弹出窗口如下:
图形化操作界面rqt:
rqt --force-discover
更改一下:
通过调整滑杆,控制小乌龟移动。
下面通过修改源代码作一些改变,首先是图标,改成小机器人robot.png:
在turtle_frame.cpp中找到如下代码:
QVector<QString> turtles;
turtles.append("ardent.png");
turtles.append("bouncy.png");
turtles.append("crystal.png");
turtles.append("dashing.png");
turtles.append("eloquent.png");
turtles.append("foxy.png");
请用注释,然后修改成如下:
//change robot.png
turtles.append("robot.png");
// turtles.append("ardent.png");
// turtles.append("bouncy.png");
// turtles.append("crystal.png");
// turtles.append("dashing.png");
// turtles.append("eloquent.png");
// turtles.append("foxy.png");
看到如下Q萌的小机器人了吧。
然后再进一步修改如下代码:
#define DEFAULT_BG_B 0x00
//调整窗口大小并测试中文
setFixedSize(888, 666);
setWindowTitle("机器人二维仿真");
效果如下:
学会阅读和修改源码是非常重要的^_^ 大部分使用ROSCPP_INFO输出的字符都是支持中文的。
再修改一下,如下:
RCLCPP_INFO(nh_->get_logger(), "Starting turtlesim with node name %s", nh_->get_node_names()[0].c_str());
调整为:
// RCLCPP_INFO(nh_->get_logger(), "Starting turtlesim with node name %s", nh_->get_node_names()[0].c_str());
RCLCPP_INFO(nh_->get_logger(), "开启二维机器人仿真节点 %s", nh_->get_node_names()[0].c_str());
是不是发现完成一个机器人仿真程序人机交互的修改,其实还是蛮简单的^_^
使用图形化界面,在环境中添加更多的小机器人:
注意左下角:
注意不要重名!
当然也可以改变画笔的颜色:
遥控重定位:
使用 Ctrl + C 关闭对应节点。关于机器人节点、主题、服务、参数和行动的更多内容,后续补充。
turtlesim.cpp
#include <QApplication>
#include <rclcpp/rclcpp.hpp>
#include "turtlesim/turtle_frame.h"
class TurtleApp : public QApplication
{
public:
rclcpp::Node::SharedPtr nh_;
explicit TurtleApp(int& argc, char** argv)
: QApplication(argc, argv)
{
rclcpp::init(argc, argv);
nh_ = rclcpp::Node::make_shared("turtlesim");
}
~TurtleApp()
{
rclcpp::shutdown();
}
int exec()
{
turtlesim::TurtleFrame frame(nh_);
frame.show();
return QApplication::exec();
}
};
int main(int argc, char** argv)
{
TurtleApp app(argc, argv);
return app.exec();
}
turtle_frame.cpp
#include "turtlesim/turtle_frame.h"
#include <QPointF>
#include <cstdlib>
#include <ctime>
#define DEFAULT_BG_R 0x45
#define DEFAULT_BG_G 0x56
#define DEFAULT_BG_B 0x00
namespace turtlesim
{
TurtleFrame::TurtleFrame(rclcpp::Node::SharedPtr& node_handle, QWidget* parent, Qt::WindowFlags f)
: QFrame(parent, f)
, path_image_(500, 500, QImage::Format_ARGB32)
, path_painter_(&path_image_)
, frame_count_(0)
, id_counter_(0)
{
// setFixedSize(500, 500);
// setWindowTitle("TurtleSim");
//调整窗口大小并测试中文
setFixedSize(888, 666);
setWindowTitle("机器人二维仿真");
srand(time(NULL));
update_timer_ = new QTimer(this);
update_timer_->setInterval(16);
update_timer_->start();
connect(update_timer_, SIGNAL(timeout()), this, SLOT(onUpdate()));
nh_ = node_handle;
rcl_interfaces::msg::IntegerRange range;
range.from_value = 0;
range.step = 1;
range.to_value = 255;
rcl_interfaces::msg::ParameterDescriptor background_r_descriptor;
background_r_descriptor.description = "Red channel of the background color";
background_r_descriptor.integer_range.push_back(range);
rcl_interfaces::msg::ParameterDescriptor background_g_descriptor;
background_g_descriptor.description = "Green channel of the background color";
background_g_descriptor.integer_range.push_back(range);
rcl_interfaces::msg::ParameterDescriptor background_b_descriptor;
background_b_descriptor.description = "Blue channel of the background color";
background_b_descriptor.integer_range.push_back(range);
nh_->declare_parameter("background_r", rclcpp::ParameterValue(DEFAULT_BG_R), background_r_descriptor);
nh_->declare_parameter("background_g", rclcpp::ParameterValue(DEFAULT_BG_G), background_g_descriptor);
nh_->declare_parameter("background_b", rclcpp::ParameterValue(DEFAULT_BG_B), background_b_descriptor);
QVector<QString> turtles;
//change robot.png
turtles.append("robot.png");
// turtles.append("ardent.png");
// turtles.append("bouncy.png");
// turtles.append("crystal.png");
// turtles.append("dashing.png");
// turtles.append("eloquent.png");
// turtles.append("foxy.png");
QString images_path = (ament_index_cpp::get_package_share_directory("turtlesim") + "/images/").c_str();
for (int i = 0; i < turtles.size(); ++i)
{
QImage img;
img.load(images_path + turtles[i]);
turtle_images_.append(img);
}
meter_ = turtle_images_[0].height();
clear();
clear_srv_ = nh_->create_service<std_srvs::srv::Empty>("clear", std::bind(&TurtleFrame::clearCallback, this, std::placeholders::_1, std::placeholders::_2));
reset_srv_ = nh_->create_service<std_srvs::srv::Empty>("reset", std::bind(&TurtleFrame::resetCallback, this, std::placeholders::_1, std::placeholders::_2));
spawn_srv_ = nh_->create_service<turtlesim::srv::Spawn>("spawn", std::bind(&TurtleFrame::spawnCallback, this, std::placeholders::_1, std::placeholders::_2));
kill_srv_ = nh_->create_service<turtlesim::srv::Kill>("kill", std::bind(&TurtleFrame::killCallback, this, std::placeholders::_1, std::placeholders::_2));
rclcpp::QoS qos(rclcpp::KeepLast(100), rmw_qos_profile_sensor_data);
parameter_event_sub_ = nh_->create_subscription<rcl_interfaces::msg::ParameterEvent>(
"/parameter_events", qos, std::bind(&TurtleFrame::parameterEventCallback, this, std::placeholders::_1));
// RCLCPP_INFO(nh_->get_logger(), "Starting turtlesim with node name %s", nh_->get_node_names()[0].c_str());
RCLCPP_INFO(nh_->get_logger(), "开启二维机器人仿真节点 %s", nh_->get_node_names()[0].c_str());
width_in_meters_ = (width() - 1) / meter_;
height_in_meters_ = (height() - 1) / meter_;
spawnTurtle("", width_in_meters_ / 2.0, height_in_meters_ / 2.0, 0);
// spawn all available turtle types
if(false)
{
for(int index = 0; index < turtles.size(); ++index)
{
QString name = turtles[index];
name = name.split(".").first();
name.replace(QString("-"), QString(""));
spawnTurtle(name.toStdString(), 1.0f + 1.5f * (index % 7), 1.0f + 1.5f * (index / 7), static_cast<float>(PI) / 2.0f, index);
}
}
}
TurtleFrame::~TurtleFrame()
{
delete update_timer_;
}
bool TurtleFrame::spawnCallback(const turtlesim::srv::Spawn::Request::SharedPtr req, turtlesim::srv::Spawn::Response::SharedPtr res)
{
std::string name = spawnTurtle(req->name, req->x, req->y, req->theta);
if (name.empty())
{
RCLCPP_ERROR(nh_->get_logger(), "A turtle named [%s] already exists", req->name.c_str());
return false;
}
res->name = name;
return true;
}
bool TurtleFrame::killCallback(const turtlesim::srv::Kill::Request::SharedPtr req, turtlesim::srv::Kill::Response::SharedPtr)
{
M_Turtle::iterator it = turtles_.find(req->name);
if (it == turtles_.end())
{
RCLCPP_ERROR(nh_->get_logger(), "Tried to kill turtle [%s], which does not exist", req->name.c_str());
return false;
}
turtles_.erase(it);
update();
return true;
}
void TurtleFrame::parameterEventCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
{
// only consider events from this node
if (event->node == nh_->get_fully_qualified_name())
{
// since parameter events for this even aren't expected frequently just always call update()
update();
}
}
bool TurtleFrame::hasTurtle(const std::string& name)
{
return turtles_.find(name) != turtles_.end();
}
std::string TurtleFrame::spawnTurtle(const std::string& name, float x, float y, float angle)
{
return spawnTurtle(name, x, y, angle, rand() % turtle_images_.size());
}
std::string TurtleFrame::spawnTurtle(const std::string& name, float x, float y, float angle, size_t index)
{
std::string real_name = name;
if (real_name.empty())
{
do
{
std::stringstream ss;
ss << "turtle" << ++id_counter_;
real_name = ss.str();
} while (hasTurtle(real_name));
}
else
{
if (hasTurtle(real_name))
{
return "";
}
}
TurtlePtr t = std::make_shared<Turtle>(nh_, real_name, turtle_images_[static_cast<int>(index)], QPointF(x, height_in_meters_ - y), angle);
turtles_[real_name] = t;
update();
RCLCPP_INFO(nh_->get_logger(), "Spawning turtle [%s] at x=[%f], y=[%f], theta=[%f]", real_name.c_str(), x, y, angle);
return real_name;
}
void TurtleFrame::clear()
{
// make all pixels fully transparent
path_image_.fill(qRgba(255, 255, 255, 0));
update();
}
void TurtleFrame::onUpdate()
{
if (!rclcpp::ok())
{
close();
return;
}
rclcpp::spin_some(nh_);
updateTurtles();
}
void TurtleFrame::paintEvent(QPaintEvent*)
{
QPainter painter(this);
int r = DEFAULT_BG_R;
int g = DEFAULT_BG_G;
int b = DEFAULT_BG_B;
nh_->get_parameter("background_r", r);
nh_->get_parameter("background_g", g);
nh_->get_parameter("background_b", b);
QRgb background_color = qRgb(r, g, b);
painter.fillRect(0, 0, width(), height(), background_color);
painter.drawImage(QPoint(0, 0), path_image_);
M_Turtle::iterator it = turtles_.begin();
M_Turtle::iterator end = turtles_.end();
for (; it != end; ++it)
{
it->second->paint(painter);
}
}
void TurtleFrame::updateTurtles()
{
if (last_turtle_update_.nanoseconds() == 0)
{
last_turtle_update_ = nh_->now();
return;
}
bool modified = false;
M_Turtle::iterator it = turtles_.begin();
M_Turtle::iterator end = turtles_.end();
for (; it != end; ++it)
{
modified |= it->second->update(0.001 * update_timer_->interval(), path_painter_, path_image_, width_in_meters_, height_in_meters_);
}
if (modified)
{
update();
}
++frame_count_;
}
bool TurtleFrame::clearCallback(const std_srvs::srv::Empty::Request::SharedPtr, std_srvs::srv::Empty::Response::SharedPtr)
{
RCLCPP_INFO(nh_->get_logger(), "Clearing turtlesim.");
clear();
return true;
}
bool TurtleFrame::resetCallback(const std_srvs::srv::Empty::Request::SharedPtr, std_srvs::srv::Empty::Response::SharedPtr)
{
RCLCPP_INFO(nh_->get_logger(), "Resetting turtlesim.");
turtles_.clear();
id_counter_ = 0;
spawnTurtle("", width_in_meters_ / 2.0, height_in_meters_ / 2.0, 0);
clear();
return true;
}
}
turtle.cpp
#include "turtlesim/turtle.h"
#include <math.h>
#include <QColor>
#include <QRgb>
#define DEFAULT_PEN_R 0xb3
#define DEFAULT_PEN_G 0xb8
#define DEFAULT_PEN_B 0xff
namespace turtlesim
{
static float normalizeAngle(float angle)
{
return angle - (TWO_PI * std::floor((angle + PI) / (TWO_PI)));
}
Turtle::Turtle(rclcpp::Node::SharedPtr& nh, const std::string& real_name, const QImage& turtle_image, const QPointF& pos, float orient)
: nh_(nh)
, turtle_image_(turtle_image)
, pos_(pos)
, orient_(orient)
, lin_vel_x_(0.0)
, lin_vel_y_(0.0)
, ang_vel_(0.0)
, pen_on_(true)
, pen_(QColor(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))
{
pen_.setWidth(3);
rclcpp::QoS qos(rclcpp::KeepLast(7));
velocity_sub_ = nh_->create_subscription<geometry_msgs::msg::Twist>(real_name + "/cmd_vel", qos, std::bind(&Turtle::velocityCallback, this, std::placeholders::_1));
pose_pub_ = nh_->create_publisher<turtlesim::msg::Pose>(real_name + "/pose", qos);
color_pub_ = nh_->create_publisher<turtlesim::msg::Color>(real_name + "/color_sensor", qos);
set_pen_srv_ = nh_->create_service<turtlesim::srv::SetPen>(real_name + "/set_pen", std::bind(&Turtle::setPenCallback, this, std::placeholders::_1, std::placeholders::_2));
teleport_relative_srv_ = nh_->create_service<turtlesim::srv::TeleportRelative>(real_name + "/teleport_relative", std::bind(&Turtle::teleportRelativeCallback, this, std::placeholders::_1, std::placeholders::_2));
teleport_absolute_srv_ = nh_->create_service<turtlesim::srv::TeleportAbsolute>(real_name + "/teleport_absolute", std::bind(&Turtle::teleportAbsoluteCallback, this, std::placeholders::_1, std::placeholders::_2));
rotate_absolute_action_server_ = rclcpp_action::create_server<turtlesim::action::RotateAbsolute>(
nh,
real_name + "/rotate_absolute",
[](const rclcpp_action::GoalUUID &, std::shared_ptr<const turtlesim::action::RotateAbsolute::Goal>)
{
// Accept all goals
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
},
[](const std::shared_ptr<RotateAbsoluteGoalHandle>)
{
// Accept all cancel requests
return rclcpp_action::CancelResponse::ACCEPT;
},
std::bind(&Turtle::rotateAbsoluteAcceptCallback, this, std::placeholders::_1));
last_command_time_ = nh_->now();
meter_ = turtle_image_.height();
rotateImage();
}
void Turtle::velocityCallback(const geometry_msgs::msg::Twist::SharedPtr vel)
{
last_command_time_ = nh_->now();
lin_vel_x_ = vel->linear.x;
lin_vel_y_ = vel->linear.y;
ang_vel_ = vel->angular.z;
// Abort any active action
if (rotate_absolute_goal_handle_)
{
RCLCPP_WARN(nh_->get_logger(), "Velocity command received during rotation goal. Aborting goal");
rotate_absolute_goal_handle_->abort(rotate_absolute_result_);
rotate_absolute_goal_handle_ = nullptr;
}
}
bool Turtle::setPenCallback(const turtlesim::srv::SetPen::Request::SharedPtr req, turtlesim::srv::SetPen::Response::SharedPtr)
{
pen_on_ = !req->off;
if (req->off)
{
return true;
}
QPen pen(QColor(req->r, req->g, req->b));
if (req->width != 0)
{
pen.setWidth(req->width);
}
pen_ = pen;
return true;
}
bool Turtle::teleportRelativeCallback(const turtlesim::srv::TeleportRelative::Request::SharedPtr req, turtlesim::srv::TeleportRelative::Response::SharedPtr)
{
teleport_requests_.push_back(TeleportRequest(0, 0, req->angular, req->linear, true));
return true;
}
bool Turtle::teleportAbsoluteCallback(const turtlesim::srv::TeleportAbsolute::Request::SharedPtr req, turtlesim::srv::TeleportAbsolute::Response::SharedPtr)
{
teleport_requests_.push_back(TeleportRequest(req->x, req->y, req->theta, 0, false));
return true;
}
void Turtle::rotateAbsoluteAcceptCallback(const std::shared_ptr<RotateAbsoluteGoalHandle> goal_handle)
{
// Abort any existing goal
if (rotate_absolute_goal_handle_)
{
RCLCPP_WARN(nh_->get_logger(), "Rotation goal received before a previous goal finished. Aborting previous goal");
rotate_absolute_goal_handle_->abort(rotate_absolute_result_);
}
rotate_absolute_goal_handle_ = goal_handle;
rotate_absolute_feedback_.reset(new turtlesim::action::RotateAbsolute::Feedback);
rotate_absolute_result_.reset(new turtlesim::action::RotateAbsolute::Result);
rotate_absolute_start_orient_ = orient_;
}
void Turtle::rotateImage()
{
QTransform transform;
transform.rotate(-orient_ * 180.0 / PI + 90.0);
turtle_rotated_image_ = turtle_image_.transformed(transform);
}
bool Turtle::update(double dt, QPainter& path_painter, const QImage& path_image, qreal canvas_width, qreal canvas_height)
{
bool modified = false;
qreal old_orient = orient_;
// first process any teleportation requests, in order
V_TeleportRequest::iterator it = teleport_requests_.begin();
V_TeleportRequest::iterator end = teleport_requests_.end();
for (; it != end; ++it)
{
const TeleportRequest& req = *it;
QPointF old_pos = pos_;
if (req.relative)
{
orient_ += req.theta;
pos_.rx() += std::cos(orient_) * req.linear;
pos_.ry() += - std::sin(orient_) * req.linear;
}
else
{
pos_.setX(req.pos.x());
pos_.setY(std::max(0.0, static_cast<double>(canvas_height - req.pos.y())));
orient_ = req.theta;
}
if (pen_on_)
{
path_painter.setPen(pen_);
path_painter.drawLine(pos_ * meter_, old_pos * meter_);
}
modified = true;
}
teleport_requests_.clear();
// Process any action requests
if (rotate_absolute_goal_handle_)
{
// Check if there was a cancel request
if (rotate_absolute_goal_handle_->is_canceling())
{
RCLCPP_INFO(nh_->get_logger(), "Rotation goal canceled");
rotate_absolute_goal_handle_->canceled(rotate_absolute_result_);
rotate_absolute_goal_handle_ = nullptr;
lin_vel_x_ = 0.0;
lin_vel_y_ = 0.0;
ang_vel_ = 0.0;
}
else
{
float theta = normalizeAngle(rotate_absolute_goal_handle_->get_goal()->theta);
float remaining = normalizeAngle(theta - static_cast<float>(orient_));
// Update result
rotate_absolute_result_->delta = normalizeAngle(static_cast<float>(rotate_absolute_start_orient_ - orient_));
// Update feedback
rotate_absolute_feedback_->remaining = remaining;
rotate_absolute_goal_handle_->publish_feedback(rotate_absolute_feedback_);
// Check stopping condition
if (fabs(normalizeAngle(static_cast<float>(orient_) - theta)) < 0.02)
{
RCLCPP_INFO(nh_->get_logger(), "Rotation goal completed successfully");
rotate_absolute_goal_handle_->succeed(rotate_absolute_result_);
rotate_absolute_goal_handle_ = nullptr;
lin_vel_x_ = 0.0;
lin_vel_y_ = 0.0;
ang_vel_ = 0.0;
}
else
{
lin_vel_x_ = 0.0;
lin_vel_y_ = 0.0;
ang_vel_ = remaining < 0.0 ? -1.0 : 1.0;
last_command_time_ = nh_->now();
}
}
}
if (nh_->now() - last_command_time_ > rclcpp::Duration(1.0, 0))
{
lin_vel_x_ = 0.0;
lin_vel_y_ = 0.0;
ang_vel_ = 0.0;
}
QPointF old_pos = pos_;
orient_ = orient_ + ang_vel_ * dt;
// Keep orient_ between -pi and +pi
orient_ = normalizeAngle(orient_);
pos_.rx() += std::cos(orient_) * lin_vel_x_ * dt
- std::sin(orient_) * lin_vel_y_ * dt;
pos_.ry() -= std::cos(orient_) * lin_vel_y_ * dt
+ std::sin(orient_) * lin_vel_x_ * dt;
// Clamp to screen size
if (pos_.x() < 0 || pos_.x() > canvas_width ||
pos_.y() < 0 || pos_.y() > canvas_height)
{
RCLCPP_WARN(nh_->get_logger(), "Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos_.x(), pos_.y());
}
pos_.setX(std::min(std::max(static_cast<double>(pos_.x()), 0.0), static_cast<double>(canvas_width)));
pos_.setY(std::min(std::max(static_cast<double>(pos_.y()), 0.0), static_cast<double>(canvas_height)));
// Publish pose of the turtle
auto p = std::make_unique<turtlesim::msg::Pose>();
p->x = pos_.x();
p->y = canvas_height - pos_.y();
p->theta = orient_;
p->linear_velocity = std::sqrt(lin_vel_x_ * lin_vel_x_ + lin_vel_y_ * lin_vel_y_);
p->angular_velocity = ang_vel_;
pose_pub_->publish(std::move(p));
// Figure out (and publish) the color underneath the turtle
{
auto color = std::make_unique<turtlesim::msg::Color>();
QRgb pixel = path_image.pixel((pos_ * meter_).toPoint());
color->r = qRed(pixel);
color->g = qGreen(pixel);
color->b = qBlue(pixel);
color_pub_->publish(std::move(color));
}
RCLCPP_DEBUG(nh_->get_logger(), "[%s]: pos_x: %f pos_y: %f theta: %f", nh_->get_namespace(), pos_.x(), pos_.y(), orient_);
if (orient_ != old_orient)
{
rotateImage();
modified = true;
}
if (pos_ != old_pos)
{
if (pen_on_)
{
path_painter.setPen(pen_);
path_painter.drawLine(pos_ * meter_, old_pos * meter_);
}
modified = true;
}
return modified;
}
void Turtle::paint(QPainter& painter)
{
QPointF p = pos_ * meter_;
p.rx() -= 0.5 * turtle_rotated_image_.width();
p.ry() -= 0.5 * turtle_rotated_image_.height();
painter.drawImage(p, turtle_rotated_image_);
}
}
-^_^-