我正在尝试在 C++ 中实现 ROS GotoGoal,这是代码
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Pose2D.h"
#include "turtlesim/Pose.h"
class Turtle {
public :
Turtle(int argc,char** argv){
ros::init(argc,argv,"mover");
ros::NodeHandle n;
pose = turtlesim::Pose();
pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 100);
sub = n.subscribe("/turtle1/pose", 100, &Turtle::Update, this);
}
void Update(const turtlesim::Pose::ConstPtr& msg){
ROS_INFO("Pose recieved : x = %f y = %f\n", msg->x, msg->y );
pose = *msg;
}
void move2goal(){
turtlesim::Pose goalPose= turtlesim::Pose() ;
std::cout<<"Enter goal x : "<<" ";
std::cin>>goalPose.x ;
std::cout<<"Enter goal y : "<<" ";
std::cin>>goalPose.y ;
float d ;
std::cout<<"Enter distance tolerance d : "<<" ";
std::cin>>d ;
auto vel_msg = geometry_msgs::Twist() ;
ros::Rate loop_rate(2.0);
while(distance(goalPose)>=d && ros::ok()){
vel_msg.linear.x = linear_velocity(goalPose,1.5);
vel_msg.linear.y =0 ;
vel_msg.linear.z = 0 ;
vel_msg.angular.x = 0 ;
vel_msg.angular.y= 0 ;
vel_msg.angular.z = angular_velocity(goalPose,6) ;
pub.publish(vel_msg) ;
loop_rate.sleep() ;
ROS_INFO("current : %f %f\n",pose.x,pose.y) ;
}
vel_msg.angular.z=0 ;
vel_msg.linear.x =0 ;
pub.publish(vel_msg) ;
ros::spin();
}
ros::Publisher pub;
ros::Subscriber sub;
turtlesim::Pose pose;
int ch = 0;
};
int main(int argc, char** argv) {
Turtle turtle = Turtle(argc,argv);
turtle.move2goal();
return 0;
}
但是更新回调函数没有被调用,并且由于姿势没有更新,乌龟正在绕圈移动。我尝试使用 ROS_INFO 来调试问题,但没有任何效果。我在这里做错了什么? 注意:由于 stackoverflow 的政策,一些函数的实现已从代码片段中删除。
[输出][1][1]:https://i.stack.imgur.com/eL9Sr.png