Going to Specific Location using Neural Network in ROS

Viewed 34

i am developing a program that can be used to move the turtlebot to specific location using neural network with lidar sensors.

in this program i have some problem, can move the turtlebot and avoid the obstacle, but they cannot go to specific location that i want.

i dont know where the problem is? theres no problem when compile and run this program (if u talking about the software i used) iam sure that the problem is in my code

import rospy
import random
import numpy as np
import tensorflow as tf
from geometry_msgs.msg import Twist, Point
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Path
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseStamped
from math import atan2
from tf.transformations import euler_from_quaternion

x = 0.0
y = 0.0
theta = 0.0

laser_range = np.array([])

def newOdom(msg):
    global x
    global y
    global theta

    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y

    rot_q = msg.pose.pose.orientation
    (roll,pitch,theta) = euler_from_quaternion ([rot_q.x,rot_q.y,rot_q.z,rot_q.w])

def scan_callback(msg):
    global laser_range
    
    laser_range = np.expand_dims(np.array(msg.ranges[:30] + msg.ranges[-30:]), axis = 0)
    laser_range[laser_range == np.inf] = 3.5

if __name__ == "__main__":

    scan_sub = rospy.Subscriber('scan', LaserScan, scan_callback)
    move = rospy.Publisher('cmd_vel', Twist, queue_size = 1)
    sub = rospy.Subscriber('/odom',Odometry, newOdom)
  
    rospy.init_node('obstacle_avoidance_tf')

    state_change_time = rospy.Time.now()
    rate = rospy.Rate(10)
    goal = Point()
    goal.x = 1
    goal.y = 1  

    model = tf.keras.models.load_model('catkin_ws/src/turtlebot3_neural_network/src/model.hdf5')

    while not rospy.is_shutdown():
        inc_x = goal.x - x
        inc_y = goal.y - y

        angle_to_goal = atan2(inc_y,inc_x)
        
        predictions = model.predict(laser_range)
        action = np.argmax(predictions)
        
        twist = Twist()
        if action == 0 and abs(angle_to_goal - theta) > 0.1:
            twist.linear.x = 0.3
        else:
            twist.angular.z = 0.3
        move.publish(twist)
        print(x)
        print(y)

        rate.sleep()
    
0 Answers
Related