This is indeed very strange behavior because I have not heard of this until now.
We even had 3 presentations yesterday, 2 of them about this exact project and 1 of them about ROS 2 autonomous navigation (where this delay would become obvious), and everything ran smoothly, which only adds to the mystery of what’s happening here 
.
I am brand new to python and writing code. I am sure that there is nothing wrong with the turtlebot and I’ve done something wrong in my code. My lack of experience means that I don’t know how to approach a problem like this and I rely heavily on this forum and the experience and knowledge of people like you. I apologize for the ignorance of my answers and hope we can resolve this issue.
- Is your internet connection being detected as low by the rosject? This is a red WiFi icon with an exclamation mark that appears in the bottom blue bar.
I can’t comment on all of my connections, but the last one was fine (no red icon).
- Do the joystick or
teleop_twist_keyboard present a similar issue? We have to make sure that what is telling the robot to keep twisting is not the code you are running. In this case, the turtlebot3 only cares about the received /cmd_vel, that’s why I ask.
If I haven’t submitted my wall_finder service the telop_twist_keyboard responds almost immediately. However, if the robot is in the middle of my wall_finder service, the robot doesn’t respond to the input for quite some time. After following @girishkumar.kannan suggestion about monitoring the /cmd_vel in a terminal all on its own, I could see the joystick input show immediately in /cmd_vel, but the robot would not respond to that input. During the wall_finder service, I would notice that the turtlebot wasn’t behaving how I expected it to and would enter “ctrl+c” into the terminal that was running the wall_finder service to stop the service. Turtlebot would continue executing the wall_finder service for 5-10 more seconds. During that time (after canceling the service) I would try to reorient the turtlebot with the joystick. I could see the input immediately with “echo /cmd_vel”, but the turtlebot was still executing the wall_finder service. After a few seconds, the turtlebot would respond to my joystick inputs.
- Does this issue happen to you in every session? Are there other issues regarding the operation of the robot, like the lidar or the odometry, that you noticed?
Yes, it happens every session. I haven’t noticed any issues with /LaserScan or /scan (I’ve forgotten what its called exactly). I haven’t gotten to the odometry readings in the real robot because I never make it past the wall_finder service.
In my initial post I mentioned that it worked one time. Thinking back on that “success,” I think that I got lucky in how I placed the turtlebot initially and it was able to move into the wall following movement even though the wall_finder service didn’t execute accurately. Inside of the wall following movement and odometry action server, things seem to run smoothly. Again, I’ve only seen the odometry run once, and the wall following twice (once for section 1, and the second is detailed above).
- What are the nodes that you are launching? are there any more in addition to the wall finder server? Maybe isolating it from the main wall follower node can help with debugging.
I hope I’m answering this question correctly: I’m launching the WallFinderService node, the OdomActionServer node, and the wall_hugger node (this is the code that calls the finder service, sends a goal to the odom action server, and contains the logic for wall following), totaling 3 nodes. This all works flawlessly in the simulation.
I’m not expecting you to proofread the code below, but for reference, I’ve included all three files and the launch file.
odom_recorder.py
import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node
from rclpy.qos import ReliabilityPolicy, QoSProfile
from odom_interface.action import OdomRecord
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point
from std_msgs.msg import Float64
import time
from rclpy.executors import MultiThreadedExecutor
class OdomActionServer(Node):
def __init__(self):
super().__init__('odom_action_server')
self._action_server = ActionServer(self, OdomRecord, 'record_odom', self.execute_callback)
self.distance_publisher = self.create_publisher(Float64, 'total_distance', 10)
self.subscriber = self.create_subscription(Odometry, '/odom', self.odom_callback, QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE))
self.execute_goal = False
self.stored_init_meas = False
self.first_odom = Point()
self.last_odom = Point()
self.odom_record = []
self.total_distance = 0.0
self.last_x = 0.0
self.last_y = 0.0
self.timer_odom_record = self.create_timer(1.0, self.update_odom_record) # make the update_odom_record update only every 1 second
def odom_callback(self, msg):
if self.stored_init_meas == False and self.execute_goal is True:
self.first_odom.x = msg.pose.pose.position.x
self.first_odom.y = msg.pose.pose.position.y
self.odom_record.append(Point(x=self.first_odom.x, y=self.first_odom.y, z=0.0))
self.stored_init_meas = True
self.last_odom.x = msg.pose.pose.position.x
self.last_odom.y = msg.pose.pose.position.y
def update_odom_record(self):
if self.execute_goal is True:
self.odom_record.append(Point(x=self.last_odom.x, y=self.last_odom.y, z=0.0))
def execute_callback(self, goal_handle):
self.get_logger().info('Executing goal...')
self.execute_goal = True
self.total_distance = 0.0
self.first_odom = Point()
self.last_odom = Point()
self.odom_record = []
self.stored_init_meas = False
#self.timer_publish_feedback = self.create_timer(1.0, lambda: self.publish_feedback()) # make the publish_feedback update only every 1 second
while self.execute_goal is True:
time.sleep(1)
self.publish_feedback()
self.get_logger().info('exited while loop within execute_callback')
goal_handle.succeed()
result = OdomRecord.Result()
result.list_of_odoms = self.odom_record
self.get_logger().info(f'Action succeeded. Total distance traveled: {self.total_distance}')
return result
def publish_feedback(self):
if len(self.odom_record) >= 2:
#self.last_x = self.odom_record[-2].x
#self.last_y = self.odom_record[-2].y
dist_p1_p2 = ((self.odom_record[-1].x - self.odom_record[-2].x) ** 2 + (self.odom_record[-1].y - self.odom_record[-2].y) ** 2) ** 0.5
self.total_distance += dist_p1_p2
#self.get_logger().info(
#f"self.last_x: {self.last_x:.3f},"
#f"self.last_odom.x {self.last_odom.x:.3f},"
#f"self.last_y: {self.last_y:.3f},"
#f"self.last_odom.y {self.last_odom.y:.3f},"
#f"self.total_distance {self.total_distance:.3f},"
#f"dist_p1_p2: {dist_p1_p2:.3f},"
#)
dist_to_start = ((self.odom_record[-1].x - self.first_odom.x) ** 2 + (self.odom_record[-1].y - self.first_odom.y) ** 2) ** 0.5
if dist_to_start < 0.075 and len(self.odom_record) >= 60:
self.get_logger().info('Robot has returned to the initial position.')
self.execute_goal = False
self.stored_init_meas = False
self.get_logger().info(
f"self.odom_record list length: {len(self.odom_record)}, "
f"total_distance: {self.total_distance:.3f}, "
f"dist_to_start: {dist_to_start:.3f}"
)
#self.get_logger().info(
# f"first_odom: {self.first_odom},"
# f"last_odom: {self.last_odom}"
#)
feedback_msg = OdomRecord.Feedback()
feedback_msg.current_total = self.total_distance
self.distance_publisher.publish(Float64(data=self.total_distance))
def main(args=None):
rclpy.init(args=args)
odom_action_server = OdomActionServer()
executor = MultiThreadedExecutor(num_threads=4)
executor.add_node(odom_action_server)
try:
executor.spin()
finally:
executor.shutdown()
odom_action_server.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
wall_finder.py
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from findwall.srv import FindWall
from rclpy.qos import ReliabilityPolicy, QoSProfile
import rclpy
from rclpy.node import Node
import math
from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup
# To launch the simulation:
# source ~/simulation_ws/install/setup.bash
# ros2 launch turtlebot3_gazebo main_turtlebot3_lab.launch.xml
# To control simualted robot:
# ros2 run teleop_twist_keyboard teleop_twist_keyboard
class WallFinderService(Node):
def __init__(self):
super().__init__('find_wall')
self.reentrant_group_1 = ReentrantCallbackGroup()
self.srv = self.create_service(FindWall, 'find_wall', self.custom_service_callback, callback_group=self.reentrant_group_1)
self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
self.subscriber = self.create_subscription(LaserScan, 'scan', self.laser_scan_callback, QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE), callback_group=self.reentrant_group_1)
self.response = FindWall.Response()
self.movement = Twist()
def custom_service_callback(self, request, response):
wall_found = False
aligned = False
approach = False
ready = False
while wall_found == False:
# Calculated Variables
self.front_laser_diff = self.front_laser_10 - self.front_laser_350
self.right_laser_diff = self.right_laser_285 - self.right_laser_255
self.theta_right_wall = math.degrees(math.atan2(self.right_laser_285 - self.right_laser_255*math.cos(math.radians(15)),self.right_laser*math.sin(math.radians(15))))
self.theta_front_wall = math.degrees(math.atan2(self.front_laser_350-self.front_laser*math.cos(math.radians(10)),self.front_laser*math.sin(math.radians(10))))
self.correction_factor_front = abs(self.theta_front_wall)/90
self.correction_factor_right = abs(self.theta_right_wall)/90
self.front_laser_avg = (self.front_laser+self.front_laser_10+self.front_laser_350)/3
self.right_laser_avg = (self.right_laser+self.right_laser_285+self.right_laser_255)/3
# print laser readings
#self.get_logger().info(
#f"Min Laser: {self.laser_min:.3f},"
#f"R Laser {self.right_laser:.3f},"
#)
#self.get_logger().info('Min Range "%s"' % str(self.laser_min), 'Front Range "%s"' % str(self.front_laser))
#self.get_logger().info('Front Range "%s"' % str(self.front_laser))
#self.get_logger().info('Front 10 Range "%s"' % str(self.front_laser_10))
#self.get_logger().info('Front 350 Range "%s"' % str(self.front_laser_350))
#self.get_logger().info('Front Theta "%s"' % str(self.theta_front_wall))
#self.get_logger().info('Wall Located "%s"' % str(self.wall_located))
#self.get_logger().info('Aligned Status "%s"' % str(aligned))
#self.get_logger().info('Approach Status "%s"' % str(approach))
#self.get_logger().info('Ready Status "%s"' % str(ready))
#Correction Factor Chop
if self.correction_factor_front > 0.99:
self.correction_factor_front = 0.99
if self.correction_factor_right > 0.99:
self.correction_factor_right = 0.99
## Move logic
if aligned == False and self.front_laser*0.70 > self.laser_min:
self.movement.angular.z = 0.3
elif aligned == False and self.front_laser*0.90 > self.laser_min:
self.movement.angular.z = self.correction_factor_front*(0.2)
elif aligned == False and self.front_laser*0.90 < self.laser_min:
self.movement.angular.z = 0.0
aligned = True
self.publisher_.publish(self.movement)
self.get_logger().info('Aligned Status "%s"' % str(aligned))
if aligned == True and approach == False and self.front_laser > 0.3:
self.movement.linear.x = 0.05
elif aligned == True and approach == False and self.front_laser <= 0.3:
self.movement.linear.x = 0.0
approach = True
#self.publisher_.publish(self.movement)
self.get_logger().info('Approach Status "%s"' % str(approach))
if approach == True and self.right_laser*0.70 > self.laser_min:
self.movement.angular.z = 0.3
elif approach == True and self.right_laser*0.95 > self.laser_min:
self.movement.angular.z = self.correction_factor_right*(0.2)
elif approach == True:
self.movement.angular.z = 0.0
ready = True
#self.publisher_.publish(self.movement)
self.get_logger().info('Ready Status "%s"' % str(ready))
# Check if ready to shut down
if ready == True:
response.wallfound = ready
self.get_logger().info("Bot in position.")
self.get_logger().info('Ready Status inside motion finder "%s"' % str(ready))
wall_found = True
break
# Publishing the cmd_vel values to a Topic
self.publisher_.publish(self.movement)
#self.response.wallfound = ready
self.get_logger().info('response.wallfound: "%s"' % str(response.wallfound))
return response
def laser_scan_callback(self, msg):
#self.get_logger().info('MULTITHREADING REENTRANT Receiving laser scan')
## Save the Laser readings
# Simulation Laser Ranges
#self.front_laser = msg.ranges[0]
#self.front_laser_10 = msg.ranges[10]
#self.front_laser_350 = msg.ranges[350]
#self.right_laser = msg.ranges[270]
#self.right_laser_285 = msg.ranges[285]
#self.right_laser_255 = msg.ranges[255]
# Real Bot Laser Ranges
self.front_laser = msg.ranges[359]
self.front_laser_10 = msg.ranges[369]
self.front_laser_350 = msg.ranges[349]
self.right_laser = msg.ranges[179]
self.right_laser_285 = msg.ranges[194]
self.right_laser_255 = msg.ranges[164]
self.laser_min = min(msg.ranges)
def main(args=None):
rclpy.init(args=args)
wall_finder_node = WallFinderService()
# Use MultiThreadedExecutor
executor = MultiThreadedExecutor(num_threads=2)
executor.add_node(wall_finder_node)
try:
executor.spin()
finally:
wall_finder_node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
wall_following.py
import rclpy
# import the ROS2 python libraries
from rclpy.node import Node
# import the Twist module from geometry_msgs interface
from geometry_msgs.msg import Twist
# import the LaserScan module from sensor_msgs interface
from sensor_msgs.msg import LaserScan
from rclpy.qos import ReliabilityPolicy, QoSProfile
import math
from findwall.srv import FindWall
from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup
from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor
from rclpy.action import ActionClient
from odom_interface.action import OdomRecord
import time
# To launch the simulation:
# source ~/simulation_ws/install/setup.bash
# ros2 launch turtlebot3_gazebo main_turtlebot3_lab.launch.xml
# To control simualted robot:
# ros2 run teleop_twist_keyboard teleop_twist_keyboard
class wall_hugger(Node):
def __init__(self):
super().__init__('wall_hugger_node')
self.group1 = ReentrantCallbackGroup()
self.group2 = ReentrantCallbackGroup()
self.group3 = ReentrantCallbackGroup()
self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
self.subscriber = self.create_subscription(LaserScan, 'scan', self.Laser_Scan_callback, QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE), callback_group=self.group2)
self.client = self.create_client(FindWall, 'find_wall', callback_group=self.group1)
self._action_client = ActionClient(self, OdomRecord, 'record_odom')
self.req = FindWall.Request()
self.response = FindWall.Response()
# checks once per second if a Service matching the type and name of the client is available.
while not self.client.wait_for_service(timeout_sec=1.0):
# if it is not available, a message is displayed
self.get_logger().info('service not available, waiting again...')
# define the motion variables
self.front_laser = 0.0
self.right_laser = 0.0
self.left_laser = 0.0
self.back_laser = 0.0
self.right_laser_285 = 0.0 #Laser 15 deg CW of the right laser
self.right_laser_255 = 0.0 #laser 15 deg CCW of the right laser
self.right_laser_diff = 0.0 #difference between the lasers on 15 deg tilt to determine robot orientation w.r.t. wall
self.theta_wall = 0.0 #angle between the right wall and the forward motion of the robot. 0 deg means moving // with the wall
self.correction_factor = 0 #turning correction factor
self.right_avg_dist = 0 #average distance from the right wall
# create a Twist message
self.movement = Twist()
self.timer_period = 0.5
self.timer = self.create_timer(self.timer_period, self.begin, callback_group=self.group3)
# create Boolean for synchronous request
self.request_sent = False
self.action_goal_sent = False
# Wall Finder Service Callbacks
def send_request(self):
self.get_logger().info('SENDING REQUEST...')
self.response = self.client.call(self.req)
def begin(self):
# sending synchronous request
if self.request_sent is False:
# making sure that the request will not be called again in the callback function
self.request_sent = True
self.send_request()
elif self.response.wallfound is True:
if self.action_goal_sent is False:
self.get_logger().info('wallfound "%s"' % str(self.response))
self.get_logger().info('Service Complete')
self.send_goal()
self.action_goal_sent = True
#time.sleep(5)
self.motion()
# Action Client Callbacks
def send_goal(self):
goal_msg = OdomRecord.Goal()
# Set any goal parameters if needed
self._action_client.wait_for_server()
self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)
self._send_goal_future.add_done_callback(self.goal_response_callback)
def goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().info('Goal rejected :(')
return
self.get_logger().info('Goal accepted :)')
self._get_result_future = goal_handle.get_result_async()
self._get_result_future.add_done_callback(self.get_result_callback)
def get_result_callback(self, future):
result = future.result().result
self.get_logger().info('Total Distance: {1}'.format(result.current_total))
def feedback_callback(self, feedback_msg):
feedback = feedback_msg.feedback.current_total
self.get_logger().info('Received feedback: Current Total Distance: {0}'.format(feedback))
# Wall Follower Callbacks
def Laser_Scan_callback(self, msg):
# save the Laser readings
# Simulation
#self.front_laser = msg.ranges[0]
#self.right_laser = msg.ranges[270]
#self.left_laser = msg.ranges[90]
#self.back_laser = msg.ranges[180]
#self.right_laser_285 = msg.ranges[285]
#self.right_laser_255 = msg.ranges[255]
# Real Robot
self.front_laser = msg.ranges[359]
self.right_laser = msg.ranges[179]
self.left_laser = msg.ranges[539]
self.back_laser = msg.ranges[0]
self.right_laser_285 = msg.ranges[194]
self.right_laser_255 = msg.ranges[164]
def motion(self):
#if self.service_done: # Check if the service has completed
# print laser readings for debug
#self.get_logger().info('Front Laser "%s"' % str(self.front_laser))
#self.get_logger().info('Right Laser Avg: "%s"' % str(self.right_avg_dist))
#self.get_logger().info('Angle: "%s"' % str(self.right_laser_diff))
#self.get_logger().info('Right 255: "%s"' % str(self.right_laser_255))
#self.get_logger().info('Right 285: "%s"' % str(self.right_laser_285))
#self.get_logger().info('Theta: "%s"' % str(self.theta_wall))
#self.get_logger().info('Correcction: "%s"' % str(self.correction_factor))
#Calculated Variables
self.right_laser_diff = self.right_laser_285 - self.right_laser_255
self.theta_wall = math.degrees(math.atan2(self.right_laser_285 - self.right_laser_255*math.cos(math.radians(15)),self.right_laser*math.sin(math.radians(15))))
self.correction_factor = abs(self.theta_wall)/90
self.right_avg_dist = (self.right_laser+self.right_laser_285+self.right_laser_255)/3
#Correction Factor Chop
if self.correction_factor > 0.99:
self.correction_factor = 0.99
## Move logic
# Front proximity
if self.front_laser < 0.4:
self.movement.angular.z = 0.4
#If angled away from the wall and within sweet spot
elif self.right_laser_diff > 0.00 and self.right_avg_dist < 0.3 and self.right_avg_dist > 0.22:
#self.get_logger().info('Sweet Spot Turning right')
self.movement.linear.x = 0.03
self.movement.angular.z = self.correction_factor*(-0.3)
#If angled towards the wall and within the sweet spot
elif self.right_laser_diff < 0.00 and self.right_avg_dist < 0.3 and self.right_avg_dist > 0.22:
#self.get_logger().info('Sweet Spot Turning left')
self.movement.linear.x = 0.03
self.movement.angular.z = self.correction_factor*(0.3)
#If angled towards the wall and too close to the wall
elif self.right_laser_diff < 0.00 and self.right_laser_285 < 0.22:
self.movement.linear.x = 0.03
self.movement.angular.z = self.correction_factor*(0.4)
#If low agnled away from the wall and too close to the wall
#elif self.right_laser_diff > 0.0 and self.right_laser_diff <= 0.15 and self.right_laser_255 < 0.22:
#self.movement.linear.x = 0.03
#self.movement.angular.z = 0.2
#If angled away from the wall and too close to the wall
elif self.right_laser_diff >= 0.05 and self.right_laser_255 < 0.22:
self.movement.linear.x = 0.03
self.movement.angular.z = self.correction_factor*(0.1)
#If angled away from the wall and too far away from the wall
elif self.right_laser_diff > 0.00 and self.right_laser_255 > 0.3:
self.movement.linear.x = 0.03
self.movement.angular.z = self.correction_factor*(-0.4)
#If angled towards the wall and too far away from the wall
elif self.right_laser_diff < -0.05 and self.right_laser_285 > 0.3:
self.movement.linear.x = 0.03
self.movement.angular.z = self.correction_factor*(-0.1)
else:
self.movement.linear.x = 0.03
self.movement.angular.z = 0.0
# Publishing the cmd_vel values to a Topic
self.publisher_.publish(self.movement)
def main(args=None):
rclpy.init(args=args)
try:
wall_hugger_node = wall_hugger()
executor = MultiThreadedExecutor(num_threads=4)
executor.add_node(wall_hugger_node)
try:
executor.spin()
finally:
executor.shutdown()
wall_hugger_node.destroy_node()
finally:
rclpy.shutdown()
if __name__ == '__main__':
main()
main.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='wall_follower',
executable='wall_finder',
output='screen'),
Node(
package='wall_follower',
executable='wall_following',
output='screen'),
Node(
package='wall_follower',
executable='odom_recorder',
output='screen'),
])
