I'm using ROS kinetic with UBUNTU 4.13.0-45-generic x86_64. When I run the following code, the callback method (void poseHandler::poseHandlerCallBack(const turtlesim::PoseConstPtr& posePtrNew)) is not invoked. What am I doing wrong?
#include <ros ros.h=""> #include <turtlesim pose.h=""> #include <geometry_msgs twist.h=""> #include <std_srvs empty.h=""> #include <signal.h> #include <termios.h> #include <stdio.h>
float initialX;
float initialY;
float initialTheta;
class poseHandler
{
public:
poseHandler();
turtlesim::PoseConstPtr currPosePtr;
turtlesim::Pose currPose;
void printPose();
void poseHandlerCallBack(const turtlesim::PoseConstPtr& posePtrNew);
};
poseHandler::poseHandler()
{
}
void poseHandler::printPose()
{
ROS_INFO("x=%f,y=%f", currPose.x, currPose.y);
ROS_INFO("y=%f,", currPose.y);
ROS_INFO("theta=%f,", currPose.theta);
ROS_INFO("lin_vel=%f,", currPose.linear_velocity);
ROS_INFO("ang_vel%f\n", currPose.angular_velocity);
}
void poseHandler::poseHandlerCallBack(const turtlesim::PoseConstPtr& posePtrNew)
{
currPosePtr = posePtrNew;
currPose.x = currPosePtr->x;
currPose.y = currPosePtr->y;
currPose.theta = currPosePtr->theta;
currPose.linear_velocity = currPosePtr->linear_velocity;
currPose.angular_velocity = currPosePtr->angular_velocity;
ROS_INFO("poseHandlerCallBack!\n");
}
int main(int argc, char** argv)
{
poseHandler pH;
ros::init(argc, argv, "draw_ractangle");
ros::NodeHandle nh;
ros::Subscriber pose_sub = nh.subscribe<turtlesim::Pose>
("/turtlesim/turtle1/pose",1000,
&poseHandler::poseHandlerCallBack, &pH);
initialX = pH.currPose.x;
initialY = pH.currPose.y;
initialTheta = pH.currPose.theta;
pH.printPose();
ros::Publisher twist_pub = nh.advertise<geometry_msgs::Twist>
("turtle1/cmd_vel", 100);
ros::Rate loop_rate(1);
geometry_msgs::Twist twist_obj;
twist_pub.publish(twist_obj);
while(ros::ok())
{
ROS_INFO("target = %f", (initialX+2));
if(pH.currPose.x < (initialX + 2))
{
twist_obj.linear.x = 0.4;
twist_obj.angular.z = 0.0;
ROS_INFO("Turtle move forward\n");
}
else
{
twist_obj.linear.x = 0.0;
twist_obj.angular.z = 0;
ROS_INFO("Turtle Stopped\n");
}
pH.printPose();
twist_pub.publish(twist_obj);
loop_rate.sleep();
ros::spinOnce();
ROS_INFO("spinOnce() called\n");
}
}