Page tree
Skip to end of metadata
Go to start of metadata

Introduction

The purpose of this project is to use a single robot that will autonomously search a predefined area for RFID tags that simulate mines. At the same time, a second robot cooperating with the first one, will head directly to the pose of the RFID detection point in order to neutralize the mine. The move_base package is used in order to cooperate effectively in a narrow area.


Sources:   https://gitlab.nps.edu/t.tsatsanifos.gr/mrc_final_project

                


Robot Environment

  • Two Turtlebot3 robots
    • "Searcher" robot with ThingMagic USBPro RFID scanner
      • Performs lawnmower search for RFID tags, where tags represent mines.
      • Designated search area: 7.5 meters2 seen below. The red star is the map origin.
    • "Neutralizer" robot
      • Travels to each RFID detection point
      • Avoids obstacles and collisions with "searcher". This is implemented by publishing navigational goals in the move_base_simple/goal.



Simulink Model Description


We subscribe to tb3_1/amcl_pose node in order to obtain real time information concerning the pose of the Searcher robot(tb3_1). This is vital, since /rfid node will give us only the time of the detection. This information should be correlated in order to localize the position of the rfid tag that will be fed to the second robot as a way point goal.

We drive the tb3_1 through specific way points, that take into account the effective range of the robots sensors, in order to ensure that the whole area of operation is effectively searched. If this was to be implemented in Search and Reacquire at sea, additional factors like current should be taken into account.

Tb3_1 aforementioned motion is implemented through MATLAB code that controls the linear and angular velocity, published through a Twist geometry message to the tb3_1/cmd_vel node. The advantage of this type of search is that our code is simpler and we control the speed of the search, which is proportional to the distance of the next waypoint. However, the disadvantage of publishing twist messages to cmd/vel is the fact that our navigation does not take into account the obstacles that are detected in our search area through the robot' s sensors. Since we are searching an area where the natural obstacles are known from our g-mapping procedure and since the navigation of the second robot does takes into account dynamic obstacles, we have excluded the possibility that our robots collide with each other.

The way we implement the real time re-acquire of the tags (simulating mines) is to pass the pose command through a block that is triggered if and only if we have a new rfid detection message. The challenge a beginner ROS developer has to face when trying to publish information in /tb3_0/move_base_simple/goal is the fact that we have to publish a PoseStamped geometry message and a Header std message. The PoseStamped message consists of a Pose geometry message, which is published directly from the amcl/pose node with no additional effort. However, the Header std message  has to be published through a MATLAB function that will assign a string to the header. Further information concerning this subject could be found in the Troubleshooting section below.

Setup

