3b

pubvelsafe.cpp

/*  Code a new node called pubvelsafe (in a new file pubvelsafe.cpp) in order to send:
 *     - A random angular velocity command and a fixed linear velocity of 1.0 when the
 *     turtle is placed in a safe zone defined by a square around the center of the window.
 *     - A random angular velocity and a random linear velocity (i.e. as done in pubvel.cpp)
 *     otherwise.
 */

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Pose.h>
#include <stdlib.h>

const float SAFE_ZONE_SQUARE = 5.0;
const float WINDOWS_SIZE = 11.0;

float turtle_pos_x;
float turtle_pos_y;
float turtle_pos_theta;

void poseMessageReceived(const turtlesim::Pose& msg)
{
    turtle_pos_x = msg.x;
    turtle_pos_y = msg.y;
    turtle_pos_theta = msg.theta;
}

bool is_in_safe_zone(float x, float y)
{
    float safe_zone_min = (WINDOWS_SIZE - SAFE_ZONE_SQUARE) / 2;
    float safe_zone_max = (WINDOWS_SIZE + SAFE_ZONE_SQUARE) / 2;

    if (x < safe_zone_min || x > safe_zone_max)
        return false;
    if (y < safe_zone_min || y > safe_zone_max)
        return false;

    return true;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "publish_velocity_safe");
    ros::NodeHandle nh;

    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1000);
    ros::Subscriber sub = nh.subscribe("turtle1/pose", 1000, &poseMessageReceived);

    srand(time(0));
    ros::Rate rate(2);
    while (ros::ok()) {
        geometry_msgs::Twist msg;

        if (is_in_safe_zone(turtle_pos_x, turtle_pos_y)) {
            msg.linear.x = 1;
        } else {
            msg.linear.x = double(rand()) / double(RAND_MAX);
        }

        msg.angular.z = 2 * double(rand()) / double(RAND_MAX) - 1;

        pub.publish(msg);

        ROS_INFO_STREAM("Sending random velocity command:"
                        << " linear=" << msg.linear.x << " angular=" << msg.angular.z);

        ros::spinOnce();
        rate.sleep();
    }
}