ROS Basics in 5 Days: Course Project 3.1 how to stop the robot after a lap?

[min.wang.1997.01.13]

Dear community,

• When the robot has done a complete lap, it has to finish and return the list of odometry recorded

How can we stop the robot? Where should define it?
For easier debugging, I uploaded the package to a github repo, here is the link.

Once finishing a lap, I break the goal_callback loop in the action server, define is_goal_successful method in the action client class, and stop the robot in the odom_monitor_thread method in the wall_follower.py.
However, it still cannot be stopped.

Another question regarding the action section:
I separate the moving logic and client/server logic, use the main thread for the wall_follow movement and the 2nd thread for monitoring the feedback from the record odom action server, is that the correct way to do the task?

Thanks a lot for your help!

Here are relevant functions:

def goal_callback(self, goal):
        rospy.loginfo(f"Action server {self.name} has been called")
        
        self._recorder.start_recording()
        success = True

        while not rospy.is_shutdown():
            if self._as.is_preempt_requested():
                rospy.loginfo(f"Action server {self.name} is preempted")
                self._as.set_preempted()
                success = False
                self._recorder.stop_recording()
                return

            self._feedback.current_total = self._recorder.get_total_distance()
            self._as.publish_feedback(self._feedback)

            # Check if the robot has completed a lap
            if self._recorder.has_completed_lap():
                rospy.loginfo("Lap completed, need to stop the robot")
                break

            self.rate.sleep()

    def is_goal_successful(self):

        state_result = self.client.get_state()

        rate = rospy.Rate(10)

        if state_result == GoalStatus.SUCCEEDED:
            rospy.loginfo("Goal is completed successfully")
            return True
        else:    
            return False

    def odom_monitor_thread(self, action_client, wall_follower):
        rate = rospy.Rate(1)
        while not rospy.is_shutdown():
            if action_client.is_goal_successful():
                rospy.loginfo("Goal has been completed for a lap, stop the robot...")
                wall_follower.stop()
                break
            rate.sleep()

        result = action_client.check_goal_status()
        rospy.loginfo(f"Recording finished with result: {result}")

[aameralshamy3]

@min.wang.1997.01.13

action_client

To stop the robot, set the angular and linear speed = 0 at the /cmd_vel topic

please share the project description

[roalgoal]

Hi @min.wang.1997.01.13 ,

There are many ways to stop the robot, and what you’ve mentioned, stopping after receiving a successful response from the action server, is probably the best way to do it in the scope of this project.

If done correctly, it can surely be stopped. You just need to make sure that another part of your program isn’t left publishing velocities.

The action server should have a callback that is called when a client sends a goal. In this callback, you can monitor the odometry and then simply return a result that can be used by another node, like your main wall follower node. A common structure for a server like this would be:

#! /usr/bin/env python
import rospy
import actionlib
from nav_msgs.msg import Odometry
from project1.msg import OdomRecordFeedback, OdomRecordResult, OdomRecordAction


class RecordClass(object):

    def __init__(self):
        # creates the action server
        self._as = actionlib.SimpleActionServer(
            "/record_odom", OdomRecordAction, self.goal_callback, False
        )

    def sub_callback(self, msg):
        self.m = msg

    def goal_callback(self, goal):
        r = rospy.Rate(1)
        self._sub = rospy.Subscriber("/odom", Odometry, self.sub_callback)

        while not rospy.is_shutdown():
            r.sleep()
            #### Implement action server logic here, use sub_callback msg as needed
            self._as.set_succeeded(self._result)
            break


if __name__ == "__main__":
    rospy.init_node("record_odom_node")
    RecordClass()
    rospy.spin()

Note that there is no velocity publishing. It’s a good practice to separate who does what. Otherwise, you will get issues exactly as you are describing.

[min.wang.1997.01.13]

@roalgoal

no velocity publishing. It’s a good practice to separate who does what. Otherwise, you will get issues exactly as you are describing.

Thank you very much!
I have fixed the issue in the simulation. It is because I forgot to initialize the starting position in odom_recoder.py.
Everything works okay in simulation but when I connected to the real robot, the distance to the starting point is very strange (it would not change that frequently)

Here is the odom server’s output in real robot:

Why it could be 1.75 for many times and then 1.34 for many times? It seems that something wrong with odom reading for the real robot. What can I do here?
In the simulation, it will change and reduce until it is close to 0.1 and then stop the robot.