Custom Control

This tutorial describes how to use /subt/pose_from_artifact_origin so the robot can find the tunnel entrance quickly. You can use this example as a reference to get started with your own solution to submit for the SubT Virtual Challenge. The SubT Simulator API documentation lists all ROS services and topics that you can use to develop your solution.

Note: The SubT virtual testbed uses Ignition for simulation and ROS for robot control. ROS is your entry point, and Ignition should be treated as a separate application that your solution will interact with, as described in the Cloudsim architecture.

1. In Terminal 1, launch the SubT Simulation

   a. Make sure you have the SubT Simulator software installed either in a Docker container (recommended) or as a Catkin workspace.

   b. For SubT Docker image, launch it using the command below (RECOMMENDED):

    ./run.bash osrf/subt-virtual-testbed competition.ign worldName:=simple_cave_01 circuit:=cave robotName1:=X1 robotConfig1:=X1_SENSOR_CONFIG_1
   

   After launching it, you should see the simulation start with a single robot in the staging area.

2. In Terminal 2, create a catkin workspace, clone the subt_seed repository, and build the solution

    mkdir -p ~/subt_solution/src && cd ~/subt_solution/src
   
    git clone https://github.com/osrf/subt_seed
   
    cd ~/subt_solution
   
    source /opt/ros/melodic/setup.bash
   
    source ~/subt_ws/install/setup.bash
   
    catkin_make install
   
    cd ~/subt_solution
   
    source install/setup.bash
   
    roslaunch subt_seed x1.launch robot_name:=X1
   

   You should see the robot start driving towards the entrance. The movement happens in 4 phases:

       1. Rotate so that robot's line of sight is perpendicular to the straight line that crosses the entrance
       2. Go forward until hitting the line
       3. Rotate until facing the entrance
       4. Go forward until reaching the entrance

Here is what happens in the subt_seed_node.cc example file.

 In the **main()** function, we initialize ROS, create our controller, and start an update loop.

    // Initialize ros
    ros::init(argc, argv, argv[1]);

    ROS_INFO("Starting seed competitor\n");

    // Create the controller
    Controller controller(argv[1]);

    // This sample code iteratively calls Controller::Update. This is just an
    // example. You can write your controller using alternative methods.
    // To get started with ROS visit: http://wiki.ros.org/ROS/Tutorials
    ros::Rate loop_rate(10);
    while (ros::ok())
    {
      controller.Update();
      ros::spinOnce();
      loop_rate.sleep();
    }

  In the Controller's constructor, a few communication mechanisms are initialized:

  * The ***CommsClient***, which provides a mechanism to communicate with other robots on your team (aka. V2V communication).

        // Create subt communication client
        this->client.reset(new subt::CommsClient(_name));
        this->client->Bind(&Controller::CommClientCallback, this);

  * The origin client, which requests the current robot pose from the entrance's point of view

        // Create a cmd_vel publisher to control a vehicle.
        this->originClient = this->n.serviceClient<subt_msgs::PoseFromArtifact>(
          "/subt/pose_from_artifact_origin");
        this->originSrv.request.robot_name.data = _name;

   * The ROS publisher, which sends `Twist` message to move the robot.

        // Create a cmd_vel publisher to control a vehicle.
        this->velPub = this->n.advertise<geometry_msgs::Twist>(_name + "/cmd_vel", 1);

   * The start signal is sent.

        // Send start signal
        std_srvs::SetBool::Request req;
        std_srvs::SetBool::Response res;
        req.data = true;
        if (!ros::service::call("/subt/start", req, res))
        {
          ROS_ERROR("Unable to send start signal.");
        }

   Finally, the Controller's **Update()** function continually asks for the current robot pose,
   calculates a forward velocity control message and publishes it


    void Controller::Update()
    {
      ...
      // Get pose
      ... this->originClient.call(this->originSrv) ...
      auto pose = this->originSrv.response.pose.pose;
      ...
      // Calculate velocity
      geometry_msgs::Twist msg;
      msg.linear.x = ...
      msg.angular.z = ...
      ...
      // Send command
      this->velPub.publish(msg);
    }

   Once the robot reaches the destination, it reports an artifact to the base station.

    // Report an artifact
    // Hardcoded to tunnel_circuit_practice_01's exginguisher_3
    subt::msgs::Artifact artifact;
    artifact.set_type(static_cast<uint32_t>(subt::ArtifactType::TYPE_EXTINGUISHER));
    artifact.mutable_pose()->mutable_position()->set_x(-8.1);
    artifact.mutable_pose()->mutable_position()->set_y(37);
    artifact.mutable_pose()->mutable_position()->set_z(0.004);

    std::string serializedData;
    if (!artifact.SerializeToString(&serializedData))
    {
      ROS_ERROR("ReportArtifact(): Error serializing message [%s]",
          artifact.DebugString().c_str());
    }
    else if (!this->client->SendTo(serializedData, subt::kBaseStationName))
    {
      ROS_ERROR("CommsClient failed to Send serialized data.");
    }