BAB V HASIL DAN PEMBAHASAN
5.6 Robot Otonom
Gambar 5.29 Diagram Sistem ROS
Gambar 5.30 Subscriber Test Indoor GPS Marvelmind
Ketika data lokasi mendekati batas area kerja yang telah ditetapkan, subscriber test memberikan instruksi kepada perangkat Arduino yang mengontrol robot. Instruksi yang dikirimkan ke Arduino bergantung pada posisi relatif robot terhadap batas area kerja. Jika robot mendekati batas dan ada risiko keluar dari area kerja, Arduino akan menginstruksikan robot untuk berputar secara otomatis agar tetap berada dalam batasan area tersebut. Namun, jika robot masih bergerak di dalam batas area kerja, Arduino akan menerima instruksi maju dari subscriber test, memungkinkan robot untuk melanjutkan pergerakan di dalam area tersebut. Pada tahap ini subscriber_test berfungsi sebagai node subscriber untuk Indoor GPS Marvelmind yang menerima data mengenai beacon mobile dan juga berfungsi sebagai node publisher untuk Arduino yang akan mengirimkan instruksi ke Arduino yang mengontrol pergerakan dari robot sehingga robot akan bergerak pada batasan-batasan yang telah ditentukan. Dengan cara ini, robot otonom ini dapat beroperasi dengan aman dan efisien dalam batasan area kerja yang telah ditentukan.
Kode Program Subscriber_test Indoor GPS Marvelmind
##!/usr/bin/env python3 import rospy
from std_msgs.msg import String, Int32 # Import the Int32 message type from marvelmind_nav.msg import hedge_pos_ang
from visualization_msgs.msg import Marker
# Constants for states STATE_IDLE = 0
STATE_TURN_RIGHT = 1 STATE_MOVE_FORWARD = 2
# Global variables state = STATE_IDLE prev_time = 0
def rviz_object(address, x, y, z, obj):
if rviz_marker_pub.get_num_connections() < 1:
return
marker = Marker()
marker.header.frame_id = "/my_frame"
marker.header.stamp = rospy.Time.now() marker.ns = "basic_shapes"
marker.id = address marker.type = rviz_shape marker.action = Marker.ADD marker.pose.position.x = x marker.pose.position.y = y marker.pose.position.z = z marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 marker.scale.x = 0.05 * SCALE_HEDGE marker.scale.y = 0.05 * SCALE_HEDGE marker.scale.z = 0.02 * SCALE_HEDGE
if obj == "hedge":
marker.color.r = 0.0 marker.color.g = 0.0 marker.color.b = 1.0 else:
marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0
marker.color.a = 1.0
if obj == "hedge":
lifetime = rospy.Duration(5) else:
lifetime = rospy.Duration(25)
marker.lifetime = lifetime rviz_marker_pub.publish(marker)
def hedge_pos_ang_callback(hedge_pos_msg):
global state, prev_time rospy.loginfo(
"Hedgehog data: Address= %d, timestamp= %d, X=%.3f Y= %.3f Z=%.3f Angle: %.1f flags=%d",
hedge_pos_msg.address, hedge_pos_msg.timestamp_ms, hedge_pos_msg.x_m,
hedge_pos_msg.y_m, hedge_pos_msg.z_m,
hedge_pos_msg.angle, hedge_pos_msg.flags, )
current_time = rospy.get_time() if state == STATE_IDLE:
# Determine the values to send to Arduino based on x_m and y_m if hedge_pos_msg.x_m <= 0 or hedge_pos_msg.x_m >= 8 or
hedge_pos_msg.y_m <= 0 or hedge_pos_msg.y_m >= 8:
# Send turn right command (4) to Arduino control_msg = Int32()
control_msg.data = 4
turtle_control_pub.publish(control_msg) state = STATE_TURN_RIGHT
prev_time = current_time else:
# Send move forward command (1) to Arduino control_msg = Int32()
control_msg.data = 1
turtle_control_pub.publish(control_msg)
elif state == STATE_TURN_RIGHT and current_time - prev_time >= 5.0:
# Send move forward command (1) to Arduino control_msg = Int32()
control_msg.data = 1
turtle_control_pub.publish(control_msg) state = STATE_MOVE_FORWARD
prev_time = current_time
elif state == STATE_MOVE_FORWARD and current_time - prev_time >= 5.0:
# Reset the state back to IDLE so that it can receive x_m and y_m again
state = STATE_IDLE
def send_forward_command(event):
# Send forward motion command (1) to Arduino control_msg = Int32()
control_msg.data = 1
turtle_control_pub.publish(control_msg) def beacon_pos_callback(beacon_pos_msg):
rospy.loginfo(
"Stationary beacon data: Address= %d, X=%.3f Y=%.3f Z=%.3f", beacon_pos_msg.address,
beacon_pos_msg.x_m, beacon_pos_msg.y_m, beacon_pos_msg.z_m,
)
rviz_object(beacon_pos_msg.address, beacon_pos_msg.x_m, beacon_pos_msg.y_m, beacon_pos_msg.z_m, "beacon")
def imu_raw_callback(hedge_imu_raw_msg):
rospy.loginfo(
"Raw IMU: Timestamp: %08d, aX=%05d aY=%05d aZ=%05d gX=%05d gY=%05d gZ=%05d cX=%05d cY=%05d cZ=%05d",
hedge_imu_raw_msg.timestamp_ms, hedge_imu_raw_msg.acc_x,
hedge_imu_raw_msg.acc_y, hedge_imu_raw_msg.acc_z, hedge_imu_raw_msg.gyro_x, hedge_imu_raw_msg.gyro_y, hedge_imu_raw_msg.gyro_z, hedge_imu_raw_msg.compass_x, hedge_imu_raw_msg.compass_y, hedge_imu_raw_msg.compass_z, )
def imu_fusion_callback(hedge_imu_fusion_msg):
rospy.loginfo(
"IMU fusion: Timestamp: %08d, X=%.3f Y=%.3f Z=%.3f q=%.3f,%.3f,
%.3f,%.3f v=%.3f,%.3f,%.3f a=%.3f,%.3f,%.3f", hedge_imu_fusion_msg.timestamp_ms, hedge_imu_fusion_msg.x_m,
hedge_imu_fusion_msg.y_m, hedge_imu_fusion_msg.z_m, hedge_imu_fusion_msg.qw, hedge_imu_fusion_msg.qx, hedge_imu_fusion_msg.qy, hedge_imu_fusion_msg.qz, hedge_imu_fusion_msg.vx, hedge_imu_fusion_msg.vy, hedge_imu_fusion_msg.vz, hedge_imu_fusion_msg.ax, hedge_imu_fusion_msg.ay, hedge_imu_fusion_msg.az, )
global orientation_qx, orientation_qy, orientation_qz, orientation_qw orientation_qx = hedge_imu_fusion_msg.qx
orientation_qy = hedge_imu_fusion_msg.qy orientation_qz = hedge_imu_fusion_msg.qz orientation_qw = hedge_imu_fusion_msg.qw
def raw_distance_callback(beacon_raw_distance_msg):
rospy.loginfo(
"Raw distance: %02d ==> %02d, Distance= %.3f",
beacon_raw_distance_msg.address_hedge, beacon_raw_distance_msg.address_beacon, beacon_raw_distance_msg.distance_m, )
def telemetry_callback(hedge_telemetry_msg):
rospy.loginfo("Vbat= %.3f V, RSSI= %02d",
hedge_telemetry_msg.battery_voltage, hedge_telemetry_msg.rssi_dbm) def quality_callback(hedge_quality_msg):
rospy.loginfo("Quality: Address= %d, Quality= %02d %%", hedge_quality_msg.address, hedge_quality_msg.quality_percents) def waypoint_callback(marvelmind_waypoint_msg):
n = marvelmind_waypoint_msg.item_index + 1 rospy.loginfo(
"Waypoint %03d/%03d: Type= %03d, Param1= %05d, Param2= %05d, Param3= %05d",
n,
marvelmind_waypoint_msg.total_items, marvelmind_waypoint_msg.movement_type, marvelmind_waypoint_msg.param1,
marvelmind_waypoint_msg.param2, marvelmind_waypoint_msg.param3, )
if __name__ == "__main__":
rospy.init_node("subscriber_test", anonymous=True)
# Initialize the publisher for the "turtle_control" topic turtle_control_pub = rospy.Publisher("turtle_control", Int32, queue_size=10)
rospy.Subscriber("/hedge_pos_ang", hedge_pos_ang, hedge_pos_ang_callback)
rviz_marker_pub = rospy.Publisher("visualization_marker", Marker, queue_size=1)
rviz_shape = Marker.CUBE SCALE_HEDGE = 3.0
rospy.spin()