plot.cpp

Go to the documentation of this file.

#include "toposens_markers/plot.h"
#include <toposens_driver/sensor.h>

namespace toposens_markers
{
Plot::Plot(ros::NodeHandle nh, ros::NodeHandle private_nh)
{
  // Get launch paramaters
  std::string scans_topic;
  private_nh.getParam("sensor_mesh",
                      _sensor_mesh);           //, "package://toposens_description/meshes/TS3.stl");
  private_nh.getParam("frame_id", _frame_id);  //, "toposens");
  private_nh.getParam("target_frame", _target_frame);  //, "toposens");
  private_nh.getParam("scans_topic", scans_topic);     //, "ts_scans");

  // Set up RViz
  _rviz.reset(new rviz_visual_tools::RvizVisualTools(_frame_id, "/" + kMarkersTopic));
  _rviz->enableBatchPublishing();

  // Set up dynamic reconfigure to change marker settings
  _srv = std::make_shared<Cfg>(private_nh);
  Cfg::CallbackType f = boost::bind(&Plot::_reconfig, this, _1, _2);
  _srv->setCallback(f);

  // Set up transform listener so points from each sensor can be transformed to
  // global coordinates
  _tf2_listener = new tf2_ros::TransformListener(_tf2_buffer);
  _tf2_scans_sub.subscribe(nh, scans_topic, 400);
  _tf2_filter = new tf2_ros::MessageFilter<toposens_msgs::TsScan>(_tf2_scans_sub, _tf2_buffer,
                                                                  _target_frame, 300, nh);

  // Subscribe to topics
  _scans_sub = nh.subscribe(scans_topic, 300, &Plot::_plot, this);
  _static_tf_sub =
      nh.subscribe<tf2_msgs::TFMessage>("tf_static", 10, &Plot::_staticTFCallback, this);

  _rviz->enableBatchPublishing();

  ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 1);
  _rviz->waitForSubscriber(marker_pub, 3);
}

Plot::~Plot()
{
  delete _tf2_listener;
  delete _tf2_filter;
}

void Plot::_reconfig(TsMarkersConfig &cfg, uint32_t level)
{
  if ((int)level > 2)
  {
    ROS_INFO("Update skipped: Parameter not recognized");
    return;
  }

  if (cfg.lifetime == 0)
  {
    cfg.lifetime = 0.01;
  }

  _color_mode = cfg.point_color;
  _color_range = cfg.color_range;
  _half_color_range = _color_range / 2.0;
  _lifetime = cfg.lifetime;
  _rviz->setLifetime(_lifetime);
  _rviz->setGlobalScale(cfg.scale);
  if (level == -1)
    ROS_INFO("Marker settings initialized");
  else
    ROS_INFO("Marker settings updated");
}

void Plot::_plot(const toposens_msgs::TsScan::ConstPtr &msg)
{
  // Get the color of the points
  std_msgs::ColorRGBA color;

  if (_color_mode == 3)
  {
    if (_pose_map.size() > 0)
    {
      color = _color_map[msg->header.frame_id];
    }
    else
    {
      color = _rviz->getColor(rviz_visual_tools::colors::RED);
    }
  }

  for (auto it = msg->points.begin(); it != msg->points.end(); ++it)
  {
    // Copy TsPoint into PointStamped
    toposens_msgs::TsPoint pt = *it;
    if (_pose_map.size() > 0)
    {
      geometry_msgs::PointStamped ps;
      ps.point.x = pt.location.x;
      ps.point.y = pt.location.y;
      ps.point.z = pt.location.z;
      ps.header.frame_id = msg->header.frame_id;
      ps.header.stamp = ros::Time(0);

      // Perform the coordinate transformation
      try
      {
        _tf2_buffer.transform(ps, ps, _target_frame);
      }
      catch (tf2::TransformException ex)
      {
        ROS_ERROR("TsPoint transformation failed: %s", ex.what());
      }

      // Publish the point as a sphere
      pt.location.x = ps.point.x;
      pt.location.y = ps.point.y;
      pt.location.z = ps.point.z;
    }

    if (_color_mode == 0)
    {
      color = _rviz->getColorScale(pt.location.x / std::max(_color_range, pt.location.x));
    }
    else if (_color_mode == 1)
    {
      float offset_y = pt.location.y + _half_color_range;
      if (offset_y < 0)
      {
        color = _rviz->getColorScale(0);
      }
      else
      {
        color = _rviz->getColorScale(offset_y / std::max(_color_range, offset_y));
      }
    }
    else if (_color_mode == 2)
    {
      float offset_z = pt.location.z + _half_color_range;
      if (offset_z < 0)
      {
        color = _rviz->getColorScale(0);
      }
      else
      {
        color = _rviz->getColorScale(offset_z / std::max(_color_range, offset_z));
      }
    }

    Eigen::Vector3d location = _rviz->convertPoint32(pt.location);
    geometry_msgs::Vector3 scale = _rviz->getScale(_baseScale, pt.intensity);
    if (scale.x > 0) _rviz->publishSphere(location, color, scale, kMarkersNs);
  }

  // Perform batch publish to RViz
  _rviz->trigger();
}

