C言語初心者です
rtimulib_rosというパッケージ内についてお聞きしたいです。
リンク内容
srcフォルダにこのプログラムがあるのですが
意味が理解できないところがあります コメントはほぼ自分の書き込みです。
//??をつけている所の処理や意味がよくわかりませんので教えていただきたいです。
またコメント書いてあるところも間違いがあれば教えていただけると嬉しいです。
<補足>
出来ればプログラムがどういう動きをしているか、プログラムの意味を教えていただきたいです。
センサ:MPU9250
最後のほうだけ少しプログラム追加しています。
言葉が変なところが少なからずあると思いますが よろしければお願いします。
#include <RTIMULib2/RTIMULib/RTIMULib.h //RTIMULib用ヘッダ #include <ros/ros.h> //ros用ヘッダ #include <sensor_msgs/Imu.h> //センサ関連のヘッダ #include <tf/tf.h> #include <tf/transform_datatypes.h> //座標変換のためのヘッダ static const double G_TO_MPSS = 9.80665; //?? int main(int argc, char **argv) { ros::init(argc, argv, "rtimulib_node"); //このノード名がrtimulib_nodeであるという意味 ROS_INFO("Imu driver is now running"); //ROS INFOログ表示 ros::NodeHandle nh("~"); //ノードハンドラの宣言 ~空間にnh格納 std::string calibration_file_path; //calibration file pathというローカル変数を生成 if(!nh.getParam("calibration_file_path", calibration_file_path)) { ROS_ERROR("The calibration_file_path parameter must be set to use a " "calibration file."); ROS_BREAK(); } //もしcalibration_file_pathのパラメータ獲得しなかった場合 //ROS_ERROR上記のメッセージをログ表示 std::string calibration_file_name = "RTIMULib"; //calibration_file_nameというローカル変数を生成しRTIMULibとする if(!nh.getParam("calibration_file_name", calibration_file_name)) { ROS_WARN_STREAM("No calibration_file_name provided - default: " << calibration_file_name); } //ストリームフォーマットログ表示 ?? std::string frame_id = "imu_link"; //frame_idというローカル変数を生成しimu_linkとする if(!nh.getParam("frame_id", frame_id)) { ROS_WARN_STREAM("No frame_id provided - default: " << frame_id); } //ストリームフォーマットログ表示 ?? ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("imu", 1); //?? // Load the RTIMULib.ini config file RTIMUSettings *settings = new RTIMUSettings(calibration_file_path.c_str(), calibration_file_name.c_str()); RTIMU *imu = RTIMU::createIMU(settings); //上の4行の意味がよくわかってません ?? if ((imu == NULL) || (imu->IMUType() == RTIMU_TYPE_NULL)) { ROS_ERROR("No Imu found"); ROS_BREAK(); } //もしimuがNULL(空)もしくはIMUTypeがRTIMU_TIPE_NULLなら //ROS_ERRORログ表示 imu->IMUInit(); // IMUInit初期化 imu->setSlerpPower(0.02); //?? imu->setGyroEnable(true); imu->setAccelEnable(true); imu->setCompassEnable(true); //センサを利用可能に 各メンバがtrue sensor_msgs::Imu imu_msg; //Imu空間内にimu_msgを生成 while (ros::ok()) { if (imu->IMURead()) { RTIMU_DATA imu_data = imu->getIMUData(); //?? imu_msg.header.stamp = ros::Time::now(); //nowをimu_msg.header.stampに代入 imu_msg.header.frame_id = frame_id; //frame_idをimu_msg.header.frameに代入 imu_msg.orientation.x = imu_data.fusionQPose.x(); imu_msg.orientation.y = imu_data.fusionQPose.y(); imu_msg.orientation.z = imu_data.fusionQPose.z(); imu_msg.orientation.w = imu_data.fusionQPose.scalar(); //imu_data.fusionQPoseをimu_msg.orientationに代入 tf::Quaternion q(imu_msg.orientation.x, imu_msg.orientation.y , imu_msg.orientation.z , imu_msg.orientation.w); //Quatetion qにimu_msg.orientation代入 tf::Matrix3x3 m(q); double roll, pitch, yaw; m.getRPY(roll, pitch, yaw); //roll pitch yaw演算 printf("%f %f %f", roll, pitch, yaw); //ノード立ち上げ時出力 imu_msg.angular_velocity.x = imu_data.gyro.x(); imu_msg.angular_velocity.y = imu_data.gyro.y(); imu_msg.angular_velocity.z = imu_data.gyro.z(); imu_msg.linear_acceleration.x = imu_data.accel.x() * G_TO_MPSS; imu_msg.linear_acceleration.y = imu_data.accel.y() * G_TO_MPSS; imu_msg.linear_acceleration.z = imu_data.accel.z() * G_TO_MPSS; //同じく代入 imu_pub.publish(imu_msg); //トピックにimu_msgを配信 } ros::spinOnce(); ros::Duration(imu->IMUGetPollInterval() / 1000.0).sleep(); } return 0; }
回答1件
あなたの回答
tips
プレビュー
バッドをするには、ログインかつ
こちらの条件を満たす必要があります。
2018/10/05 04:18
2018/10/05 04:27
2018/10/06 15:18