-1
$\begingroup$

I am using Turtlebot4Navigation package to navigate the robots through the environment. Turtlebots are set up in a way that they wait for a message on a topic that will have them leave the dock and start navigating. And that worked fine until I started changing parameters in the configuration file for nav2. When the robots stopped working properly I rolled back all the changes but they still don't work. They leave the dock normally but then they either don't continue navigating at all, or navigate to the first pose and stop there. Node also stops all logs when it stops navigating but there is no error, everything just stops. What could be the cause of this.

My code:

import rclpy
#from rclpy.node import Node
from rclpy.action import ActionClient
from rclpy.qos import qos_profile_sensor_data, qos_profile_system_default

from std_msgs.msg import Bool
from action_msgs.msg import GoalStatus
from sensor_msgs.msg import BatteryState
from custom_interfaces.msg import RobotState
from custom_interfaces.srv import CallNext
from turtlebot4_navigation.turtlebot4_navigator import TurtleBot4Directions, TurtleBot4Navigator

from irobot_create_msgs.action import DriveDistance

import time

class nav_node(TurtleBot4Navigator):

    def __init__(self):
        super().__init__()
        self.info(self.get_namespace())
        self.drive_action_client = ActionClient(self,DriveDistance,'drive_distance')
        self.drive_result_future = None
        self.battery_charge = None
        self.create_subscription(BatteryState,'battery_state',self.battery_state_callback,qos_profile=qos_profile_sensor_data)
        self.my_state_pub = self.create_publisher(RobotState,'robot_state',qos_profile_system_default)
        self.create_subscription(Bool,'launch_robot',self.run_robot_callback,qos_profile_system_default)
        self.create_poses()
        self.create_timer(5.0,self.pub_robot_state)
        self.timer_replace=self.create_timer(180,self.timer_callback)
        self.running = False
        return
    

    def create_poses(self):
        '''loads appropriate poses for the robot'''
        poses_robots = [self.getPoseStamped([0.53, 2.93], TurtleBot4Directions.NORTH_WEST),
                        self.getPoseStamped([-1.37, -0.67], TurtleBot4Directions.SOUTH_EAST),
                        self.getPoseStamped([-2.66, -0.71], TurtleBot4Directions.SOUTH_WEST),
                        self.getPoseStamped([-2.59, 0.55], TurtleBot4Directions.NORTH_WEST)]

        dock_poses = {'/robot6' : self.getPoseStamped([-3.27, -0.65], TurtleBot4Directions.SOUTH_EAST),
                    '/robot7' : self.getPoseStamped([-2.54, -1.26], TurtleBot4Directions.SOUTH_EAST),
                    '/robot8' : self.getPoseStamped([-0.29, -0.01],TurtleBot4Directions.NORTH),
                    '/robot9' : self.getPoseStamped([-0.25, 0.73],TurtleBot4Directions.NORTH)
                    }
        
        # extract navigation poses and dock pose for robot currently in use
        self.poses = poses_robots
        self.dock_pose = dock_poses[self.get_namespace()]
        return
        


    def run_robot_callback(self,msg):
        '''callback function for launch_robot topic'''
        self.running = msg.data
        self.info('RUN ROBOT CALLBACK: {}'.format(msg.data))
        return


    def undock(self):
        super().undock()
        self.info('undockal sam se')
        time.sleep(1)
        self.drive_forward(0.2)
        return
    
    def drive_forward(self,dist):
        '''
        perform DriveDistance action driving forvard for dist
        
        in:
            dist: distance for robot to drive
        '''
        self.info('Driving for {}'.format(dist))
        self.drive_send_goal(dist)

        while not self.isDriveComplete():
            time.sleep(0.1)
        return
    

    def drive_send_goal(self,dist):
        goal_msg = DriveDistance.Goal(distance = dist)
        self.drive_action_client.wait_for_server()
        goal_future = self.drive_action_client.send_goal_async(goal_msg)

        rclpy.spin_until_future_complete(self, goal_future)

        self.drive_goal_handle = goal_future.result()

        if not self.drive_goal_handle.accepted:
            self.error('DriveDistance goal rejected')
            return

        self.undock_result_future = self.drive_goal_handle.get_result_async()
        return
    

    def isDriveComplete(self):
        """
        Get status of DriveDistance action.

        :return: ``True`` if finished, ``False`` otherwise.
        """
        if self.drive_result_future is None or not self.dock_result_future:
            return True

        rclpy.spin_until_future_complete(self, self.drive_result_future, timeout_sec=0.1)

        if self.drive_result_future.result():
            self.drive_status = self.drive_result_future.result().status
            if self.drive_status != GoalStatus.STATUS_SUCCEEDED:
                self.info(f'Goal failed with status code: {self.status}')
                return True
        else:
            return False

        self.info('Undock succeeded')
        return True
    
    def battery_state_callback(self,msg):
        '''callback function for battery state topic'''
        self.battery_charge = msg.percentage
        self.info('Battery at: {:.2f}%'.format(self.battery_charge*100))
        return
    
    def pub_robot_state(self):
        '''timer callback function for periodic publishing of robot_state topic'''
        msg = RobotState()
        #self.info(self.get_namespace().strip('/'))
        msg.robot_name = self.get_namespace().strip('/')
        #self.info(str(self.battery_charge))
        #self.info(str(self.getDockedStatus()))
        if self.battery_charge == None:
            return
        msg.docked_status = self.getDockedStatus()
        msg.battery_charge = self.battery_charge
        #msg.request_replacement = self.call_next
        self.my_state_pub.publish(msg)
        self.info('Publishing robot state')
        self.info('Battery at: {:.2f}%'.format(self.battery_charge*100))
        return
    
    def call_next_robot(self):
        self.get_logger().info('Calling next robot')
        while not self.my_call_next_cli.service_is_ready():
            self.get_logger().info('Waiting for master service')
            rclpy.spin_once(self,timeout_sec=0.2)
        future = self.my_call_next_cli.call_async(CallNext.Request())
        rclpy.spin_until_future_complete(self,future)
        self.get_logger().info(str(future.result()))
        return future.result()
    
    def timer_callback(self):
        self.running = False

    

