BAB V HASIL DAN PEMBAHASAN
5.4 Combine Publisher and Subscriber in a Closed Loop System
5.4.1 Simulasi Turtle Node
Untuk menjalankan simulasi turtle node pada platform ROS menggunakan instruksi $ roscore pada terminal ubuntu. Instruksi roscore berperan sebagai pusat komunikasi utama dalam lingkungan Robot Operating System (ROS). Ketika dijalankan, roscore memulai "master node" yang mengatur aliran informasi antara semua node dalam sistem. Master node mengelola parameter, layanan, dan memungkinkan node untuk mendaftar diri serta menemukan node lain.
Ini juga memfasilitasi publikasi dan langganan topik untuk pertukaran pesan antar node.
Dengan roscore, komunikasi terstruktur diaktifkan, memungkinkan node-node ROS berinteraksi, berbagi data, dan beroperasi bersama secara terkoordinasi. Pastikan untuk menjaga terminal roscore tetap terbuka agar komunikasi tetap berjalan. Instruksi ini adalah langkah kunci dalam menjalankan sistem berbasis ROS. Selanjutnya adalah menjalakan paket turtlesim_node menggunakan instruksi rosrun turtlesim turtlesim_node pada terminal ubuntu untuk memunculkan node dari turtlesim seperti yang ditampilkan pada gambar 5.1
Gambar 5.14 Simulasi Turtle Node
Dengan menjalankan instruksi turtlesim_node seperti gambar diatas maka akan menghasilkan beberapa topic yang berisi data dari turtlesim node. Adapun topik-topic yang dihasilkan yaitu /turtle1/cmd_vel, /turtle1/color_sensor, /turtle1/pose. Topik /turtle1/cmd_vel berisi data untuk mengatur pergerakan dari turtle node yaitu kecepatan dan pergerakan turtle node secara linier dan angular. Topik ini yang akan digunakan untuk memberikan instruksi ke turtle node yang akan bergerak dalam sebuah bidang. Topik /turtle1/pose berisi data lokasi dari turtle node dalam koordinat x,y yang digunakan sebagai inputan untuk menentukan pergerakan dari turtle node. Dimana jika turtle node mendekati bidang atau batasan yang ditentukan pada koordinat x,y maka turtle node akan mulai melambat dan berputar untuk menghindari batasan yang ditentukan. Dalam tahap ini hanya menggunakan kedua topik terebut untuk mengatur pergerakan dari turtle node.
Gambar 5.15 Topik dari Turtlesim Node
Dari Gambar 5.14 dapat dilihat bahwa muncul beberepa topik setelah menjalanakan paket dari turtlesim node. Dimana /turtesim berperan sebagai publishers untuk topik /turtle1/pose dan sekaligus sebagai subscriber untuk topik /turtle/cmd_vel. Pada tahap ini turltel node belum berjalan secara otomatis dikarenakan system yang dibuat belum berjalan pada close loop system seperti yang ditampilkan Gambar 5.15
Gambar 5.16 System Turtlesim Node
Kode Program Turtlesim Node
#include "turtlesim/turtle.h"
#include <QColor>
#include <QRgb>
#define DEFAULT_PEN_R 0xb3
#define DEFAULT_PEN_G 0xb8
#define DEFAULT_PEN_B 0xff
namespace turtlesim {
Turtle::Turtle(const ros::NodeHandle& nh, 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);
velocity_sub_ = nh_.subscribe("cmd_vel", 1, &Turtle::velocityCallback, this);
pose_pub_ = nh_.advertise<Pose>("pose", 1);
color_pub_ = nh_.advertise<Color>("color_sensor", 1);
set_pen_srv_ = nh_.advertiseService("set_pen", &Turtle::setPenCallback, this);
teleport_relative_srv_ = nh_.advertiseService("teleport_relative",
&Turtle::teleportRelativeCallback, this);
teleport_absolute_srv_ = nh_.advertiseService("teleport_absolute",
&Turtle::teleportAbsoluteCallback, this);
meter_ = turtle_image_.height();
rotateImage();
}
void Turtle::velocityCallback(const geometry_msgs::Twist::ConstPtr& vel) {
last_command_time_ = ros::WallTime::now();
lin_vel_x_ = vel->linear.x;
lin_vel_y_ = vel->linear.y;
ang_vel_ = vel->angular.z;
}
bool Turtle::setPenCallback(turtlesim::SetPen::Request& req, turtlesim::SetPen::Response&)
{
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(turtlesim::TeleportRelative::Request& req, turtlesim::TeleportRelative::Response&)
{
teleport_requests_.push_back(TeleportRequest(0, 0, req.angular, req.linear, true));
return true;
}
bool Turtle::teleportAbsoluteCallback(turtlesim::TeleportAbsolute::Request& req, turtlesim::TeleportAbsolute::Response&)
{
teleport_requests_.push_back(TeleportRequest(req.x, req.y, req.theta, 0, false));
return true;
}
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();
if (ros::WallTime::now() - last_command_time_ > ros::WallDuration(1.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_ -= 2*PI * std::floor((orient_ + PI)/(2*PI));
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) {
ROS_WARN("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 Pose p;
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(p);
// Figure out (and publish) the color underneath the turtle {
Color color;
QRgb pixel = path_image.pixel((pos_ * meter_).toPoint());
color.r = qRed(pixel);
color.g = qGreen(pixel);
color.b = qBlue(pixel);
color_pub_.publish(color);
}
ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f", nh_.getNamespace().c_str(), 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_);
} }