Tasks not being executed in Simple Commander Script

Ask
1

Hi guys,

I am trying to set up a list of tasks that the robot is supposed to do at each position of the basic security patrol route.

For some sensor data collection I want the robot to lift its arm to 0.5, then to 1.05. After that the Robot should rotate 4 times by 90°. After each action, the robot should stay and wait in position for 5 seconds.

So for testing this, at first I tried to add any small tasks into the script, but I am facing two issues:
The code for this is:

Summary 
    '# Do something during our route (e.x. AI detection on camera images for anomalies)
    # Simply print ETA for the demonstation
    i = 0
    while not navigator.isTaskComplete():
        i += 1
        feedback = navigator.getFeedback()
        if feedback and i % 3 == 0:
            navigator.get_logger().info('Executing current waypoint: ' +
                str(feedback.current_waypoint + 1) + '/' + str(len(route_poses)))
            now = navigator.get_clock().now()
            print("Arm out")
            robot.arm.move_to(0.3)
            robot.push_command()
            robot.arm.wait_until_at_setpoint()
            time.sleep(5)
            print("Arm in")
            robot.arm.move_to(0.005)
            robot.push_command()
            robot.arm.wait_until_at_setpoint()
            # Some navigation timeout to demo cancellation
            if now - nav_start > Duration(seconds=600.0):
                navigator.cancelTask()

    # If at end of route, reverse the route to restart
    security_route.reverse()`
  1. The error message says, that the robot.lift and the robot.arm are not calibrated
  2. When testing the rotation, the base obviously does not give the error 1). However, the task is not executed, nor is the text printed as I want it.

What is wrong here? I have fed 3 positions into the script, including the origin, hence I changed the condition to i%3 instead of the i%5 from the original tutorial script.

Looking forward to any help here :open_mouth:

Thanks,
Noor

1 Like
2

Hi @roboor,

  1. For the first issue:

You can try homing (calibrating) the robot first using the command stretch_robot_home.py and then start running your Nav2 demo script.

  1. For the second issue:
  • Could you provide more details on how the base velocities are executed by the ros2 controllers using the method robot.base.set_velocity()?
    • Because It looks like you are importing the Robot() class instance from stretch_body directly into your ros2 simple commander demo code. If you are using the BasicNavigator() to interact with Nav2 action servers, Nav2 publishes base velocities to the /cmd_vel topic which is subscribed by the Stretch Driver node that has the Robot() class instance already running and executes the base velocities on the robot. [Note: Only one Robot() class instance can be running at a time]
  • The general approach is to have the ros2 Stretch Driver node running with Nav2 and you use a Joint trajectory action command to position control the robot within your simple commander demo script when a Navigation task is already active. You can refer to the pointed tutorial. @bshah might be able to help you when he is back in office who is more experienced with our ros2 stack.