ノード内のシュミレータロボに座標[5,5]の地点まで動くようにしたい

前提・実現したいこと

今私はROSを使用してturtlesimノードの亀が今いる座標軸や回転角を購読してそこから回転、移動して座標[5.5]地点まで動くプログラミングを作りたいと思っており、プログラミングを組み立ててみたのですが、ビルドはできたのですが，亀は超高速で円を描くだけでした。以下にプログラミングを載せますので改善点を教えてもらえないでしょうか。恐らくsetMoveVector関数の中の書き方が違うのかなと思われます。

該当のソースコード

C++
1ソースコード
2#include "ros/ros.h"
3#include <geometry_msgs/Twist.h>
4#include <turtlesim/Pose.h>
5
6class turtleSim{
7public:
8    turtleSim();
9    ~turtleSim();
10    void poseCallback(const turtlesim::PoseConstPtr& pose);
11    void setMoveVector(float liner_x,int theta, int cnt);
12    void timerCallback(const ros::TimerEvent&);
13
14private:
15    ros::Publisher twist_pub;
16    ros::Subscriber pose_sub;
17    ros::Timer timer;
18    ros::NodeHandle nh;
19　　ros::Publisher pose_x;
20    ros::Publisher pose_y;
21    ros::Publisher pose_theta;
22
23};
24
25turtleSim::turtleSim(){
26    twist_pub = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1000);
27    pose_sub = nh.subscribe<turtlesim::Pose>("turtle1/pose", 1, &turtleSim::poseCallback, this);
28    timer = nh.createTimer(ros::Duration(0.1), &turtleSim::timerCallback, this);
29
30    geometry_msgs::Twist twist;
31    twist.linear.x = 0.0;
32    twist.linear.y = 0.0;
33    twist.linear.z = 0.0;
34    twist.angular.x = 0.0;
35    twist.angular.y = 0.0;
36    twist.angular.z = 0.0;
37    twist_pub.publish(twist);
38}
39
40turtleSim::~turtleSim(){
41
42}
43
44void turtleSim::poseCallback(const turtlesim::PoseConstPtr& pose){
45    ROS_INFO("x:%f",pose->x);
46}
47
48void turtleSim::timerCallback(const ros::TimerEvent&){
49    setMoveVector(0.2,20,10);
50    setMoveVector(－0.2,20,10);
51}
52
53vvoid turtleSim::setMoveVector(float liner_x,int theta, int cnt){
54    int i ;
55    float x, y,angular,pose_x,pose_y ;
56    geometry_msgs::Twist twist;
57    ros::Rate loop_rate(10);
58
59    x = 5.000000 - pose_x ;
60    y = 5.000000 - pose_y ;
61    theta = atan(y/x) ;
62
63    for(i=0;i<cnt;i++){
64     twist.liner.x=liner_x;
65     twist.angular.x=theta;
66     twist_pub.publish(twist);
67     loop_rate.sleep();
68     }
69}
70
71
72int main(int argc, char **argv){
73    ros::init(argc, argv, "move_turtlesim");
74
75    turtleSim turtlesim;
76
77    ros::spin();
78    return 0;
79}
80

試したこと

とりあえず自分の座標［ｘ，ｙ］と回転角θを購読しそこから目的の座標［５，５］と自分の座標の差を求めそこからatanθを算出そこから亀がどれくらい回転すれば良いのか算出したつもりでした（まぁ、実際のところはできていませんが...）

補足情報（FW/ツールのバージョンなど）

今の環境としは、ubuntsu１6.4でＣ＋＋ベースです。

Penpen7

2020/06/25 21:52 編集

どうでもいいかもしれませんが、setMoveVectorでpose_x,pose_y, pose_thetaを初期化しないまま使ってますけど大丈夫ですか？ 2π=6.283185やπ/2=1.570796など急にマジックナンバーが出てくるとびっくりするので、cmathをインクルードしてconst float pi = acos(-1)なり、M_PIといった定数を使ったほうがいいです。

Penpen7

2020/06/25 21:57

そもそも、pose_x, pose_y, pose_thetaには何も代入されていないので、現在地点は取れてないのではないですか？ ROS_INFO("pose_x:%f",pose_x) ; ROS_INFO("pose_y:%f",pose_y) ; ROS_INFO("theta:%f",theta) ; でちゃんと正しい値は出てますか？

macoffee

2020/06/25 22:02

一応ＲＯＳ＿ＩＮＦＯでｘ，ｙ、thetaの値は出力されます。そしてその情報をそれぞれpose_x,pose_y,pose_thetaに代入したいのですが、解決策はありますでしょうか。そしてマジックナンバーの件なんですが、私の計算だと必要かなと思い追加しましたが、考えているうちに必要のない計算だということがわかり編集させていただきました。

Penpen7

2020/06/25 22:11

現在地を保持するにはクラスに現在地用のメンバ変数を作ったほうが早いと思います

Penpen7

2020/06/25 22:17 編集

クラスの定義の中で変数をメソッドの外に定義したものをメンバ変数と言います。現在地をいじる時には逐次そのメンバ変数をいじるようにして、現在地が必要な時にそのメンバ変数の値を参照すればいいです。 poseCallbackという関数でどこにいるか取っているんですかね？それならposeCallBackでメンバー変数に代入する処理をすればいいと思います。

macoffee

2020/06/25 22:26 編集

すみません。質問なのですがその関数内（この場合ではsetMoveVector関数）でしか現在地は使用しないのであればわざわざクラス内で宣言する必要性はあるのでしょうか？関数内で宣言ではだめなのでしょうか？

Penpen7

2020/06/25 22:30

今はそうなのかもしれませんが、今後、他の処理でも現在地を見たいときに改修するのは大変だと思います。オブジェクト指向的にも現在いる場所はメンバ変数として定義するのはむしろ自然に感じます。

macoffee

2020/06/25 22:54 編集

なるほどです。修正させていただきました。