Department of Electrical Engineering

Faculty Member:LE Monadi Sial Date: 24/05/2024

Semester: 8th Group: 1

EE381 Robotics
Lab 12: C++ Programming in ROS
<Open-ended Lab>

PLO5-CLO4 PLO5- PLO8- PLO9-

CLO5 CLO6 CLO7
Name Reg. Viva / Analysis Modern Ethics Individual
No Quiz / of Data Tool and Team
Demo in Lab Usage Work
Report

5 Marks 5 Marks 5 Marks 5 Marks 5 Marks

Fatima Humayun 339732

Ali Khalid 336676

EE-381 – Robotics
Introduction

This laboratory exercise will focus on designing and simulation a robot

simulation that performs a specified task while employing nodes driven by C++
programs.

Objectives

• Write C++ program script files

• Implement C++ nodes in ROS

Lab Conduct

• Respect faculty and peers through speech and actions

• The lab faculty will be available to assist the students. In case some aspect
of the lab experiment is not understood, the students are advised to seek
help from the faculty.
• In the tasks, there are commented lines such as #YOUR CODE STARTS
HERE# where you have to provide the code. You must put the code
between the #START and #END parts of these commented lines. Do NOT
remove the commented lines.
• Use the tab key to provide the indentation in python.

EE-381 – Robotics
Theory
In robotics, the two most popular languages for programming robots are C++
and Python. For prototype development and design concepts, python is used
due to its user-friendly nature. For practical implementation, C++ is used as the
code is compiled and thus runs much faster on the machine. A major issue with
C++ programming is its’ high difficulty. In ROS, there is extensive support for
both Python (rclpy) and C++ (rclcpp) as well as limited support for other
programming languages.

Common terminal commands for ROS 2 are provided below:

colcon build --packages-select <package_name>

build a specific package whenever a node is created or modified

. install/setup.bash
make the terminal “aware” of the workspace (notice the dot and space)

ros2 pkg create --build-type ament_python <package_name>

create a new package (must be done in src directory)

ros2 pkg create --build-type ament_python <package_name> --

dependencies rclpy std_msgs geometry_msgs sensor_msgs
create a new package with dependencies

ros2 run <package_name> <node_name>

execute a node in the terminal

EE-381 – Robotics
Open-Ended Lab Details ____________________________________________________________
Design and implement a ROS implementation that solves a specified problem with the
constraint of using C++ programming in the nodes.

Note that the problem to be solved, as well as the solution to be designed are both
open-ended in nature. Students are expected to come up with their own creative
ideas and implementations for the lab.

Specify the complete details of the problem and how it is to be solved.

### PROBLEM SPECIFICATION DETAILS START HERE ###

The C code is a Webots simulation controller for an 8-motor robot that
navigates through predefined target points using GPS and compass sensors.
The robot can be controlled manually using keyboard inputs or switch to an
autopilot mode to follow a sequence of waypoints. The robot adjusts its speeds
and direction based on its current position and orientation to reach the targets.
### PROBLEM SPECIFICATION DETAILS END HERE ###

Provide all relevant codes, screenshots/videos and explanations of your work.

### CODES START HERE ###

/*

* Copyright 1996-2023 Cyberbotics Ltd.

* Licensed under the Apache License, Version 2.0 (the "License");

* you may not use this file except in compliance with the License.

* You may obtain a copy of the License at

EE-381 – Robotics
* https://www.apache.org/licenses/LICENSE-2.0

* Unless required by applicable law or agreed to in writing, software

* distributed under the License is distributed on an "AS IS" BASIS,

* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

* See the License for the specific language governing permissions and

* limitations under the License.

*/

#include <math.h>

#include <stdio.h>

#include <webots/compass.h>

#include <webots/gps.h>

#include <webots/keyboard.h>

#include <webots/motor.h>

#include <webots/robot.h>

#define TIME_STEP 16

#define TARGET_POINTS_SIZE 13

#define DISTANCE_TOLERANCE 1.5

#define MAX_SPEED 7.0

EE-381 – Robotics
#define TURN_COEFFICIENT 4.0

enum XYZAComponents { X = 0, Y, Z, ALPHA };

