ROS Gazebo のgetRPY(roll,pitch,yaw);で得られる値はradianでしょうか?
#実現したいこと
publisher でlink(upperLegL)のピッチ角度が1rad -1radを行き来するサイン波になるようにしてgazebo上で動かしたところ、見た目ではその通りに動いています。そこで、link の角度を取得したいと思っています。-1を行き来しません
#発生している問題
subscriberでlinkの情報を得て、getRPY(roll,pitch,yaw);で角度を得たのですが、pitchが1
rollは2~-3 pitchは0.3-0.01 yawは7~-5を行き来している感じです
例↓
cpp
1[ INFO] [1564473297.262904900, 127.259000000]: --- roll: [-3.139837],pich: [-0.106960],yaw: [-0.552070]
subscriberは以下のような感じです。
cpp
1#include <ros/ros.h> 2#include <gazebo_msgs/LinkStates.h> 3#include <tf/transform_broadcaster.h> 4#include <tf/transform_datatypes.h> 5using namespace std; 6#include <math.h> 7 8double roll_plt(0),x(0); 9void linkStates_callback(const gazebo_msgs::LinkStates::ConstPtr& msg){ 10 const std::vector<std::string> &names = msg->name; 11 tf::Quaternion q; 12 for(size_t i=0; i<names.size(); ++i){ 13 if(names[i].compare("testrobot_control::upperLegL") == 0){ 14 msg->pose[i]; 15 q =tf::Quaternion(msg->pose[i].orientation.x,msg->pose[i].orientation.y,msg->pose[i].orientation.z,msg->pose[i].orientation.w); 16 break; 17 } 18 } 19 tf::Matrix3x3 m(q); 20 double roll,pitch,yaw; 21 m.getRPY(roll,pitch,yaw); 22 ROS_INFO(" --- roll: [%f],pich: [%f],yaw: [%f]", roll,pitch,yaw); 23} 24 25int main(int argc, char **argv){ 26 ros::init(argc,argv,"joint_subscribe"); 27 ros::NodeHandle nh; 28 ros::Subscriber sub =nh.subscribe("/gazebo/link_states",100,linkStates_callback); 29 vector<double> x(0),y(0); 30 ros::Rate rate(100); 31 while(ros::ok()){ 32 ros::spinOnce(); 33 rate.sleep(); 34 } 35 36 return 0; 37} 38 39
正しい値を得るためにはどうしたらよいでしょうか
おそらく、tf::Quaternionや、getRPY(roll,pitch,yaw);の扱いを間違っているのだと思いますが...
ROSを触り始めて1週間の身ですので何か前提が間違っている可能性は大いにあります。ご了承ください。
足りない情報があればすぐ追加します。
#環境↓
ubuntu: 18.04 LTS ROS: melodic
回答1件
あなたの回答
tips
プレビュー
バッドをするには、ログインかつ
こちらの条件を満たす必要があります。