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()