Skip to content

Instantly share code, notes, and snippets.

@ksvbka
Created August 23, 2021 02:07
Show Gist options
  • Save ksvbka/f5993830d6c85e1653eb906526a28ab4 to your computer and use it in GitHub Desktop.
Save ksvbka/f5993830d6c85e1653eb906526a28ab4 to your computer and use it in GitHub Desktop.
3b
/* 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();
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment