Robot Learning Project - Project1: Go Chase it! (4)

Place the robot

So far, a robot model was created from scratch, sensors were added to it to visualize its surroundings, and a package was developed to start the robot in a simulated environment. This is a real achievement!

But without placing the robot in a specific environment, let's place it in a built world.

Add World file

Paste myworld.world ( https://github.com/jeffnd/Robotics-Software-Engineer/blob/master/Project1-BuildMyWorld/world/World4 ) into the world directory of my_robot. In the worlds directory, there are now two files - empty.world and myworld.world, feel free to delete the empty.world file, we don't need it anymore.

start the world

Edit the world.launch file and add a reference to myworld.world. Open the world.launch file and edit this line:

<arg name="world_file"default="$(findmy_robot)/worlds/empty.world"/>

replace it with:

<arg name="world_file"default="$(findmy_robot)/worlds/myworld.world"/>

start up!

Now that the world file has been added to the my_robot package, start and visualize the robot:

$ cd /home/workspace/catkin_ws/
$ source devel/setup.bash
$ roslaunch my_robot world.launch

Initialize the position and orientation of the robot

The initial position of the robot can be achieved by editing the world.launch file:

<!-- Robot pose -->
<argname="x"default="0"/>
<argname="y"default="0"/>
<argname="z"default="0"/>
<argname="roll"default="0"/>
<argname="pitch"default="0"/>
<argname="yaw"default="0"/>

The best way to find out these numbers is to change the robot's position and orientation in Gazebo, record its pose, and then update the launch file.

Summary: Follow the steps below to place a robot in an environment:

11. Set the ball_chaser

The second main task in this project is to create the ball_chaser ROS package. In this package, the images captured by the camera are analyzed to determine the position of the white ball, and then drive the robot towards the direction of the ball. The nodes in ball_chaser establish communication with the my_robot package by subscribing to the data from the robot's camera sensor and publishing it to the robot's wheel joints.

package node

The ball_chaser package has two c++ nodes: drive_bot and process_image

drive_bot : This server node will provide a ball_chaser/command_robot service to drive the robot by controlling its linear x and angular z velocity. The service will publish a message containing the velocity to the wheel joint.

process_image : This client node will subscribe to the robot's camera images and analyze each image to determine the position of the white ball. Once the position of the ball is determined, the client node will request a service to drive the robot left, right or forward.

Now, follow the steps to setup ball_chaser .

Create the ball_chaser package

1) Navigate to the src directory of catkin_ws and create the ball_chaser package:

Write node in c++, since we already know that this package will contain c++ sources and messages, let's create the package with these dependencies:

$ cd /home/workspace/catkin_ws/src/
$ catkin_create_pkg ball_chaser roscpp std_msgs message_generation

2) Next, create a srv and a launch folder, which will further define the structure of the package:

$ cd /home/workspace/catkin_ws/src/ball_chaser/
$ mkdir srv
$ mkdir launch

Remember, srv is the directory where service files are stored, and launch is the directory where startup files are stored. The src directory for storing c++ programs is created by default.

build pack

$ cd /home/workspace/catkin_ws/
$ catkin_make

Summary: Follow the steps below to setup the ball_chaser package:

12. ROS node: drive_bot

This server node provides a ball_chaser/command_robot service to drive the robot by setting its linear x and angular z velocity. The service server publishes messages containing the wheel joint velocities.

After writing this node, you will be able to request the ball_chaser/command_robot service from a terminal or client node to drive the robot by controlling its linear x and angular z velocity.

ROS service file

1) Write DriveToTarget.srv file

In the srv directory of ball_chaser, create a DriveToTarget.srv file. Then, define the DriveToTarget.srv file as follows:

Require:

linear_x type float64

angular_z type float64

response:

msg_feedback type string

2) Test DriveToTarget.srv

After writing the service file, test it with ROS. Open a new terminal and execute the following command:

$ cd /home/workspace/catkin_ws/
$ source devel/setup.bash
$ rossrv show DriveToTarget

Received the following reply:

[ball_chaser/DriveToTarget]:
float64 linear_x
float64 angular_z
---
string msg_feedback

drive_bot.cpp node

Create scripts in the src directory of the ball_chaser package. Attached below is a program that will continuously publish to the robot/cmd_vel topic, this code will drive the robot forward:

#include"ros/ros.h"
#include"geometry_msgs/Twist.h"
//TODO: Include the ball_chaser "DriveToTarget" header file

// ROS::Publisher motor commands;
ros::Publisher motor_command_publisher;

// TODO: Create a handle_drive_request callback function that executes whenever a drive_bot service is requested
// This function should publish the requested linear x and angular velocities to the robot wheel joints
// After publishing the requested velocities, a message feedback should be returned with the requested wheel velocities

int main(int argc, char** argv)
{
    // Initialize a ROS node
    ros::init(argc, argv, "drive_bot");

    // Create a ROS NodeHandle object
    ros::NodeHandle n;

    // Inform ROS master that we will be publishing a message of type geometry_msgs::Twist on the robot actuation topic with a publishing queue size of 10
    motor_command_publisher = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10);

    // TODO: Define a drive /ball_chaser/command_robot service with a handle_drive_request callback function

    // TODO: Delete the loop, move the code to the inside of the callback function and make the necessary changes to publish the requested velocities instead of constant values
    while (ros::ok()) {
        // Create a motor_command object of type geometry_msgs::Twist
        geometry_msgs::Twist motor_command;
        // Set wheel velocities, forward [0.5, 0.0]
        motor_command.linear.x = 0.5;
        motor_command.angular.z = 0.0;
        // Publish angles to drive the robot
        motor_command_publisher.publish(motor_command);
    }

    // TODO: Handle ROS communication events
    //ros::spin();

    return0;
}