These steps assume two Turtlebots are setup and configured for remote ROS communication. Follow the steps in sections TurtleBot 3 Setup and Remote ROS Communication for setup and configuration.

  1. Base Station: Start the ROS master

    roscore
  2. Turtlebot0 (neutralizer):
    1. Place the robot at the coordinates that your launch file defines.
    2. Check to make sure that the Turtlebot time is similar to the laptop time. If not, follow the instructions Raspberry Pi Sync Time With Time Machine to setup your Turtlebot to automatically synchronize time with the local time server.
    3. Connect to the robot via SSH

      ssh frl@192.168.11.113
    4. Start the Turtlebot nodes with namespace and tf_prefix "tb3_0"

      ROS_NAMESPACE=tb3_0 roslaunch turtlebot3_bringup turtlebot3_robot.launch multi_robot_name:="tb3_0" set_lidar_frame_id:="tb3_0/base_scan"
  3. Turtlebot1 (searcher):
    1. Place the robot at the coordinates that your launch file defines.
    2. Check to make sure that the Turtlebot time is similar to the laptop time. If not, setup your Turtlebot to automatically synchronize time with the local time server.
    3. Connect to the robot via SSH

      ssh frl@192.168.11.118
    4. Start the Turtlebot nodes with namespace and tf_prefix "tb3_1"

      ROS_NAMESPACE=tb3_1 roslaunch turtlebot3_bringup turtlebot3_robot.launch multi_robot_name:="tb3_1" set_lidar_frame_id:="tb3_1/base_scan"
    5. Launch the thing_reader_node for RFID detection

      roslaunch thingmagic_rfid reader.launch port:=/dev/ttyACM1

      For instructions on how to integrate the ThingMagic RFID reader with the Turtlebot3 platform, follow the link: ThingMagic USB Pro RFID Reader, Turtlebot3 Integration

  4. Base Station:
    1. Start the move_base node for both robots

      roslaunch turtlebot3_nps move_base_two.launch

      Turtlebot1 (searcher) does not require the move_base node. The following command will start a move_base node for Turtlebot0 only:

      roslaunch turtlebot3_nps move_base_namespace.launch robot_namespace:=tb3_0

      The rosnode graph and tf_tree presented below assume the move_base_two launch file is used.

    2. Launch the base station nodes for navigation

      roslaunch turtlebot3_nps navigation_two.launch map_file:=${HOME}/map.yaml tb3_1_init_x:=1.0

      The navigation_two.launch file does the following:

      1. Takes optional command line arguments for the initial pose of each robot and the map config file location.

      2. Starts a single map_server node to provide the map for localization
      3. For each turtlebot (tb3_0 and tb3_1), starts nodes with the appropriate namespace and tf_prefix
        1. robot_state_publisher node
        2. amcl node
        3. joy and teleop_twist_joy nodes configured for ports /dev/js0 and /dev/js1

          Two joysticks may be used to control the robot. This function is optional.

      4. Starts RVIZ



Results

If everything goes as planned, your robot trajectories should look like this:


Your rosnode graph should look like this:


and your tf_tree like this:


Mine Waypoint Management

One major issue with the current configuration is managing RFID waypoints. As soon as a new RFID is detected, that location is sent to the neutralizer, preempting all previous waypoints. In order to ensure all mines are reached, the waypoints must be stored and managed. This was attempted using a goal queue as a persistent variable under the Waypoint Management block in the following Simulink model.


