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

Question

前提・実現したいこと

この質問の編集前の皆さんのアドバイスのおかげで一応計算は出来ました。しかし今度はプログラミングにそれを書き出そうとすると一応ビルドは出来るのですが目標座標まで到達してくれません。恐らくsetMoveVector関数の中の書き方が違うのかなと思われまが、おおまかな原因がわかりません。改善点を教えて貰えますでしょうか？

コード
#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
#include <turtlesim/Pose.h>

class turtleSim{
public:
    turtleSim();
    ~turtleSim();
    void poseCallback(const turtlesim::PoseConstPtr& pose);
    void setMoveVector(int count);
    void timerCallback(const ros::TimerEvent&);

private:
    ros::Publisher twist_pub;
    ros::Subscriber pose_sub;
    ros::Timer timer;
    ros::NodeHandle nh;
};

turtleSim::turtleSim(){
    twist_pub = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1000);
    pose_sub = nh.subscribe<turtlesim::Pose>("turtle1/pose", 1, &turtleSim::poseCallback, this);
    timer = nh.createTimer(ros::Duration(0.1), &turtleSim::timerCallback, this);

    geometry_msgs::Twist twist;
    twist.linear.x = 0.0;
    twist.linear.y = 0.0;
    twist.linear.z = 0.0;
    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    twist.angular.z = 0.0;
    twist_pub.publish(twist);
}

turtleSim::~turtleSim(){

}

void turtleSim::poseCallback(const turtlesim::PoseConstPtr& pose){
    ROS_INFO("x:%f",pose->x);
}

void turtleSim::timerCallback(const ros::TimerEvent&){
    setMoveVector(10);
    setMoveVector(10);
}

void turtleSim::setMoveVector(int count){
    int i ;
    float x, y, theta, angular,pose_x,pose_y,pose_theta ;
    geometry_msgs::Twist twist;
    ros::Rate loop_rate(10);

    x = 5.000000 - pose_x ;
    y = 5.000000 - pose_y ;
    theta = atan(y/x) ;
    angular = 6.283185 - pose_theta ;

    ROS_INFO("pose_x:%f",pose_x) ;
　　 ROS_INFO("pose_y:%f",pose_y) ;
    ROS_INFO("theta:%f",theta) ;
    ROS_INFO("angular:%f",angular) ;
    for(i=0;i < count; i++) {
     if(x==0.000000) {
     ROS_INFO("x:%f",x) ;
     twist.angular.z = 1.570796 ;
     twist.linear.x = y ;
     twist_pub.publish(twist) ;
     loop_rate.sleep() ;
    }
    else {
     twist.angular.z = angular ;
     twist.linear.x = x ;
     twist.linear.y = y ;
     twist_pub.publish(twist) ;
     loop_rate.sleep() ;
    }
   }
}

int main(int argc, char **argv){
    ros::init(argc, argv, "move_turtlesim");

    turtleSim turtlesim;

    ros::spin();
    return 0;
}

Comments

[thkana]

https://teratail.com/questions/271234 と酷似した質問に見えますが、
利用規約 https://teratail.com/legal
第7条（禁止事項）
(11)複数のユーザーIDを1人で保有する行為
にはあたらないのですね?

Answer 1

図内の(5,5)が (x_w,y_w)側の系での値なのだとして……

亀の位置(x,y)からこの(5,5)地点に向かうベクトルをVとする．
V = ( 5-x. 5-y )

• d_x, d_y は，このベクトルVと，(x_r, y_r)側の座標系の基底ベクトル(これはθ_rから求まる)との内積によって得られる．
• 角度θ_gがわざわざ欲しい場合には，d_xとd_yを用いてatanあたりで求めればよいかと．

Comments

[fana]

既存回答と被った．

[episteme]

お先に～♪

[fana]

（湧き上がる敗北感に耐えつつ，二度と投稿前のリロードを怠るまいと固く誓うのであった……　完）

Answer 2

(x,y)と(5,5)間の距離をRとすると
dy = Rsinθg
dx = Rcosθg

[追記]
回転行列(-θr) に ベクトル(5-x, 5-y)を掛ければ(dx,dy)が求まると思われ。

[追追記]
てかアレだね、欲しいのがθgならdx, dy 求めてどーこーするよか
θr + θg を求めるのはどってことないから、そっからθrさっ引くほうが楽ね。

Comments

[ozwk]

θgは未知では?

[episteme]

あ、紫□は未知なのか。
ならば(x,y)を中心に(5,5)をθrだけ右回転。
アフィン変換だからどってことない。

[episteme]

カメさんが(5,5)に向かうにはどんだけ曲がればいいか(θg)を求めたいのかなきっと。

[sousoudig]

座標［5，5］の位置に行くためにどれだけ曲がればいいのかArctanを使用して求める問題なのですがそれ以前のdx,dyをお恥ずかしい話どうやって求めればいいのかわからず今回質問させていただきました。

[episteme]

追記しました。θg = arctan(dy/dx)

[sousoudig]

今回の場合だったらdx、dyを求めるためにはdx=（5-x）sinθr,dy=(5-y)cosθrでよろしいのでしょうか。

[episteme]

よろしいのでしょうかって... (x,y) と θr にテキトーな値ぶち込んで検算すりゃえぇやん。
方眼紙用意してやってみりゃえぇ、勉強ってそーゆーもんでしょ。

[sousoudig]

すごい心に響きました…心を改め清い心でこれを必ずや解いてみせます。

[episteme]

(x,y)=(4,3)で検算するといいカンジになります♪

[sousoudig]

ヒントあざます！！

Answer 3

三角関数というのを使います

Comments

[sousoudig]

すみません。具体的にどういった感じで求めていけばいいでしょうか。

[y_waiwai]

三角関数、でぐぐればいろいろ解説が出てきます
一通り読んでみましょう

sinとかcosででてきます

＃高校の数学の問題ですね

[Q71]

あと、座標変換。xy(w)をθrだけ回転させてxy(r)座標にします。

[Q71]

あ、ごめんなさい。(5,5)は、rとwのどちらの座標系での座標ですか？r系なら、回転は要らないですね。

[swordone]

いや、紫四角の値を出すのに座標変換の知識はあまりいらないですね。