def main():
    rclpy.init()
    navigator = nav_node()
    navigator.info('Navigator node started')

    flag = True

    if not navigator.getDockedStatus():
        navigator.dock()

    while 1:
        if navigator.running:
            
            if flag:
                # set innitial guess of the robot pose for localization
                navigator.setInitialPose(navigator.dock_pose)
                navigator.info('init pose set')

                navigator.undock()
                navigator.info('unocked')



                # Wait for Nav2
                navigator.waitUntilNav2Active()
                navigator.info('nav2 ready')
                flag = False
                navigator.timer_replace.reset()

            #time.sleep(1)
            for pose in navigator.poses:
                navigator.startToPose(pose)
            #time.sleep(1)
            if not navigator.running or navigator.battery_charge < 0.25:
                navigator.startToPose(navigator.dock_pose)
                navigator.dock()
                navigator.running = False
                flag = True
        else:
            rclpy.spin_once(navigator,timeout_sec=5)


if __name__ == '__main__':
    main()
```
$\endgroup$
3
  • $\begingroup$ You claim the behaviour is due to parameter changes, please provide them. $\endgroup$
    – Scoeerg
    Commented Jun 15 at 13:33
  • $\begingroup$ @Scoeerg well i removed all changes and it didn't fix it, so I would guess that's not an issue. $\endgroup$
    – zdaga
    Commented Jun 17 at 11:25
  • $\begingroup$ Please provide terminal commands and outputs from every terminal that you use to run the system. Although you might not see an error, seeing the last command that came through can be helpful or sometimes the error isn't the last output that you see. Also check journalctl for any output from a service that may have failed. Sometimes the feedback is shown in the log but not in the terminal. $\endgroup$
    – Hilary Luo
    Commented Jun 18 at 16:18

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.