enum Sides { LEFT, RIGHT };

typedef struct _Vector {

double u;

double v;

} Vector;

static WbDeviceTag motors[8];

static WbDeviceTag gps;

static WbDeviceTag compass;

static Vector targets[TARGET_POINTS_SIZE] = {

{-4.209318, 9.147717}, {0.946812, 9.404304}, {0.175989, -1.784311}, {-2.805353, -8.829694}, {-

3.846730, -15.602851},

{-4.394915, -24.550777}, {-1.701877, -33.617226}, {-4.394915, -24.550777}, {-3.846730, -

15.602851}, {-2.805353, -8.829694},

{0.175989, -1.784311}, {0.946812, 9.404304}, {-7.930821, 6.421292}

};

EE-381 – Robotics
static int current_target_index = 0;

static bool autopilot = true;

static bool old_autopilot = true;

static int old_key = -1;

static double modulus_double(double a, double m) {

const int div = (int)(a / m);

double r = a - div * m;

if (r < 0.0)

r += m;

return r;

// set left and right motor speed [rad/s]

static void robot_set_speed(double left, double right) {

int i;

for (i = 0; i < 4; i++) {

wb_motor_set_velocity(motors[i + 0], left);

wb_motor_set_velocity(motors[i + 4], right);

EE-381 – Robotics
static void check_keyboard() {

double speeds[2] = {0.0, 0.0};

int key = wb_keyboard_get_key();

if (key >= 0) {

switch (key) {

case WB_KEYBOARD_UP:

speeds[LEFT] = MAX_SPEED;

speeds[RIGHT] = MAX_SPEED;

autopilot = false;

break;

case WB_KEYBOARD_DOWN:

speeds[LEFT] = -MAX_SPEED;

speeds[RIGHT] = -MAX_SPEED;

autopilot = false;

break;

case WB_KEYBOARD_RIGHT:

speeds[LEFT] = MAX_SPEED;

speeds[RIGHT] = -MAX_SPEED;

autopilot = false;

EE-381 – Robotics
 break;

case WB_KEYBOARD_LEFT:

speeds[LEFT] = -MAX_SPEED;

speeds[RIGHT] = MAX_SPEED;

autopilot = false;

break;

case 'P':

if (key != old_key) { // perform this action just once

const double *position_3d = wb_gps_get_values(gps);

printf("position: {%f, %f}\n", position_3d[X], position_3d[Y]);

break;

case 'A':

if (key != old_key) // perform this action just once

autopilot = !autopilot;

break;

if (autopilot != old_autopilot) {

old_autopilot = autopilot;

if (autopilot)

EE-381 – Robotics
 printf("auto control\n");

else

printf("manual control\n");

robot_set_speed(speeds[LEFT], speeds[RIGHT]);

old_key = key;

// ||v||

static double norm(const Vector *v) {

return sqrt(v->u * v->u + v->v * v->v);

// v = v/||v||

static void normalize(Vector *v) {

double n = norm(v);

v->u /= n;

v->v /= n;

EE-381 – Robotics
// v = v1-v2

static void minus(Vector *v, const Vector *const v1, const Vector *const v2) {

v->u = v1->u - v2->u;

v->v = v1->v - v2->v;

// autopilot

// pass trough the predefined target positions

static void run_autopilot() {

// prepare the speed array

double speeds[2] = {0.0, 0.0};

// read gps position and compass values

const double *position_3d = wb_gps_get_values(gps);

const double *north_3d = wb_compass_get_values(compass);

// compute the 2D position of the robot and its orientation

const Vector position = {position_3d[X], position_3d[Y]};

// compute the direction and the distance to the target

Vector direction;

EE-381 – Robotics
 minus(&direction, &(targets[current_target_index]), &position);

const double distance = norm(&direction);

normalize(&direction);

// compute the error angle

const double robot_angle = atan2(north_3d[0], north_3d[1]);

const double target_angle = atan2(direction.v, direction.u);

double beta = modulus_double(target_angle - robot_angle, 2.0 * M_PI) - M_PI;

// move singularity

if (beta > 0)

beta = M_PI - beta;

else

beta = -beta - M_PI;

// a target position has been reached

if (distance < DISTANCE_TOLERANCE) {

char index_char[3] = "th";

if (current_target_index == 0)

sprintf(index_char, "st");

else if (current_target_index == 1)

EE-381 – Robotics
 sprintf(index_char, "nd");

else if (current_target_index == 2)

sprintf(index_char, "rd");

printf("%d%s target reached\n", current_target_index + 1, index_char);

current_target_index++;

current_target_index %= TARGET_POINTS_SIZE;

// move the robot to the next target

else {

speeds[LEFT] = MAX_SPEED - M_PI + TURN_COEFFICIENT * beta;

speeds[RIGHT] = MAX_SPEED - M_PI - TURN_COEFFICIENT * beta;

// set the motor speeds

robot_set_speed(speeds[LEFT], speeds[RIGHT]);

int main(int argc, char *argv[]) {

// initialize webots communication

wb_robot_init();

EE-381 – Robotics
 // print user instructions

printf("You can drive this robot:\n");

printf("Select the 3D window and use cursor keys:\n");

printf("Press 'A' to return to the autopilot mode\n");

printf("Press 'P' to get the robot position\n");

printf("\n");

wb_robot_step(1000);

const char *names[8] = {"left motor 1", "left motor 2", "left motor 3", "left motor 4",

"right motor 1", "right motor 2", "right motor 3", "right motor 4"};

// get motor tags

int i;

for (i = 0; i < 8; i++) {

motors[i] = wb_robot_get_device(names[i]);

wb_motor_set_position(motors[i], INFINITY);

// get gps tag and enable

gps = wb_robot_get_device("gps");

EE-381 – Robotics
 wb_gps_enable(gps, TIME_STEP);

// get compass tag and enable

compass = wb_robot_get_device("compass");

wb_compass_enable(compass, TIME_STEP);

// enable keyboard

wb_keyboard_enable(TIME_STEP);

// start forward motion

robot_set_speed(MAX_SPEED, MAX_SPEED);

// main loop

while (wb_robot_step(TIME_STEP) != -1) {

check_keyboard();

if (autopilot)

run_autopilot();

wb_robot_cleanup();

EE-381 – Robotics
 return 0;

### CODES END HERE ###

### SCREENSHOTS START HERE ###

### SCREENSHOTS END HERE ###

### EXPLANATION-CONCLUSION START HERE ###

1. Libraries and Definitions:
 The code includes necessary libraries (math.h, stdio.h,
webots/compass.h, webots/gps.h, webots/keyboard.h,
webots/motor.h, and webots/robot.h) and defines constants such as

EE-381 – Robotics
 TIME_STEP, TARGET_POINTS_SIZE, DISTANCE_TOLERANCE,
MAX_SPEED, and TURN_COEFFICIENT.
2. Data Structures:
 Vector struct to represent 2D vectors.
 Enums for array indexing and side identification.
3. Global Variables:
 Motor tags, GPS, and compass device tags.
 An array of target points (targets) and control variables for autopilot
and keyboard states.
4. Utility Functions:
 modulus_double: Calculates the modulus of a double value.
 robot_set_speed: Sets the speed for all motors.
 check_keyboard: Processes keyboard inputs to control the robot
manually or toggle autopilot.
 norm, normalize, minus: Vector operations for distance calculation
and direction normalization.
5. Autopilot Function (run_autopilot):
 Reads GPS and compass data.
 Computes the direction and distance to the current target.
 Normalizes the direction vector.
 Calculates the error angle between the robot's orientation and the
target direction.
 Adjusts the robot's speed to move towards the target.
 Advances to the next target upon reaching the current one.
6. Main Function:
 Initializes Webots communication.
 Prints user instructions.
 Retrieves motor, GPS, and compass devices.
 Enables GPS, compass, and keyboard.
 Sets initial motor speeds.
 Main loop to continually check keyboard input and run autopilot if
enabled.

EE-381 – Robotics
  Cleans up Webots resources upon exit.

### EXPLANATION-CONCLUSION END HERE ###

EE-381 – Robotics