Take a look at the program and try to understand what's going on. Then, copy it to drive_bot.cpp and make the necessary changes to define a ball_chaser/command_robot service.

The reference code is as follows ---

#include"ros/ros.h"
#include"geometry_msgs/Twist.h"
//TODO: Include the ball_chaser "DriveToTarget" header file
#include "ball_chaser/DriveToTarget.h"

using namespace std;
// ROS::Publisher motor commands;
ros::Publisher motor_command_publisher;

// TODO: Create a handle_drive_request callback function that executes whenever a drive_bot service is requested
// This function should publish the requested linear x and angular velocities to the robot wheel joints
// After publishing the requested velocities, a message feedback should be returned with the requested wheel velocities
bool handle_drive_request(ball_chaser::DriveToTarget::Request& req, ball_chaser::DriveToTarget::Response& res) 
{
    ROS_INFO("DriveToTargetRequest received - linear_x: %.2f, angular_z: %.2f", req.linear_x, req.angular_z);
    
    // Create a motor_command object of type geometry_msgs::Twist
    geometry_msgs::Twist motor_command;
    
    // Set wheel velocities
    motor_command.linear.x = req.linear_x;
    motor_command.angular.z = req.angular_z;
    
    // Publish data to drive the robot
    motor_command_publisher.publish(motor_command);

    // Return a response message
    res.msg_feedback = "Motor Command is: linear_x: " + to_string(motor_command.linear.x) + ", angular_z: " + to_string(motor_command.angular.z);
    ROS_INFO_STREAM(res.msg_feedback);

    return true;
}

int main(int argc, char** argv)
{
    // Initialize a ROS node
    ros::init(argc, argv, "drive_bot");

    // Create a ROS NodeHandle object
    ros::NodeHandle n;

    // Inform ROS master that we will be publishing a message of type geometry_msgs::Twist on the robot actuation topic with a publishing queue size of 10
    motor_command_publisher = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10);

    // TODO: Define a drive /ball_chaser/command_robot service with a handle_drive_request callback function
    ros::ServiceServer service = n.advertiseService("/ball_chaser/command_robot", handle_drive_request);
    // TODO: Delete the loop, move the code to the inside of the callback function and make the necessary changes to publish the requested velocities instead of constant values
    /*while (ros::ok()) {
        // Create a motor_command object of type geometry_msgs::Twist
        geometry_msgs::Twist motor_command;
        // Set wheel velocities, forward [0.5, 0.0]
        motor_command.linear.x = 0.5;
        motor_command.angular.z = 0.0;
        // Publish angles to drive the robot
        motor_command_publisher.publish(motor_command);
    }*/

    // TODO: Handle ROS communication events
    ros::spin();

    return0;
}

Edit CMakeLists.txt

After writing a server node in c++, the following dependencies must be added:

Add add_compile_options for c++11 dependencies, this step is optional, depends on your code

Add add_service_files dependency which defines DriveToTarget.srv file

Add generate_messages dependency

Add include_directories dependency

Add add_executable, target_link_libraries and add_dependencies dependencies for drive_bot.cppscript

build pack

Now that you have included specific instructions for the drive_bot.cpp code in the CMakeLists.txt file, compile it with the following command:

$ cd /home/workspace/catkin_ws/
$ catkin_make

test-drive_bot.cpp

To test that the written service is working, first start the bot. Then call the /ball_chaser/command_robot service to drive the robot forward, left, then right.

1) Start the robot

$ cd /home/workspace/catkin_ws/
$ source devel/setup.bash
$ roslaunch my_robot world.launch

2) Run the drive_bot node

$ cd /home/workspace/catkin_ws/
$ source devel/setup.bash
$ rosrun ball_chaser drive_bot

3) Request ball_chaser/command_robot service

Test the service by requesting different speed sets from the terminal. Open a new terminal with all nodes running and type:

$ cd /home/workspace/catkin_ws/
$ source devel/setup.bash

$ rosservice call /ball_chaser/command_robot "linear_x: 0.5
angular_z: 0.0"# This request should drive your robot forward

$ rosservice call /ball_chaser/command_robot "linear_x: 0.0
angular_z: 0.5"# This request should drive your robot left

$ rosservice call /ball_chaser/command_robot "linear_x: 0.0
angular_z: -0.5"# This request should drive your robot right

$ rosservice call /ball_chaser/command_robot "linear_x: 0.0
angular_z: 0.0"# This request should bring your robot to a complete stop

startup file

Add the drive_bot node to a startup file. Create a ball_chaser.launch in the Launch directory of your ball_chaser package, and copy the following code into it:

<launch>

 <!-- The drive_bot node -->
  <node name="drive_bot" type="drive_bot" pkg="ball_chaser" output="screen">
  </node>

</launch>

This code will start the drive_bot node, which is included in the ball_chaser package. The server node outputs all logs to the terminal window.

Follow the steps below to create a drive_bot node:

Robot Learning Project - Project1: Go Chase it! (4)

Guess you like