Commit 92589d0d authored by Maurice Fallon's avatar Maurice Fallon
Browse files

methods of publish output

parent 92d17c3e
......@@ -39,6 +39,20 @@ public:
std::string name,
int64_t utime = -1) = 0;
virtual void publishOdomPoses(Eigen::Isometry3d pose_,
int param,
std::string name,
int64_t utime = -1) = 0;
virtual void publishPriorPoses(Eigen::Isometry3d pose_,
int param,
std::string name,
int64_t utime = -1) = 0;
virtual void publishOdomToMapPose(Eigen::Isometry3d pose_,
int64_t utime = -1) = 0;
// Gets
virtual const PathPoses& getPath() = 0;
......
......@@ -51,6 +51,19 @@ public:
void publishPoses(PathPoses poses,
int param, std::string name, int64_t utime);
void publishOdomPoses(Eigen::Isometry3d pose,
int param, std::string name, int64_t utime);
void publishOdomPoses(PathPoses poses,
int param, std::string name, int64_t utime);
void publishPriorPoses(Eigen::Isometry3d pose,
int param, std::string name, int64_t utime);
void publishPriorPoses(PathPoses poses,
int param, std::string name, int64_t utime);
void publishOdomToMapPose(Eigen::Isometry3d pose, int64_t utime);
// Publish tf from fixed_frame to odom
void publishFixedFrameToOdomTF(const Eigen::Isometry3d& fixed_frame_to_base_eigen,
ros::Time msg_time);
......@@ -68,11 +81,18 @@ private:
ros::Publisher prior_map_pub_;
ros::Publisher aligned_map_pub_;
ros::Publisher pose_pub_;
ros::Publisher odom_pose_pub_;
ros::Publisher prior_pose_pub_;
ros::Publisher fixed_to_odom_pub_;
ros::Publisher odom_to_map_pub_;
// Duplicates the list in collections renderer. assumed to be 3xN colors
std::vector<double> colors_;
// Path (vector of poses)
PathPoses path_;
PathPoses odom_path_;
PathPoses prior_path_;
std::string fixed_frame_; // map or map_test
std::string odom_frame_;
......
......@@ -18,7 +18,13 @@ ROSVisualizer::ROSVisualizer(ros::NodeHandle& nh, string fixed_frame) : nh_(nh),
prior_map_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/aicp/prior_map", 10);
aligned_map_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/aicp/aligned_map", 10);
pose_pub_ = nh_.advertise<nav_msgs::Path>("/aicp/poses",100);
odom_pose_pub_ = nh_.advertise<nav_msgs::Path>("/aicp/odom_poses",100);
prior_pose_pub_ = nh_.advertise<nav_msgs::Path>("/aicp/prior_poses",100);
fixed_to_odom_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>(fixed_to_odom_prefix_ + fixed_frame_ + "_to_odom", 10);
odom_to_map_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("/icp_tools/map_pose", 10);
colors_ = {
51/255.0, 160/255.0, 44/255.0, //0
166/255.0, 206/255.0, 227/255.0,
......@@ -153,6 +159,72 @@ void ROSVisualizer::publishPoses(PathPoses poses, int param, string name, int64_
pose_pub_.publish(path_msg);
}
void ROSVisualizer::publishOdomPoses(Eigen::Isometry3d pose, int param, std::string name, int64_t utime)
{
odom_path_.push_back(pose);
publishOdomPoses(odom_path_, param, name, utime);
}
void ROSVisualizer::publishOdomPoses(PathPoses poses, int param, string name, int64_t utime){
nav_msgs::Path path_msg;
int secs = utime*1E-6;
int nsecs = (utime - (secs*1E6))*1E3;
path_msg.header.stamp = ros::Time(secs, nsecs);
path_msg.header.frame_id = fixed_frame_;
for (size_t i = 0; i < poses.size(); ++i){
geometry_msgs::PoseStamped m;
m.header.stamp = ros::Time(secs, nsecs);
m.header.frame_id = fixed_frame_;
tf::poseEigenToMsg(poses[i], m.pose);
path_msg.poses.push_back(m);
}
odom_pose_pub_.publish(path_msg);
}
void ROSVisualizer::publishPriorPoses(Eigen::Isometry3d pose, int param, std::string name, int64_t utime)
{
prior_path_.push_back(pose);
publishPriorPoses(odom_path_, param, name, utime);
}
void ROSVisualizer::publishPriorPoses(PathPoses poses, int param, string name, int64_t utime){
nav_msgs::Path path_msg;
int secs = utime*1E-6;
int nsecs = (utime - (secs*1E6))*1E3;
path_msg.header.stamp = ros::Time(secs, nsecs);
path_msg.header.frame_id = fixed_frame_;
for (size_t i = 0; i < poses.size(); ++i){
geometry_msgs::PoseStamped m;
m.header.stamp = ros::Time(secs, nsecs);
m.header.frame_id = fixed_frame_;
tf::poseEigenToMsg(poses[i], m.pose);
path_msg.poses.push_back(m);
}
prior_pose_pub_.publish(path_msg);
}
void ROSVisualizer::publishOdomToMapPose(Eigen::Isometry3d pose, int64_t utime)
{
int secs = utime*1E-6;
int nsecs = (utime - (secs*1E6))*1E3;
geometry_msgs::PoseWithCovarianceStamped msg;
msg.header.stamp = ros::Time(secs, nsecs);
msg.header.frame_id = "odom";
tf::poseEigenToMsg(pose, msg.pose.pose);
odom_to_map_pub_.publish(msg);
}
void ROSVisualizer::publishFixedFrameToOdomPose(const Eigen::Isometry3d &fixed_frame_to_base_eigen,
ros::Time msg_time)
{
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment