BAB V HASIL DAN PEMBAHASAN
5.4 Combine Publisher and Subscriber in a Closed Loop System
Turtle node dalam ROS adalah sebuah simulasi perangkat lunak yang mewakili sebuah robot sederhana berbentuk kura-kura.Node ini diimplementasikan dalam paket turtlesim yang disediakan oleh ROS. Tujuan dari turtle node adalah untuk memberikan lingkungan simulasi yang sederhana dan interaktif untuk mengembangkan dan menguji algoritma pengendalian robot.Implementasi combine publisher and subscriber in a closed loop system dengan tujuan untuk menggerakkan turtle node secara otomatis dalam sebuah bidang dimana jika turtle node mendekati batas bidang atau dinding maka turtle node akan melambat dan berputar secara otomatis untuk menghindari tabrakan dengan bidang. Hal ini akan menjadi sebuah sistem keamanan untuk turtle node dalam pergerakan secara otomatis. Tahap implementasinya adalah membuat sebuah node sebagai subscriber dan node lainnya sebagai publisher. Tahap selanjutnya juga membuat node yang lebih kompleks dimana node tersebut sebagai subscriber dan sekaligus publisher untuk topik lainnya.Hal ini yang akan membuat sytem berjalan dalam sebuah close loop system.
5.4.1.1.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.17
Gambar 5.17 Simulasi Turtle Node
Dengan menjalankan instruksi turtlesim_node seperti gambar diatas maka akan menghasilkan beberapa topic yang berisi data dari turtlesim node. Adapun topik-topik 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.18 Topik dari Turtlesim Node
Dari Gambar 5.18 dapat dilihat bahwa muncul beberepa topik setelah menjalanakan
/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.19
Gambar 5.19 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;
}
{
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_);
{
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_);
} }
5.4.2
Turtle Controller
Untuk membuat sistem turtlesim node berjalan dalam sebuah close loop system maka perlu membuat node baru yaitu turtle controller yang berperan sebagai publisher untuk topik /turtle/cmd_vel dan subscriber untuk topik /turtle/pose. Sehinggaa terjadi sebuah close loop system seperti yang ditampilkan Gambar 5.20
Gambar 5.20 Close Loop System
Dengan penambahan node baru turtle controller maka system akan berjalan dalam close system dimana node turtle controller akan berperan sebagai publisher dan sekaligus subscriber untuk topik lainnya seperti yang ditampilkann pada Gambar 5.21. Turtle controler akan menerima informasi mengenai lokasi dari turtle node dari topic /turtle1/pose dan akan
mengontrol pergerakan dari turtle node melalui topic /turtle1/cmd_vel dimana jika data lokasi yang diperoleh dari /turtle1/cmd_vel mendekati batas yang ditentukan maka akan mengontrol turtle node untuk melambat dan berputar untuk menghindari turtle node bergerak melewati batas yang telah ditentukan.
Gambar 5.21 Node Turtle Controller
Turtle controller akan menerima data lokasi turtle node dari topik turtle/pose yang berisi nilai koordinat x,y dari turtle node. Berdasarkan data tersebut turtle controller akan mempublish sebuah intruksi ke topik turtle/cmd_vel untuk mengatur pergerakan dari turtle node. Dengan sistem tersebut makan turtle node akan berjalan secara otomatis dalam sebuah bidang dimana turtle node akan mulai melambat dan berputar ketika mendekati bidang untuk menghindari tabrakan dengan bidang tersebut. Pada gambar 5.18 ditampilkan pergerakan dan lokasi pada koordinat x,y dari turtlesim node .
Gambar 5.22 Pergerakan Turtlesim Node Secara Otomatis
Kode Program Node Turtle Controller
#!/usr/bin/env python3
from geometry_msgs.msg import Twist def pose_callback(pose: Pose):
cmd = Twist()
if pose.x > 9.0 or pose.x < 2.0 or pose.y > 9.0 or pose.y < 2.0:
cmd.linear.x = 1.0 cmd.angular.z = 1.4 else:
cmd.linear.x = 5.0 cmd.angular.z = 0.0 pub.publish(cmd)
if __name__=='__main__':
rospy.init_node("turtle_controller")
pub = rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=10)
sub = rospy.Subscriber("/turtle1/pose", Pose, callback=pose_callback) rospy.loginfo("Node has been started.")
rospy.spin()
Kode Program Node Pose Subscriber
#!/usr/bin/env python3 import rospy
from turtlesim.msg import Pose
def pose_callback(msg: Pose):
rospy.loginfo("(" + str(msg.x) + "," + str(msg.y) +")")
if __name__ == '__main__':
rospy.init_node("turtle_pose_subscriber")
sub = rospy.Subscriber("/turtle1/pose", Pose, callback=pose_callback) rospy.loginfo("Node has been started.")
rospy.spin()
Pada program turtle controller dapat dilihat bahwa node tersebut publisher untuk turtle/cmd/veld an subscriber untuk turtle/pose. Turtle controller mempublish instruksi untuk pergerakan dari turtle node. Instruksi yang dipublish yaitu cmd_linier dan cmd_angular dari turtle node.