我想创建一个将turtlesim节点移动到x,y位置的类。但是,当我检查rostopic / turtle1 / cmd_vel时,它没有显示任何Twist,并且turtlesim节点根本不移动。我仍然得到了乌龟的位置,但由于turlte没有移动,所以它只是永远停留在while循环中。任何帮助将不胜感激。
class PathFinding{
Private:
turtlesim::Pose initLocation;
turtlesim::Pose finalLocation;
double distance_tolerance;
ros::Publisher cmd_vel_pub;
ros::Subscriber pose_sub;
ros::NodeHandle nh;
public:
PathFinding(turtlesim::Pose goal, double d, ros::NodeHandle &n){
finalLocation = goal;
nh = n;
distance_tolerance = d;
cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
pose_sub = nh.subscribe("/turtle1/pose", 1000, &PathFinding::poseCallBack,this);
}
double getDistance(double x1, double y1, double x2,double y2){
return sqrt(pow((x1-x2),2) + pow((y1-y2),2));
}
void moveGoal(){
geometry_msgs::Twist vel_msg;
ros::Rate loop_rate(10);
do{
vel_msg.linear.x = 1.5*getDistance(initLocation.x,initLocation.y,finalLocation.x,finalLocation.y);
vel_msg.linear.y = 0;
vel_msg.linear.z = 0;
vel_msg.angular.x = 0;
vel_msg.angular.y = 0;
vel_msg.angular.z = 4*(atan2(finalLocation.y-initLocation.y,finalLocation.x-initLocation.x) - initLocation.theta);
cmd_vel_pub.publish(vel_msg);
ros::spinOnce();
loop_rate.sleep();
}
while(getDistance(initLocation.x,initLocation.y,finalLocation.x,finalLocation.y) > distance_tolerance);
vel_msg.linear.x = 0;
vel_msg.angular.z = 0;
cmd_vel_pub.publish(vel_msg);
}
void poseCallBack(const turtlesim::Pose &p_msg){
initLocation.x = p_msg.x;
initLocation.y = p_msg.y;
initLocation.theta = p_msg.theta;
}
};
}
这就是我使用它的方式:
int main(int argc, char **argv) {
ros::init(argc, argv, "turtleClassMoving");
ros::NodeHandle n;
turtlesim::Pose goal;
goal.x = 1;
goal.y = 1;
goal.theta = 0;
PathFinding t(goal, 0.5, n);
t.moveGoal();
ros::spin();
return(0);
}
答案 0 :(得分:0)
将节点句柄传递给类时的常见错误(如您的情况)是忘记传递私有节点句柄。所以也许你的程序实际上是在主题上发送命令,而不是你期望的那个。
因此,首先检查是否有任何其他cmd vel与
一起发送rostopic列表
如果,正如我所料,有一个类似的名称主题,使用不同的前缀,请尝试修改您的主要
ros :: NodeHandle n(&#34;〜&#34;);