Feature/init on tf #40
Draft
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
nyxrobotics
force-pushed
the
feature/init_on_tf
branch
from
c9adf10 to
0b1be75
Compare
jsupratman13
requested changes
Feb 14, 2024
| @@ -267,11 +267,13 @@ void HdlLocalizationNodelet::pointsCallback(const sensor_msgs::PointCloud2ConstP | |||
| // transform pointcloud into base_frame_id | |||
| std::string tf_error; | |||
| pcl::PointCloud<HdlLocalizationNodelet::PointT>::Ptr cloud(new pcl::PointCloud<HdlLocalizationNodelet::PointT>()); | |||
| if (this->tf_buffer_.canTransform(base_frame_id_, pcl_cloud->header.frame_id, stamp, ros::Duration(0.1), &tf_error)) | |||
| if (this->tf_buffer_.canTransform(base_frame_id_, pcl_cloud->header.frame_id, ros::Time(0), ros::Duration(0.1), | |||
| tf_global2rviz.transform.rotation.z = 0.0; | ||
| tf_global2rviz.transform.rotation.w = 1.0; | ||
| // If the origin of rviz and the global(map) frame are different, coordinate transformation is performed | ||
| if (init_with_tf_ && global_frame_id_ != pose_msg->header.frame_id && !global_frame_id_.empty() && |
Comment on lines
+469
to
+473
| if (tf_global2rviz.header.frame_id != global_frame_id_) | ||
| { | ||
| ROS_ERROR("[Failed to get transform from %s to %s", global_frame_id_.c_str(), pose_msg->header.frame_id.c_str()); | ||
| } | ||
| } |
Comment on lines
+474
to
+486
| tf2::Vector3 vec_rviz2robot(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, | ||
| pose_msg->pose.pose.position.z); | ||
| tf2::Quaternion q_rviz2robot(pose_msg->pose.pose.orientation.x, pose_msg->pose.pose.orientation.y, | ||
| pose_msg->pose.pose.orientation.z, pose_msg->pose.pose.orientation.w); | ||
| // Get initial pose from global to robot | ||
| tf2::Vector3 vec_global2rviz(tf_global2rviz.transform.translation.x, tf_global2rviz.transform.translation.y, | ||
| tf_global2rviz.transform.translation.z); | ||
| tf2::Quaternion q_global2rviz(tf_global2rviz.transform.rotation.x, tf_global2rviz.transform.rotation.y, | ||
| tf_global2rviz.transform.rotation.z, tf_global2rviz.transform.rotation.w); | ||
| tf2::Vector3 vec_global2robot = vec_global2rviz + (tf2::Matrix3x3(q_global2rviz) * vec_rviz2robot); | ||
| tf2::Quaternion q_global2robot = q_global2rviz * q_rviz2robot; | ||
| Eigen::Vector3f init_position = Eigen::Vector3f(vec_global2robot.x(), vec_global2robot.y(), vec_global2robot.z()); | ||
| Eigen::Quaternionf init_orientation = |
There was a problem hiding this comment.
tf2_eigen.hのtf2::transformToEigenを使うとメッセージ変換が簡単にできると思います．
| @@ -36,6 +36,7 @@ void HdlLocalizationNodelet::onInit() | |||
| bool specify_init_pose = private_nh_.param<bool>("specify_init_pose", false); | |||
| Eigen::Vector3f init_position = Eigen::Vector3f::Zero(); | |||
| Eigen::Quaternionf init_orientation = Eigen::Quaternionf::Identity(); | |||
| init_with_tf_ = private_nh_.param<bool>("init_with_tf", true); | |||
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Summary
rvizでinitial_poseのframe_idがmapでない場合mapへ座標変換
init_with_tfがtrueのときbase_linkのTFの位置を原点にしてスキャンマッチングを開始
fix Rvizでmap以外のフレームを原点にinitialposeを設定すると初期位置が誤った位置になる #37
fix robot_localizationと組み合わせる際に「現在のTFの関係」を参照して初期位置に設定したい #38
Attention