User

import rclpy

import math

import random

import array

from rclpy$.$node import Node

from geometry$\_$msgs$.$msg import Twist

from nav$\_$msgs$.$msg import Odometry

from sensor$\_$msgs$.$msg import LaserScan

class MyRobot$($Node$)$:

def $\_\_$init$\_\_($self$)$:

super$().\_\_$init$\_\_(\text{'}$myrobot$\_$node'$)$

qos$\_$policy $=$ rclpy$.$qos.QoSProfile$($

reliability$=$rclpy$.$qos.ReliabilityPolicy.BEST$\_$EFFORT,

history$=$rclpy$.$qos.HistoryPolicy.KEEP$\_$LAST, depth$=1)$

self.sub$1=$ self.create$\_$subscription$($LaserScan$,$ 'scan', self.scan$\_$callback, qos$\_$profile$=$qos$\_$policy$)$

self.sub$2=$ self.create$\_$subscription$($

Odometry,

'odom',

self.odom$\_$callback,

qos$\_$profile$=$qos$\_$policy

$)$

self.pub $=$ self.create$\_$publisher$($Twist$,\text{'}$cmd$\_$vel', $10)$

self.cmd$\_$vel$\_$pub $=$ self.create$\_$publisher$($Twist$,\text{'}$cmd$\_$vel', $10)$ # Added missing publisher

timer$\_$period $=0\mathrm{.}1$ # seconds

self.timer $=$ self.create$\_$timer$($timer$\_$period, self.timer$\_$callback$)$

self.my$\_$x $=0$

self.my$\_$y $=0$

self.my$\_$angle $=0$

self.my$\_$ls $=$ LaserScan$()$

self.my$\_$ls$.$ranges $=$ array.array$(\text{'}$f$\text{'},[0\mathrm{.}0]*360)$

self.my$\_$position $=$ "forward"

self.target$\_$angle $=0$

def scan$\_$callback$($self$,$ msg$)$:

self.my$\_$ls$.$ranges $=$ msg$.$ranges

# print $(\text{'}$l $=\{2$:$5\mathrm{.}2$f$\},$ f $=\{1$:$5\mathrm{.}2$f$\},$ r $=\{0$:$5\mathrm{.}2$f$\}\text{'}.$format$($self$.$my$\_$ls$.$ranges$[15],$ self.my$\_$ls$.$ranges$[0],$ self.my$\_$ls$.$ranges$[345]))$

def odom$\_$callback$($self$,$ msg$)$:

self.my$\_$x $=$ msg$.$pose.pose.position.x

self.my$\_$y $=$ msg$.$pose.pose.position.y

# convert quaternion to Euler angles

q$0=$ msg$.$pose.pose.orientation.w

q$1=$ msg$.$pose.pose.orientation.x

q$2=$ msg$.$pose.pose.orientation.y

q$3=$ msg$.$pose.pose.orientation.z

self.my$\_$angle $=$ math.atan$2(2*($q$0*$ q$3+$ q$1*$ q$2),(1-2*($q$2*$ q$2+$ q$3*$ q$3)))*180/$ math.pi

# print$(\text{'}$x $=\{0$:$5\mathrm{.}2$f$\},$ y $=\{1$:$5\mathrm{.}2$f$\},$ th $=\{2$:$5\mathrm{.}2$f$\}\text{'}.$format$($self$.$my$\_$x$,$ self.my$\_$y$,$ self.my$\_$angle$))$

self.save$\_$odometry$\_$data$()$

def turn$($self$,$ target$\_$angle$)$:

move $=$ Twist$()$

if self.my$\_$angle $<$ target$\_$angle:

move.angular.z $=0\mathrm{.}1$

else:

move.angular.z $=0\mathrm{.}0$

return True # Signal that rotation is complete

return False

def timer$\_$callback$($self$)$:

move $=$ Twist$()$

# Implement obstacle avoidance logic here based on laser scan data

# For example, if an obstacle is too close, stop the robot

if $($self$.$my$\_$ls$.$ranges$[0]<0\mathrm{.}5$ or self.my$\_$ls$.$ranges$[15]<0\mathrm{.}5$ or self.my$\_$ls$.$ranges$[345]<0\mathrm{.}5)$ and self.my$\_$position $==$ "forward":

self.my$\_$position $=$ "left" # random.choice$([\text{'}$left$\text{'},$ 'right'$])$

if self.my$\_$position $==$ "left":

self.target$\_$angle $=$ self.my$\_$angle $-90$

if self.my$\_$position $==$ "right":

self.target$\_$angle $=$ self.my$\_$angle $+45$

elif self.my$\_$ls$.$ranges$[0]>0\mathrm{.}5$:

self.my$\_$position $=$ "forward"

print$($self$.$my$\_$position$)$

if self.my$\_$position $==$ 'forward':

move.linear.x $=0\mathrm{.}1$ # Speed to be adjust

move.angular.z $=0\mathrm{.}0$

if self.my$\_$position $==$ 'left':

move.linear.x $=0\mathrm{.}0$

move.angular.z $=0\mathrm{.}5$ # The turning angle to be adjust

if self.my$\_$angle $>$ self.target$\_$angle:

self.my$\_$position $==$ "forward"

elif self.my$\_$position $==$ 'right':

move.linear.x $=0\mathrm{.}0$

move.angular.z $=-0\mathrm{.}5$ # The turning angle to be adjust

if self.my$\_$angle $<$ self.target$\_$angle:

self.my$\_$position $==$ "forward"

self.cmd$\_$vel$\_$pub.publish$($move$)$

def save$\_$odometry$\_$data$($self$)$:

with open$("$src$/$ce$215\_$pkg$/$ce$215\_$pkg$/$myodom$.$txt$","$a$")$ as myfile:

myfile.write$($f$"\{$self$.$my$\_$x$\},\{$self$.$my$\_$y$\},\{$self$.$my$\_$angle$\}$

$")$

def main$($args$=$None$)$:

rclpy$.$init$($args$=$args$)$

myrobot$\_$node $=$ MyRobot$()$

rclpy$.$spin$($myrobot$\_$node$)$

myrobot$\_$node.destroy$\_$node$()$

rclpy$.$shutdown$()$

if $\_\_$name$\_\_==\text{'}\_\_$main$\_\_\text{'}$:

main$()$ Explain the code shown above in full description for each part of the command. This code is used for moving the turtlebot$3$ burger robot in gazebo simulation without hitting obstacles to prevent from crashing. Needs to cover the whole map from the Rviz simulation.