質問をすることでしか得られない、回答やアドバイスがある。

15分調べてもわからないことは、質問しよう!

新規登録して質問してみよう
ただいま回答率
85.50%
アルゴリズム

アルゴリズムとは、定められた目的を達成するために、プログラムの理論的な動作を定義するものです。

C++

C++はC言語をもとにしてつくられた最もよく使われるマルチパラダイムプログラミング言語の1つです。オブジェクト指向、ジェネリック、命令型など広く対応しており、多目的に使用されています。

Q&A

3回答

918閲覧

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

sousoudig

総合スコア0

アルゴリズム

アルゴリズムとは、定められた目的を達成するために、プログラムの理論的な動作を定義するものです。

C++

C++はC言語をもとにしてつくられた最もよく使われるマルチパラダイムプログラミング言語の1つです。オブジェクト指向、ジェネリック、命令型など広く対応しており、多目的に使用されています。

0グッド

1クリップ

投稿2020/06/22 06:06

編集2020/06/25 14:54

前提・実現したいこと

この質問の編集前の皆さんのアドバイスのおかげで一応計算は出来ました。しかし今度はプログラミングにそれを書き出そうとすると一応ビルドは出来るのですが目標座標まで到達してくれません。恐らく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(int count); 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}; 20 21turtleSim::turtleSim(){ 22 twist_pub = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1000); 23 pose_sub = nh.subscribe<turtlesim::Pose>("turtle1/pose", 1, &turtleSim::poseCallback, this); 24 timer = nh.createTimer(ros::Duration(0.1), &turtleSim::timerCallback, this); 25 26 geometry_msgs::Twist twist; 27 twist.linear.x = 0.0; 28 twist.linear.y = 0.0; 29 twist.linear.z = 0.0; 30 twist.angular.x = 0.0; 31 twist.angular.y = 0.0; 32 twist.angular.z = 0.0; 33 twist_pub.publish(twist); 34} 35 36turtleSim::~turtleSim(){ 37 38} 39 40void turtleSim::poseCallback(const turtlesim::PoseConstPtr& pose){ 41 ROS_INFO("x:%f",pose->x); 42} 43 44void turtleSim::timerCallback(const ros::TimerEvent&){ 45 setMoveVector(10); 46 setMoveVector(10); 47} 48 49void turtleSim::setMoveVector(int count){ 50 int i ; 51 float x, y, theta, angular,pose_x,pose_y,pose_theta ; 52 geometry_msgs::Twist twist; 53 ros::Rate loop_rate(10); 54 55 x = 5.000000 - pose_x ; 56 y = 5.000000 - pose_y ; 57 theta = atan(y/x) ; 58 angular = 6.283185 - pose_theta ; 59 60 ROS_INFO("pose_x:%f",pose_x) ; 61   ROS_INFO("pose_y:%f",pose_y) ; 62 ROS_INFO("theta:%f",theta) ; 63 ROS_INFO("angular:%f",angular) ; 64 for(i=0;i < count; i++) { 65 if(x==0.000000) { 66 ROS_INFO("x:%f",x) ; 67 twist.angular.z = 1.570796 ; 68 twist.linear.x = y ; 69 twist_pub.publish(twist) ; 70 loop_rate.sleep() ; 71 } 72 else { 73 twist.angular.z = angular ; 74 twist.linear.x = x ; 75 twist.linear.y = y ; 76 twist_pub.publish(twist) ; 77 loop_rate.sleep() ; 78 } 79 } 80} 81 82int main(int argc, char **argv){ 83 ros::init(argc, argv, "move_turtlesim"); 84 85 turtleSim turtlesim; 86 87 ros::spin(); 88 return 0; 89} 90

気になる質問をクリップする

クリップした質問は、後からいつでもMYページで確認できます。

またクリップした質問に回答があった際、通知やメールを受け取ることができます。

バッドをするには、ログインかつ

こちらの条件を満たす必要があります。

guest

回答3

0

図内の(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あたりで求めればよいかと.

投稿2020/06/22 07:58

fana

総合スコア11634

バッドをするには、ログインかつ

こちらの条件を満たす必要があります。

fana

2020/06/22 07:59

既存回答と被った.
fana

2020/06/22 08:06

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

0

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

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

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

投稿2020/06/22 07:28

編集2020/06/22 12:24
episteme

総合スコア16614

バッドをするには、ログインかつ

こちらの条件を満たす必要があります。

ozwk

2020/06/22 07:31

θgは未知では?
episteme

2020/06/22 07:34 編集

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

2020/06/22 07:40

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

2020/06/22 07:51

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

2020/06/22 07:52

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

2020/06/22 07:57

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

2020/06/22 08:01 編集

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

2020/06/22 08:54

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

2020/06/22 08:59

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

2020/06/22 09:04

ヒントあざます!!
guest

0

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

投稿2020/06/22 06:17

y_waiwai

総合スコア87719

バッドをするには、ログインかつ

こちらの条件を満たす必要があります。

sousoudig

2020/06/22 06:58

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

2020/06/22 07:05

三角関数、でぐぐればいろいろ解説が出てきます 一通り読んでみましょう sinとかcosででてきます #高校の数学の問題ですね
Q71

2020/06/22 07:11

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

2020/06/22 07:14

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

2020/06/22 07:15

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

あなたの回答

tips

太字

斜体

打ち消し線

見出し

引用テキストの挿入

コードの挿入

リンクの挿入

リストの挿入

番号リストの挿入

表の挿入

水平線の挿入

プレビュー

まだベストアンサーが選ばれていません

会員登録して回答してみよう

アカウントをお持ちの方は

15分調べてもわからないことは
teratailで質問しよう!

ただいまの回答率
85.50%

質問をまとめることで
思考を整理して素早く解決

テンプレート機能で
簡単に質問をまとめる

質問する

関連した質問