Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Parameter to specify odom target frame #39

Merged
merged 1 commit into from
Apr 6, 2020
Merged

Parameter to specify odom target frame #39

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 18 additions & 5 deletions apps/hdl_localization_nodelet.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_broadcaster.h>

Expand Down Expand Up @@ -42,6 +43,8 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {

processing_time.resize(16);
initialize_params();

odom_child_frame_id = private_nh.param<std::string>("odom_child_frame_id", "base_link");

use_imu = private_nh.param<bool>("use_imu", true);
invert_imu = private_nh.param<bool>("invert_imu", false);
Expand Down Expand Up @@ -127,14 +130,21 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
}

const auto& stamp = points_msg->header.stamp;
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
pcl::fromROSMsg(*points_msg, *cloud);
pcl::PointCloud<PointT>::Ptr pcl_cloud(new pcl::PointCloud<PointT>());
pcl::fromROSMsg(*points_msg, *pcl_cloud);

if(cloud->empty()) {
if(pcl_cloud->empty()) {
NODELET_ERROR("cloud is empty!!");
return;
}

// transform pointcloud into odom_child_frame_id
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
if(!pcl_ros::transformPointCloud(odom_child_frame_id, *pcl_cloud, *cloud, this->tf_listener)) {
NODELET_ERROR("point cloud cannot be transformed into target frame!!");
return;
}

auto filtered = downsample(cloud);

// predict
Expand Down Expand Up @@ -230,7 +240,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
*/
void publish_odometry(const ros::Time& stamp, const Eigen::Matrix4f& pose) {
// broadcast the transform over tf
geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, "map", "velodyne");
geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, "map", odom_child_frame_id);
pose_broadcaster.sendTransform(odom_trans);

// publish the transform
Expand All @@ -243,7 +253,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
odom.pose.pose.position.z = pose(2, 3);
odom.pose.pose.orientation = odom_trans.transform.rotation;

odom.child_frame_id = "velodyne";
odom.child_frame_id = odom_child_frame_id;
odom.twist.twist.linear.x = 0.0;
odom.twist.twist.linear.y = 0.0;
odom.twist.twist.angular.z = 0.0;
Expand Down Expand Up @@ -286,6 +296,8 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
ros::NodeHandle nh;
ros::NodeHandle mt_nh;
ros::NodeHandle private_nh;

std::string odom_child_frame_id;

bool use_imu;
bool invert_imu;
Expand All @@ -297,6 +309,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
ros::Publisher pose_pub;
ros::Publisher aligned_pub;
tf::TransformBroadcaster pose_broadcaster;
tf::TransformListener tf_listener;

// imu input buffer
std::mutex imu_data_mutex;
Expand Down
7 changes: 7 additions & 0 deletions launch/hdl_localization.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,9 @@
<launch>
<!-- arguments -->
<arg name="nodelet_manager" default="velodyne_nodelet_manager" />
<arg name="points_topic" default="/velodyne_points" />
<arg name="imu_topic" default="/gpsimu_driver/imu_data" />
<arg name="odom_child_frame_id" default="velodyne" />

<!-- in case you use velodyne_driver, comment out the following line -->
<node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/>
Expand All @@ -14,6 +17,10 @@

<!-- hdl_localization_nodelet -->
<node pkg="nodelet" type="nodelet" name="hdl_localization_nodelet" args="load hdl_localization/HdlLocalizationNodelet $(arg nodelet_manager)">
<remap from="/velodyne_points" to="$(arg points_topic)" />
<remap from="/gpsimu_driver/imu_data" to="$(arg imu_topic)" />
<!-- odometry frame_id -->
<param name="odom_child_frame_id" value="$(arg odom_child_frame_id)" />
<!-- imu settings -->
<!-- during "cool_time", imu inputs are ignored -->
<param name="use_imu" value="true" />
Expand Down