#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Twist.h>
#include
#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include <unistd.h>
class MoveHusky {
public:
ros::NodeHandle nh;
double cov_x, cov_y, cov_z, norm_cov;
ros::Publisher vel_pub;
geometry_msgs::Twist vel_msg;
MoveHusky() {
ros::Subscriber pose_cov =
nh.subscribe("amcl_pose", 1000, &MoveHusky::poseCallback, this);
vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
ROS_INFO("Constructed");
ros::spin();
}
// ~MoveHusky() {}
void poseCallback(
const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &pose_msg) {
cov_x = pose_msg->pose.covariance[0];
cov_y = pose_msg->pose.covariance[8];
cov_z = pose_msg->pose.covariance[35];
ROS_INFO("cov_x: %f", cov_x);
ROS_INFO("cov_y: %f", cov_y);
ROS_INFO("cov_z: %f", cov_z);
norm_cov = (cov_x + cov_y + cov_z) / 3;
ROS_INFO("norm_cov: %f", norm_cov);
}
void forward(int t) {
int i = 0;
ROS_INFO("Forwarding");
while (i < t) {
vel_msg.linear.x = 1.0;
vel_msg.angular.z = 0.0;
vel_pub.publish(vel_msg);
ros::Duration(0.01).sleep();
i++;
}
}
void turn(int t) {
int i = 0;
ROS_INFO("Turning");
while (i < t) {
vel_msg.linear.x = 0.0;
vel_msg.angular.z = 0.5;
vel_pub.publish(vel_msg);
ros::Duration(0.1).sleep();
i++;
}
}
void stop(int t) {
int i = 0;
ROS_INFO("Stopping");
while (i < t) {
vel_msg.linear.x = 0.0;
vel_msg.angular.z = 0.0;
vel_pub.publish(vel_msg);
ros::Duration(0.1).sleep();
i++;
}
}
void move_in_square() {
int j = 0;
while (j < 4) {
forward(15);
turn(30);
stop(1);
j++;
}
}
bool my_callback(std_srvs::Empty::Request &req,
std_srvs::Empty::Response &res) {
ROS_INFO("The Service /move_husky_in_square has been called");
move_in_square();
ROS_INFO("Finished service /move_husky_in_square");
return true;
}
};
int main(int argc, char **argv) {
ros::init(argc, argv, “move_husky_node”);
ros::NodeHandle nh;
ros::ServiceClient reset_client;
reset_client = nh.serviceClient<std_srvs::Empty>(“global_localization”);
MoveHusky movehusky;
movehusky.norm_cov = 1.0;
while (movehusky.norm_cov > 0.65) {
// movehusky.recalculate_cov();
ROS_INFO("norm_cov : %f", movehusky.norm_cov);
movehusky.move_in_square();
std_srvs::Empty redistribute;
if (reset_client.call(redistribute)) {
ROS_INFO("Husky is still lost.");
ROS_INFO(
"Reset service successfully called. Distributing particles cloud.");
} else {
ROS_ERROR("Failed to call reset_service /global_localization");
}
}
ROS_INFO(“Husky has accurately localized itself!”);
ros::spin();
return 0;
}
Hi Mr Bayode, can you please help me with my code, I have been stuck for quite a while, this is my C++ attempt on ROSNavigation Chapter 3: Localization last exercise, my problem is I do not know how to write subscriber in class properly, my loop is forever stuck in the sub call back if I put ros::spin() in the constructor, but if I take the spin out and put it into the main, it will never callback. Thank you so much!!