There were some limitations with Simulink, embedded MATLAB code and published messages that we needed to work around.

  • move_base_simple/goal: Ideally, we would be able to subscribe to Turtlebot0's own goal management by subscribing to /tb3_0/move_base/status. The GoalStatusArray displays the status of 16 goals. However, because we are publishing to /tb3_0/move_base_simple/goal, the status array is not saved. For future work, publishing to /tb3_0/move_base/goal will make managing waypoints though move_base possible. The work around was to subscribe to /tb3_0/move_base/goal (current goal position) and /tb3_0/amcl_pose (current AMCL position of Turtlebot0). The distance between these two positions will tell us if the goal is achieved.
  • Dynamic arrays: The original plan was to create a goal queue using a dynamic array in an embedded MATLAB function. However, embedded MATLAB functions do not allow dynamic arrays. To work around this, a static array of 20 goals is used. The number of goals and current goal are tracked along with the goal queue as persistent variables.
  • Trigger logic: A trigger logic block kept Simulink from calling the embedded MATLAB function every timestep, and overrunning the goal array. If the distance from Turtlebot0's position to its current goal and less than the threshold, or if a new RFID tag is detected, the MATLAB function activates. Below is the MATLAB function.

    function goal_cmd  = goal_fcn(current_goal,waypoint,dist)
    % Function to implement waypoint guidance
    % Inputs:
    %  - current_goal: current goal from move_base [X_pose Y_pose]
    %  - waypoint: Current mine detection [X_pose Y_pose]
    %  - dist: distance from curent pose to current goal
    % Outputs:
    %  - goal_cmd: current goal of neutralizer [X_pose Y_pose]
    
    coder.extrinsic('exist')    % Bypass code generation
    persistent goal_queue num_goal goal_offset
    
    % Initialize goal queue
    if isempty(goal_queue)
        goal_queue = [waypoint'; zeros(20,2)];
        num_goal = 1;   
        goal_offset = 0;  
    end
    
    if dist <= 0.15 && all(current_goal)
        % Delete achieved goals from beginning of queue
        goal_offset = goal_offset + 1;
        num_goal = num_goal - 1;
    end
    if ~any(goal_queue(:,1)==waypoint(1))
        % Add new goal to queue
        num_goal = num_goal + 1;
        goal_queue(num_goal+goal_offset,:) = waypoint';
    end
    
    % Assign current goal
    goal_cmd = goal_queue(num_goal+goal_offset,:);

    This function initializes the goal queue and bookkeeping variables that keep track of the number of goals (num_goals) and number of achieved goals (goal_offset). If the distance between Turtlebot0's position and the current goal position is less than 0.15m and the current goal is not zero, move on to the next goal. If there are any new goals not currently in the queue, add it to the queue.  Finally, assign the goal using the bookkeeping variables.

Unfortunately, this attempt was unsuccessful and achieves that same behavior, ignoring any previous goals. To simplify the problem, it is suggested to separate the two events (new RFID and achieved goal). This can be done by creating a separate node for managing waypoints, making the goal queue a global variable, reading/writing gloals to a file, etc.


Conclusion/Future Work

As can be seen from the video, the search a reacquire is successful. The searcher robot successfully performed a lawnmower search of the area and detected RFID tags. The neutralizer robot then moved to the positions of the RFID detections. There are some shortfalls to this implementation, however. The Monte-Carlo localization of the neutralizer robot was not accurate enough for the first RFID detection. Giving this robot some movement to help localize itself may alleviate this problem.

Ultimately, we found that move_base, although it has many built in features, does not work easily with Simulink. Creating similar behaviors through Twist messages may be more user friendly. For future work, waypoint management should be implemented. Suggestions include using the move_base/goal message, creating a separate node, or managing a queue by reading and writing to a file.



Troubleshooting

Move_base package

The move_base package is a useful tool in order to accomplish navigational goals. The move_base node links together a global and local planner to accomplish its global navigation task.


Remark 1: Quaternion vector close to zero. Navigational goal is rejected.

When you want to publish the Pose.Orientation of the tb3_1/amcl node that we are subscribed to, in the tb3_0/move_base_simple/goal node, you will receive the error message shown in Remark1 in the move_base terminal and your waypoint goal will be rejected.

Solution:  Pre-define a quaternion  goal that corresponds to a yaw target of 90 degrees.

Remark 2: Introducing new navigational plans every second that make the robot constantly try to reach the waypoint goal

By default the goal tolerance parameters are very small. As a result the message goal reached will hardly ever reach the global and local planners, that will constantly compute new trajectory in order to reach the desired location and avoid collision with the cooperating robots.

Solution: Increase the goal tolerance parameters for DWAPlannerRos concerning xy position and yaw angle.

Remark 3: Publishing a move_base/simple_goal message needs a Header Reference ID to be transmitted via a String Message.

Since Simulink does not allow you to pass the ACML/POSE/HEADER message directly through a ROS encode block, you have to implement a new assignSTRING function box in the Simulink block diagram. However, as soon as your Simulink model has a different name from the developer 's assignSTRING function box, you will obtain an error from the Simulink compiler because the output message of this box is not recognized.

Solution: Edit the type of the output message of the assignSTRING bus in accordance with the name of your Simulink model.


Stopping the Turtlebots

Turtlebot0 receives commands through the move_base node. Its actions can be cancelled by the following command

rostopic pub -1 /tb3_0/move_base/cancel actionlib_msgs/GoalID -- {}

Turtlebot1 receives commands through a Twist message. It can be stopped by sending the following Twist message

rostopic pub -1 /tb3_1/cmd_vel geometry_msgs/Twist -- '[0.0, 0.0, 0.0]' '[0.0, 0.0, 0.0]'
  • No labels

1 Comment

    • For the "Results" section it isn't quite obvious what Simulink model you are running to generate the behavior you show. 
    • Good description of the issue working with the ROS topic of /tb3_0/move_base_simple/goal  - it would be ideal to have Simulink connect to the move_base action server.
    • Good documentation of some of the troubleshooting steps.  This is always important to help folks reproduce your work.