diff --git a/include/publisher_graph.h b/include/publisher_graph.h
index 285dd4f79659ea85b40dba835dd9d41d68f47cbd..7b1ceb04dc469452307c9d49e15a26334e37e782 100644
--- a/include/publisher_graph.h
+++ b/include/publisher_graph.h
@@ -68,7 +68,7 @@ class PublisherGraph: public Publisher
std::string map_frame_id_;
bool viz_overlapped_factors_;
double viz_scale_, text_scale_, factors_width_, factors_absolute_height_, landmark_text_z_offset_, landmark_width_, landmark_length_, frame_width_, frame_length_;
- std_msgs::ColorRGBA frame_color_, factor_abs_color_, factor_motion_color_, factor_loop_color_, factor_lmk_color_, factor_geom_color_;
+ std_msgs::ColorRGBA frame_color_, factor_abs_color_, factor_motion_color_, factor_loop_color_, factor_lmk_color_, factor_geom_color_, factor_other_color_;
// auxiliar variables
unsigned int landmark_max_hits_;
diff --git a/include/publisher_tf.h b/include/publisher_tf.h
index 64740385df04674c289e06cc6940a6bb4fc04b26..d9f01bc452f0155fdbf2f5c11d847f1e21de7a6f 100644
--- a/include/publisher_tf.h
+++ b/include/publisher_tf.h
@@ -18,6 +18,22 @@
namespace wolf
{
+tf::Transform stateToTfTransform(VectorComposite state, int dim)
+{
+ // 2D
+ if (dim == 2)
+ {
+ return tf::Transform (tf::createQuaternionFromYaw(state['O'](0)),
+ tf::Vector3(state['P'](0), state['P'](1), 0) );
+ }
+ // 3D
+ else
+ {
+ return tf::Transform (tf::Quaternion(state['O'](0), state['O'](1), state['O'](2), state['O'](3)),
+ tf::Vector3(state['P'](0), state['P'](1), state['P'](2)) );
+ }
+}
+
class PublisherTf: public Publisher
{
protected:
@@ -25,6 +41,9 @@ class PublisherTf: public Publisher
tf::TransformBroadcaster tfb_;
tf::TransformListener tfl_;
+ tf::StampedTransform T_odom2base_, T_map2odom_;
+
+ bool publish_odom_tf_;
bool state_available_; // used to not repeat warnings regarding availability of state
public:
diff --git a/src/publisher_graph.cpp b/src/publisher_graph.cpp
index 04bc5540116b6c2fe6fdff50cec384d53aaa6b78..e81628f1aa134a7619104983e0ea966fde2c5b27 100644
--- a/src/publisher_graph.cpp
+++ b/src/publisher_graph.cpp
@@ -33,26 +33,30 @@ PublisherGraph::PublisherGraph(const std::string& _unique_name,
// factors
factors_width_ = getParamWithDefault<double> (_server, prefix_ + "/factors_width", 0.02);
factors_absolute_height_= getParamWithDefault<double> (_server, prefix_ + "/factors_absolute_height", 2);
- factor_abs_color_.r = getParamWithDefault<double> (_server, prefix_ + "/factor_abs_color_r", 0.9);
- factor_abs_color_.g = getParamWithDefault<double> (_server, prefix_ + "/factor_abs_color_g", 0.2);
- factor_abs_color_.b = getParamWithDefault<double> (_server, prefix_ + "/factor_abs_color_b", 0.6);
+ factor_abs_color_.r = getParamWithDefault<double> (_server, prefix_ + "/factor_abs_color_r", 1); //red
+ factor_abs_color_.g = getParamWithDefault<double> (_server, prefix_ + "/factor_abs_color_g", 0);
+ factor_abs_color_.b = getParamWithDefault<double> (_server, prefix_ + "/factor_abs_color_b", 0);
factor_abs_color_.a = getParamWithDefault<double> (_server, prefix_ + "/factor_abs_color_a", 1);
- factor_motion_color_.r = getParamWithDefault<double> (_server, prefix_ + "/factor_motion_color_r", 1);
+ factor_motion_color_.r = getParamWithDefault<double> (_server, prefix_ + "/factor_motion_color_r", 1);//yellow
factor_motion_color_.g = getParamWithDefault<double> (_server, prefix_ + "/factor_motion_color_g", 1);
factor_motion_color_.b = getParamWithDefault<double> (_server, prefix_ + "/factor_motion_color_b", 0);
factor_motion_color_.a = getParamWithDefault<double> (_server, prefix_ + "/factor_motion_color_a", 1);
- factor_loop_color_.r = getParamWithDefault<double> (_server, prefix_ + "/factor_loop_color_r", 1);
- factor_loop_color_.g = getParamWithDefault<double> (_server, prefix_ + "/factor_loop_color_g", 0);
+ factor_loop_color_.r = getParamWithDefault<double> (_server, prefix_ + "/factor_loop_color_r", 0); //green
+ factor_loop_color_.g = getParamWithDefault<double> (_server, prefix_ + "/factor_loop_color_g", 1);
factor_loop_color_.b = getParamWithDefault<double> (_server, prefix_ + "/factor_loop_color_b", 0);
factor_loop_color_.a = getParamWithDefault<double> (_server, prefix_ + "/factor_loop_color_a", 1);
- factor_lmk_color_.r = getParamWithDefault<double> (_server, prefix_ + "/factor_lmk_color_r", 0);
- factor_lmk_color_.g = getParamWithDefault<double> (_server, prefix_ + "/factor_lmk_color_g", 0);
+ factor_lmk_color_.r = getParamWithDefault<double> (_server, prefix_ + "/factor_lmk_color_r", 0); //cyan
+ factor_lmk_color_.g = getParamWithDefault<double> (_server, prefix_ + "/factor_lmk_color_g", 1);
factor_lmk_color_.b = getParamWithDefault<double> (_server, prefix_ + "/factor_lmk_color_b", 1);
factor_lmk_color_.a = getParamWithDefault<double> (_server, prefix_ + "/factor_lmk_color_a", 1);
- factor_geom_color_.r = getParamWithDefault<double> (_server, prefix_ + "/factor_geom_color_r", 0);
- factor_geom_color_.g = getParamWithDefault<double> (_server, prefix_ + "/factor_geom_color_g", 1);
+ factor_geom_color_.r = getParamWithDefault<double> (_server, prefix_ + "/factor_geom_color_r", 0); //blue
+ factor_geom_color_.g = getParamWithDefault<double> (_server, prefix_ + "/factor_geom_color_g", 0);
factor_geom_color_.b = getParamWithDefault<double> (_server, prefix_ + "/factor_geom_color_b", 1);
factor_geom_color_.a = getParamWithDefault<double> (_server, prefix_ + "/factor_geom_color_a", 1);
+ factor_other_color_.r = getParamWithDefault<double> (_server, prefix_ + "/factor_other_color_r", 1); //white
+ factor_other_color_.g = getParamWithDefault<double> (_server, prefix_ + "/factor_other_color_g", 1);
+ factor_other_color_.b = getParamWithDefault<double> (_server, prefix_ + "/factor_other_color_b", 1);
+ factor_other_color_.a = getParamWithDefault<double> (_server, prefix_ + "/factor_other_color_a", 1);
// INIT MARKERS ---------------------------------------------------
// factor markers message
@@ -478,16 +482,18 @@ void PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac,
// colors ------------------------------------------------------
auto color = frame_color_;
- if (fac->getTopology() == "ABS")
+ if (fac->getTopology() == TOP_ABS)
color = factor_abs_color_;
- if (fac->getTopology() == "MOTION")
+ if (fac->getTopology() == TOP_MOTION)
color = factor_motion_color_;
- if (fac->getTopology() == "LOOP")
+ if (fac->getTopology() == TOP_LOOP)
color = factor_loop_color_;
- if (fac->getTopology() == "LMK")
+ if (fac->getTopology() == TOP_LMK)
color = factor_lmk_color_;
- if (fac->getTopology() == "GEOM")
+ if (fac->getTopology() == TOP_GEOM)
color = factor_geom_color_;
+ if (fac->getTopology() == TOP_OTHER)
+ color = factor_other_color_;
// more transparent if inactive
if (fac->getStatus() == FAC_INACTIVE)
diff --git a/src/publisher_tf.cpp b/src/publisher_tf.cpp
index 59e4606a50f685a22192cc3667db316a92d22584..331147eecc147b93590a398a24f96e16e73e8e7b 100644
--- a/src/publisher_tf.cpp
+++ b/src/publisher_tf.cpp
@@ -16,9 +16,20 @@ PublisherTf::PublisherTf(const std::string& _unique_name,
Publisher(_unique_name, _server, _problem),
state_available_(true)
{
- map_frame_id_ = _server.getParam<std::string>(prefix_ + "/map_frame_id");
- odom_frame_id_ = _server.getParam<std::string>(prefix_ + "/odom_frame_id");
- base_frame_id_ = _server.getParam<std::string>(prefix_ + "/base_frame_id");
+ map_frame_id_ = _server.getParam<std::string>(prefix_ + "/map_frame_id");
+ odom_frame_id_ = _server.getParam<std::string>(prefix_ + "/odom_frame_id");
+ base_frame_id_ = _server.getParam<std::string>(prefix_ + "/base_frame_id");
+ publish_odom_tf_ = _server.getParam<bool>(prefix_ + "/publish_odom_tf");
+
+ // initialize TF transforms
+ T_odom2base_.setIdentity();
+ T_odom2base_.frame_id_ = odom_frame_id_;
+ T_odom2base_.child_frame_id_ = base_frame_id_;
+ T_odom2base_.stamp_ = ros::Time::now();
+ T_map2odom_.setIdentity();
+ T_map2odom_.frame_id_ = map_frame_id_;
+ T_map2odom_.child_frame_id_ = odom_frame_id_;
+ T_map2odom_.stamp_ = ros::Time::now();
}
void PublisherTf::initialize(ros::NodeHandle& nh, const std::string& topic)
@@ -31,6 +42,7 @@ void PublisherTf::publishDerived()
auto current_state = problem_->getState("PO");
auto current_ts = problem_->getTimeStamp();
+ // MAP - BASE
//Get map2base from wolf result, and builds base2map pose
tf::Transform T_map2base;
if (current_state.count('P') == 0 or
@@ -51,50 +63,59 @@ void PublisherTf::publishDerived()
ROS_INFO("PublisherTf: State available!");
state_available_ = true; // warning won't be displayed again
}
- // 2D
- if (problem_->getDim() == 2)
+ T_map2base = stateToTfTransform(current_state, problem_->getDim());
+ }
+
+ // MAP - ODOM
+ // Wolf odometry
+ if (publish_odom_tf_)
+ {
+ ros::Time transform_time;
+ std::string error_msg;
+ if (tfl_.getLatestCommonTime(odom_frame_id_, base_frame_id_, transform_time, &error_msg) == tf::ErrorValues::NO_ERROR and
+ transform_time != T_odom2base_.stamp_)
{
- T_map2base = tf::Transform (tf::createQuaternionFromYaw(current_state['O'](0)),
- tf::Vector3(current_state['P'](0),
- current_state['P'](1),
- 0) );
+ WOLF_ERROR("PublisherTf: option 'publish_odom_tf' enabled but a transform between ",
+ base_frame_id_, " and ", odom_frame_id_,
+ " was found published by a third party. Not publishing this transformation.");
}
- // 3D
else
{
- T_map2base = tf::Transform (tf::Quaternion(current_state['O'](0),
- current_state['O'](1),
- current_state['O'](2),
- current_state['O'](3)),
- tf::Vector3(current_state['P'](0),
- current_state['P'](1),
- current_state['P'](2)) );
- }
- }
+ VectorComposite odom;
+ for(auto pm : problem_->getProcessorIsMotionList())
+ {
+ /*auto p = std::dynamic_pointer_cast<ProcessorBase>(pm);
+ if(p != nullptr)
+ WOLF_TRACE("Getting odometry of processor ", p->getName(), ":\n", pm->getOdometry());//*/
+ odom = pm->getOdometry();
+ }
- //gets T_map2odom_ (odom wrt map), by using tf listener, and assuming an odometry node is broadcasting odom2base
- tf::StampedTransform T_base2odom;
- if ( tfl_.waitForTransform(base_frame_id_, odom_frame_id_, ros::Time(0), ros::Duration(0.1)) )
- {
- tfl_.lookupTransform(base_frame_id_, odom_frame_id_, ros::Time(0), T_base2odom);
- //std::cout << ros::Time::now().sec << " Odometry: " << T_base2odom.inverse().getOrigin().getX() << " " << T_base2odom.inverse().getOrigin().getY() << " " << T_base2odom.inverse().getRotation().getAngle() << std::endl;
+ T_odom2base_.setData(stateToTfTransform(odom, problem_->getDim()));
+ T_odom2base_.stamp_ = ros::Time::now();
+ tfb_.sendTransform(T_odom2base_);
+ }
}
+ // TF odometry
else
{
- ROS_WARN("No %s to %s frame received", base_frame_id_.c_str(), odom_frame_id_.c_str());
- T_base2odom.setIdentity();
- T_base2odom.frame_id_ = base_frame_id_;
- T_base2odom.child_frame_id_ = odom_frame_id_;
- T_base2odom.stamp_ = ros::Time::now();
+ try
+ {
+ tfl_.lookupTransform(odom_frame_id_, base_frame_id_, ros::Time(0), T_odom2base_);
+ }
+ catch(...)
+ {
+ ROS_WARN("No %s to %s frame received. Assuming identity.", base_frame_id_.c_str(), odom_frame_id_.c_str());
+ T_odom2base_.setIdentity();
+ T_odom2base_.stamp_ = ros::Time::now();
+ }
}
+
// Broadcast transform ---------------------------------------------------------------------------
- tf::StampedTransform T_map2odom(T_map2base * T_base2odom,
- ros::Time::now(),
- map_frame_id_,
- odom_frame_id_);
+ T_map2odom_.setData(T_map2base * T_odom2base_.inverse());
+ T_map2odom_.stamp_ = ros::Time::now();
//std::cout << "T_map2odom: " << T_map2odom.getOrigin().getX() << " " << T_map2odom.getOrigin().getY() << " " << T_map2odom.getRotation().getAngle() << std::endl;
- tfb_.sendTransform(T_map2odom);
+ tfb_.sendTransform(T_map2odom_);
}
}