rviz上で、ロボットを3次元で動かしたい。

Question

前提・実現したいこと

取得した/cmd_velのノードを使って、rviz上でロボットを3次元で動かしたいです。

該当のソースコード

以下のソースコードは
https://github.com/project-srs/ros_lecture/tree/master/vis_lecture
から拝借したものです。なお、以下のソースコードはvis_robot_sim.cppです。

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose2D.h>
#include <tf/transform_broadcaster.h>

#include <string>
#include <math.h>

void robot_pose_publish(geometry_msgs::Pose2D pose)
{
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin(tf::Vector3(pose.x, pose.y, 0.0));
  tf::Quaternion q;
  q.setRPY(0, 0, pose.theta);
  transform.setRotation(q);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "base_link"));
}

geometry_msgs::Pose2D robot_tick(geometry_msgs::Pose2D pose, geometry_msgs::Twist cmd_vel, float dt)
{
  geometry_msgs::Pose2D output;
  float tmp_rz = pose.theta + cmd_vel.angular.z * dt / 2;
  output.x = pose.x + (cos(tmp_rz) * cmd_vel.linear.x - sin(tmp_rz) * cmd_vel.linear.y) * dt;
  output.y = pose.y + (sin(tmp_rz) * cmd_vel.linear.x + cos(tmp_rz) * cmd_vel.linear.y) * dt;
  output.theta = pose.theta + cmd_vel.angular.z * dt;
  return output;
}

geometry_msgs::Twist twist_last;
void twist_callback(const geometry_msgs::Twist& twist_msg)
{
  twist_last = twist_msg;
}

float normal_rad(float value)
{
  if (value > 0)
    return fmod(value, 3.1415);
  else
    return -fmod(-value, 3.1415);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "vis_robot_sim");
  ros::NodeHandle nh;
  ros::Subscriber twist_sub = nh.subscribe("/cmd_vel", 10, twist_callback);
  geometry_msgs::Pose2D pose;

  ros::Rate loop_rate(20);
  while (ros::ok())
  {
    pose = robot_tick(pose, twist_last, 0.05);
    robot_pose_publish(pose);
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}

試したこと

transform.setOrigin(tf::Vector3(pose.x, pose.y, pose.z));

といったようにpose.zを追加してみたところ

error: ‘geometry_msgs::Pose2D {aka struct geometry_msgs::Pose2D_<std::allocator<void> >}’ has no member named ‘z’

とエラーメッセージが帰ってきます。
C++言語は初心者ですので、ご指導をよろしくお願いいたします。

Answer 1

エラー文を読んでいると、**'geometry_msgs::Pose2Dには 'z' という名前のメンバがありません。**と言われております。

transform.setOrigin(tf::Vector3(pose.x, pose.y, pose.z));

ここが三次元なのに対して、Pose2Dは名前的に2次元なので次元が違います。

Comments

[Eshia]

回答ありがとうございます。
Pose2Dではない、例えばPoseStampedなどにするのだろうとは思っており、
実際にPoseStampedなどにしても、名前のメンバがありませんと返ってきます。
できれば、PoseStampedなどに変更した際に、名前を追加する方法も教えていただきたいです。

[stdio]

名前のメンバがありませんというのは、「Pose2Dの構造体？の中にzという変数（メンバー）がありません。」というエラーになります。
貴方は、robot_pose_publishという関数の中で既にその解決方法を行っているように私の眼には見えます。

[Eshia]

「貴方は、robot_pose_publishという関数の中で既にその解決方法を行っている」とありますが、具体的にどこでしょうか。このソースコードは私が作ったものではない上、C++は全然わかってないので教えてください。
stdioさんは、ご存知と思いますが、Pose2Dは「x,y,theta」がもともと用意されていて、2次元のパラメータしかなく、もちろんzという変数がないのも理解しています。

[Eshia]

stdioさん、なんとかできました！
double z, roll , pitch, yaw;
を追加して、
transform.setOrigin(tf::Vector3(pose.x, pose.y, z));
q.setRPY(roll, pitch, yaw);
に変更してできました。
stdioさん、先日はありがとうございました。

[stdio]

> このソースコードは私が作ったものではない上、C++は全然わかってないので教えてください。

お節介かも知れませんが、そういうことは今後は言わない方がいいですよ。
自分でコードを提示する以上、そのコードは理解しているものだと回答者には判断されてしまいます。

[Eshia]

stdioさん、ご忠告ありがとうございます。
書かないようにいたします。
コードがまだ読み切れない節があるため、指導も兼ねて今回質問させていただきましたが、良くないのですね。勉強に励みます。