I studied Hector_trajectory_server code in Hector SLAM. And I would like to derive curr_coords, the coordinates of the current Lidar. However, if you run Hector SLAM and create a map in the rviz, only the Hector_trajectory_server node will appear. How can the curr_coords coordinates in this cpp file be imported into nodes? I know how to make a node in the main sentence, but it's too difficult to make a node here, so I leave a question. Thank you.

//================================================================================================= // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt // All rights reserved.

// Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // * Neither the name of the Simulation, Systems Optimization and Robotics // group, TU Darmstadt nor the names of its contributors may be used to // endorse or promote products derived from this software without // specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //=================================================================================================

include <cstdio>

include "ros/ros.h"

include "ros/console.h"

include "nav_msgs/Path.h"

include "std_msgs/String.h"

include <geometry_msgs quaternion.h="">

include <geometry_msgs posestamped.h="">

include "tf/transform_listener.h"

include <hector_nav_msgs getrobottrajectory.h="">

include <hector_nav_msgs getrecoveryinfo.h="">

include <tf tf.h="">

include <algorithm>

using namespace std;

bool comparePoseStampedStamps (const geometry_msgs::PoseStamped& t1, const geometry_msgs::PoseStamped& t2) { return (t1.header.stamp < t2.header.stamp); }

/** * @brief Map generation node. */ class PathContainer { public: PathContainer() { ros::NodeHandle private_nh("~");

private_nh.param("target_frame_name", p_target_frame_name_, std::string("map"));
private_nh.param("source_frame_name", p_source_frame_name_, std::string("base_link"));
private_nh.param("trajectory_update_rate", p_trajectory_update_rate_, 4.0);
private_nh.param("trajectory_publish_rate", p_trajectory_publish_rate_, 0.25);

waitForTf();

ros::NodeHandle nh;
sys_cmd_sub_ = nh.subscribe("syscommand", 1, &PathContainer::sysCmdCallback, this);
trajectory_pub_ = nh.advertise<nav_msgs::Path>("trajectory",1, true);

trajectory_provider_service_ = nh.advertiseService("trajectory", &PathContainer::trajectoryProviderCallBack, this);
recovery_info_provider_service_ = nh.advertiseService("trajectory_recovery_info", &PathContainer::recoveryInfoProviderCallBack, this);

last_reset_time_ = ros::Time::now();

update_trajectory_timer_ = private_nh.createTimer(ros::Duration(1.0 / p_trajectory_update_rate_), &PathContainer::trajectoryUpdateTimerCallback, this, false);
publish_trajectory_timer_ = private_nh.createTimer(ros::Duration(1.0 / p_trajectory_publish_rate_), &PathContainer::publishTrajectoryTimerCallback, this, false);

pose_source_.pose.orientation.w = 1.0;
pose_source_.header.frame_id = p_source_frame_name_;

trajectory_.trajectory.header.frame_id = p_target_frame_name_;

}

void waitForTf() { ros::Time start = ros::Time::now(); ROS_INFO ... (more)