void Plot::_staticTFCallback(const tf2_msgs::TFMessage::ConstPtr &tf)
{
  float color_scale = tf->transforms.size() - 1;
  if (color_scale == 0)
  {
    color_scale++;
  }
  for (int i = 0; i < tf->transforms.size(); i++)
  {
    if (!strncmp(tf->transforms[i].header.frame_id.c_str(), _target_frame.c_str(),
                 _target_frame.length()))
    {
      geometry_msgs::Pose pose;
      pose.position.x = tf->transforms[i].transform.translation.x;
      pose.position.y = tf->transforms[i].transform.translation.y;
      pose.position.z = tf->transforms[i].transform.translation.z;
      pose.orientation.x = tf->transforms[i].transform.rotation.x;
      pose.orientation.y = tf->transforms[i].transform.rotation.y;
      pose.orientation.z = tf->transforms[i].transform.rotation.z;
      pose.orientation.w = tf->transforms[i].transform.rotation.w;
      _pose_map[tf->transforms[i].child_frame_id] = pose;
      _mesh_color_map[tf->transforms[i].child_frame_id] = (rviz_visual_tools::colors)((i % 13) + 1);
      _color_map[tf->transforms[i].child_frame_id] =
          _rviz->getColor(_mesh_color_map[tf->transforms[i].child_frame_id]);
      ROS_INFO_STREAM(tf->transforms[i].child_frame_id
                      << ", \tR: " << _color_map[tf->transforms[i].child_frame_id].r
                      << ", G: " << _color_map[tf->transforms[i].child_frame_id].g
                      << ", B: " << _color_map[tf->transforms[i].child_frame_id].b
                      << ", A: " << _color_map[tf->transforms[i].child_frame_id].a
                      << " \t\t Position - X: " << pose.position.x << " \t Y: " << pose.position.y
                      << " \t Z: " << pose.position.z);
    }
  }
}

void Plot::publishSensorMeshes()
{
  std::map<std::string, geometry_msgs::Pose>::iterator pose_it;
  rviz_visual_tools::scales text_scale = (rviz_visual_tools::scales)10;
  rviz_visual_tools::colors text_color = (rviz_visual_tools::colors)13;
  rviz_visual_tools::scales axis_scale = (rviz_visual_tools::scales)7;

  _rviz->setLifetime(0);
  for (pose_it = _pose_map.begin(); pose_it != _pose_map.end(); pose_it++)
  {
    geometry_msgs::Pose label_pose;
    label_pose.position.x = pose_it->second.position.x - .1;
    label_pose.position.y = pose_it->second.position.y;
    label_pose.position.z = pose_it->second.position.z;
    label_pose.orientation.x = pose_it->second.orientation.x;
    label_pose.orientation.y = pose_it->second.orientation.y;
    label_pose.orientation.z = pose_it->second.orientation.z;

    _rviz->publishAxis(pose_it->second, axis_scale);
    _rviz->publishMesh(pose_it->second, _sensor_mesh, _mesh_color_map[pose_it->first], 1.0,
                       kMeshNs);
    _rviz->publishText(label_pose, pose_it->first, text_color, text_scale, false);
  }
  _rviz->trigger();
  _rviz->setLifetime(_lifetime);
}

void Plot::publishDefaultSensorMesh()
{
  _rviz->setLifetime(0);
  geometry_msgs::Pose pose;
  pose.position.x = 0;
  pose.position.y = 0;
  pose.position.z = 0;
  pose.orientation.x = 0;
  pose.orientation.y = 0;
  pose.orientation.z = 0;
  pose.orientation.w = 0;
  _rviz->publishMesh(pose, _sensor_mesh, rviz_visual_tools::colors::WHITE, 1.0, kMeshNs);
  _rviz->trigger();
  _rviz->setLifetime(_lifetime);
}

}  // namespace toposens_markers