Skip to content
This repository is currently being migrated. It's locked while the migration is in progress.

Commit

Permalink
tf2-fix
Browse files Browse the repository at this point in the history
  • Loading branch information
borongyuan committed Mar 25, 2021
1 parent f42e722 commit 91266af
Show file tree
Hide file tree
Showing 2 changed files with 7 additions and 7 deletions.
2 changes: 1 addition & 1 deletion rviz_cfg/loam_livox.rviz
Original file line number Diff line number Diff line change
Expand Up @@ -484,7 +484,7 @@ Visualization Manager:
Global Options:
Background Color: 0; 0; 0
Default Light: true
Fixed Frame: /camera_init
Fixed Frame: camera_init
Frame Rate: 30
Name: root
Tools:
Expand Down
12 changes: 6 additions & 6 deletions src/laserMapping.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -402,8 +402,8 @@ int main(int argc, char** argv)

ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> ("/aft_mapped_to_init", 1);
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.frame_id = "camera_init";
odomAftMapped.child_frame_id = "aft_mapped";

std::string map_file_path;
ros::param::get("~map_file_path",map_file_path);
Expand Down Expand Up @@ -1117,13 +1117,13 @@ int main(int argc, char** argv)
sensor_msgs::PointCloud2 laserCloudSurround3;
pcl::toROSMsg(*laserCloudSurround2, laserCloudSurround3);
laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserCloudCornerLast);
laserCloudSurround3.header.frame_id = "/camera_init";
laserCloudSurround3.header.frame_id = "camera_init";
pubLaserCloudSurround.publish(laserCloudSurround3);

sensor_msgs::PointCloud2 laserCloudSurround3_corner;
pcl::toROSMsg(*laserCloudSurround2_corner, laserCloudSurround3_corner);
laserCloudSurround3_corner.header.stamp = ros::Time().fromSec(timeLaserCloudCornerLast);
laserCloudSurround3_corner.header.frame_id = "/camera_init";
laserCloudSurround3_corner.header.frame_id = "camera_init";
pubLaserCloudSurround_corner.publish(laserCloudSurround3_corner);


Expand All @@ -1141,7 +1141,7 @@ int main(int argc, char** argv)
sensor_msgs::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg(*laserCloudFullResColor, laserCloudFullRes3);
laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserCloudCornerLast);
laserCloudFullRes3.header.frame_id = "/camera_init";
laserCloudFullRes3.header.frame_id = "camera_init";
pubLaserCloudFullRes.publish(laserCloudFullRes3);

*laserCloudFullResColor_pcd += *laserCloudFullResColor;
Expand Down Expand Up @@ -1171,7 +1171,7 @@ int main(int argc, char** argv)
q.setY( odomAftMapped.pose.pose.orientation.y );
q.setZ( odomAftMapped.pose.pose.orientation.z );
transform.setRotation( q );
br.sendTransform( tf::StampedTransform( transform, odomAftMapped.header.stamp, "/camera_init", "/aft_mapped" ) );
br.sendTransform( tf::StampedTransform( transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped" ) );

kfNum++;

Expand Down

0 comments on commit 91266af

Please sign in to comment.