Feature/init on tf #40
There are no files selected for viewing
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
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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -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); | ||
| if (specify_init_pose) | ||
| { | ||
| init_position.x() = private_nh_.param<double>("init_pos_x", 0.0); | ||
|
|
@@ -46,6 +47,38 @@ void HdlLocalizationNodelet::onInit() | |
| init_orientation.z() = private_nh_.param<double>("init_ori_z", 0.0); | ||
| init_orientation.w() = private_nh_.param<double>("init_ori_w", 1.0); | ||
| } | ||
| else if (init_with_tf_) | ||
| { | ||
| if (!tf_broadcast_ && tf_buffer_.canTransform(global_frame_id_, base_frame_id_, ros::Time(0), ros::Duration(2.0))) | ||
| { | ||
| // If global frame and base frame already exist, initial position is set to keep the relative position | ||
| geometry_msgs::TransformStamped tf_map2base = | ||
| tf_buffer_.lookupTransform(global_frame_id_, base_frame_id_, ros::Time(0)); | ||
| init_position = Eigen::Vector3f(tf_map2base.transform.translation.x, tf_map2base.transform.translation.y, | ||
| tf_map2base.transform.translation.z); | ||
| init_orientation = Eigen::Quaternionf(tf_map2base.transform.rotation.w, tf_map2base.transform.rotation.x, | ||
| tf_map2base.transform.rotation.y, tf_map2base.transform.rotation.z); | ||
| NODELET_INFO("global frame(%s) and base frame(%s) exist, use them for initial pose", global_frame_id_.c_str(), | ||
| base_frame_id_.c_str()); | ||
| } | ||
| else if (use_odom_ && tf_buffer_.canTransform(odom_frame_id_, base_frame_id_, ros::Time(0), ros::Duration(2.0))) | ||
| { | ||
| // If there is no global frame, also check the odom frame | ||
| geometry_msgs::TransformStamped tf_odom2base = | ||
| tf_buffer_.lookupTransform(odom_frame_id_, base_frame_id_, ros::Time(0)); | ||
| init_position = Eigen::Vector3f(tf_odom2base.transform.translation.x, tf_odom2base.transform.translation.y, | ||
| tf_odom2base.transform.translation.z); | ||
| init_orientation = Eigen::Quaternionf(tf_odom2base.transform.rotation.w, tf_odom2base.transform.rotation.x, | ||
| tf_odom2base.transform.rotation.y, tf_odom2base.transform.rotation.z); | ||
| NODELET_INFO("global frame(%s) does not exist, use odom frame(%s) instead", global_frame_id_.c_str(), | ||
| odom_frame_id_.c_str()); | ||
| } | ||
| else | ||
| { | ||
| NODELET_WARN("global frame(%s) and odom frame(%s) do not exist, skip initialization", global_frame_id_.c_str(), | ||
| odom_frame_id_.c_str()); | ||
| } | ||
| } | ||
| // Initialize pose estimator | ||
| NODELET_INFO("initialize pose estimator with specified parameters!!"); | ||
| pose_estimator_.reset( | ||
|
|
@@ -234,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, ros::Time(0), ros::Duration(0.1), | ||
|
|
||
| &tf_error)) | ||
| { | ||
| if (!pcl_ros::transformPointCloud(base_frame_id_, *pcl_cloud, *cloud, this->tf_buffer_)) | ||
| { | ||
| NODELET_ERROR("point cloud cannot be transformed into target frame (%s -> %s)", | ||
| pcl_cloud->header.frame_id.c_str(), base_frame_id_.c_str()); | ||
| return; | ||
| } | ||
| } | ||
|
|
@@ -291,61 +326,17 @@ void HdlLocalizationNodelet::pointsCallback(const sensor_msgs::PointCloud2ConstP | |
| } | ||
| else if (use_odom_) | ||
| { | ||
| // PointClouds + Odometry prediction | ||
| nav_msgs::Odometry odom; | ||
| if (!getOdomFromTf(odom)) | ||
| { | ||
| pose_estimator_->predict(stamp); | ||
| return; | ||
| } | ||
| Eigen::Vector3f odom_twist_linear(odom.twist.twist.linear.x, odom.twist.twist.linear.y, odom.twist.twist.linear.z); | ||
| Eigen::Vector3f odom_twist_angular(odom.twist.twist.angular.x, odom.twist.twist.angular.y, | ||
| odom.twist.twist.angular.z); | ||
| pose_estimator_->predictOdom(odom.header.stamp, odom_twist_linear, odom_twist_angular); | ||
| } | ||
| else | ||
| { | ||
|
|
@@ -460,20 +451,51 @@ bool HdlLocalizationNodelet::relocalize(std_srvs::EmptyRequest& /*req*/, std_srv | |
| void HdlLocalizationNodelet::initialposeCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose_msg) | ||
| { | ||
| NODELET_INFO("initialpose received"); | ||
| // initialize tf_global2rviz on origin | ||
| geometry_msgs::TransformStamped tf_global2rviz; | ||
| tf_global2rviz.transform.translation.x = 0.0; | ||
| tf_global2rviz.transform.translation.y = 0.0; | ||
| tf_global2rviz.transform.translation.z = 0.0; | ||
| tf_global2rviz.transform.rotation.x = 0.0; | ||
| tf_global2rviz.transform.rotation.y = 0.0; | ||
| 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() && | ||
|
|
||
| !pose_msg->header.frame_id.empty()) | ||
| { | ||
| tf_global2rviz = | ||
| tf_buffer_.lookupTransform(global_frame_id_, pose_msg->header.frame_id, ros::Time(0), ros::Duration(2.0)); | ||
| 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
+469
to
+473
|
||
| 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 = | ||
|
Comment on lines
+474
to
+486
There was a problem hiding this comment. tf2_eigen.hのtf2::transformToEigenを使うとメッセージ変換が簡単にできると思います． |
||
| Eigen::Quaternionf(q_global2robot.w(), q_global2robot.x(), q_global2robot.y(), q_global2robot.z()); | ||
| if (use_odom_) | ||
| { | ||
| resetTfBuffer(); | ||
| } | ||
| if (use_imu_) | ||
| { | ||
| imu_data_.clear(); | ||
| } | ||
| std::lock_guard<std::mutex> lock(pose_estimator_mutex_); | ||
| pose_estimator_.reset( | ||
| new hdl_localization::PoseEstimator(registration_, init_position, init_orientation, cool_time_duration_)); | ||
| } | ||
|
|
||
| /** | ||
|
|
@@ -643,6 +665,89 @@ void HdlLocalizationNodelet::publishScanMatchingStatus(const std_msgs::Header& h | |
| status_pub_.publish(status); | ||
| } | ||
|
|
||
| bool HdlLocalizationNodelet::getOdomFromTf(nav_msgs::Odometry& odom_out) | ||
| { | ||
| double initialize_timeout = 0.1; | ||
| if (odom_frame_id_.empty() || base_frame_id_.empty()) | ||
| { | ||
| NODELET_WARN_THROTTLE(1.0, "odom_frame_id(%s) or base_frame_id(%s) is not set", odom_frame_id_.c_str(), | ||
| base_frame_id_.c_str()); | ||
| return false; | ||
| } | ||
| if (tf_buffer_.canTransform(base_frame_id_, odom_stamp_last_, base_frame_id_, ros::Time(0), odom_frame_id_, | ||
| ros::Duration(initialize_timeout)) && | ||
| tf_buffer_.canTransform(odom_frame_id_, base_frame_id_, ros::Time(0), ros::Duration(initialize_timeout))) | ||
| { | ||
| // Get the amount of odometry movement since the last calculation | ||
| // Coordinate system where the front of the robot is x | ||
| geometry_msgs::TransformStamped odom_delta = | ||
| tf_buffer_.lookupTransform(base_frame_id_, odom_stamp_last_, base_frame_id_, ros::Time(0), odom_frame_id_); | ||
| // Get the latest base_frame_ to get the time | ||
| geometry_msgs::TransformStamped odom_now = tf_buffer_.lookupTransform(odom_frame_id_, base_frame_id_, ros::Time(0)); | ||
| ros::Time odom_stamp = odom_now.header.stamp; | ||
| double odom_time_diff = (odom_stamp - odom_stamp_last_).toSec(); | ||
| if (odom_delta.header.stamp.isZero() || odom_time_diff < std::numeric_limits<double>::epsilon()) | ||
| { | ||
| NODELET_WARN("Wrong timestamp detected: odom_time_diff = %f", odom_time_diff); | ||
| odom_stamp_last_ = odom_stamp; | ||
| return false; | ||
| } | ||
| else | ||
| { | ||
| Eigen::Vector3f odom_travel_linear(odom_delta.transform.translation.x, odom_delta.transform.translation.y, | ||
| odom_delta.transform.translation.z); | ||
| Eigen::Vector3f odom_twist_linear = odom_travel_linear / odom_time_diff; | ||
| tf::Quaternion odom_travel_angular(odom_delta.transform.rotation.x, odom_delta.transform.rotation.y, | ||
| odom_delta.transform.rotation.z, odom_delta.transform.rotation.w); | ||
| double roll, pitch, yaw; | ||
| tf::Matrix3x3(odom_travel_angular).getRPY(roll, pitch, yaw); | ||
| Eigen::Vector3f odom_twist_angular(roll / odom_time_diff, pitch / odom_time_diff, yaw / odom_time_diff); | ||
| odom_out.header.stamp = odom_stamp; | ||
| odom_out.header.frame_id = odom_frame_id_; | ||
| odom_out.child_frame_id = base_frame_id_; | ||
| odom_out.pose.pose.position.x = odom_now.transform.translation.x; | ||
| odom_out.pose.pose.position.y = odom_now.transform.translation.y; | ||
| odom_out.pose.pose.position.z = odom_now.transform.translation.z; | ||
| odom_out.pose.pose.orientation.x = odom_now.transform.rotation.x; | ||
| odom_out.pose.pose.orientation.y = odom_now.transform.rotation.y; | ||
| odom_out.pose.pose.orientation.z = odom_now.transform.rotation.z; | ||
| odom_out.pose.pose.orientation.w = odom_now.transform.rotation.w; | ||
| odom_out.twist.twist.linear.x = odom_twist_linear.x(); | ||
| odom_out.twist.twist.linear.y = odom_twist_linear.y(); | ||
| odom_out.twist.twist.linear.z = odom_twist_linear.z(); | ||
| odom_out.twist.twist.angular.x = odom_twist_angular.x(); | ||
| odom_out.twist.twist.angular.y = odom_twist_angular.y(); | ||
| odom_out.twist.twist.angular.z = odom_twist_angular.z(); | ||
| } | ||
| odom_stamp_last_ = odom_stamp; | ||
| return true; | ||
| } | ||
| else | ||
| { | ||
| if (tf_buffer_.canTransform(odom_frame_id_, base_frame_id_, ros::Time(0), ros::Duration(initialize_timeout))) | ||
| { | ||
| NODELET_INFO("Received the first odom transform"); | ||
| // Get the latest base_frame_ to get the time | ||
| geometry_msgs::TransformStamped odom_now = | ||
| tf_buffer_.lookupTransform(odom_frame_id_, base_frame_id_, ros::Time(0)); | ||
| odom_stamp_last_ = odom_now.header.stamp; | ||
| return false; | ||
| } | ||
| else | ||
| { | ||
| NODELET_WARN_STREAM("Failed to look up transform between " << odom_frame_id_ << " and " << odom_frame_id_); | ||
| return false; | ||
| } | ||
| } | ||
| return false; | ||
| } | ||
|
|
||
| void HdlLocalizationNodelet::resetTfBuffer() | ||
| { | ||
| tf_buffer_.clear(); | ||
| odom_stamp_last_ = ros::Time(0); | ||
| } | ||
|
|
||
| } // namespace hdl_localization | ||
|
|
||
| PLUGINLIB_EXPORT_CLASS(hdl_localization::HdlLocalizationNodelet, nodelet::Nodelet) | ||
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
init_with_tf_がfalseになるとどうなりますか？