builder.h

Go to the documentation of this file.

#ifndef _ROBOT_MARKERS_BUILDER_H_
#define _ROBOT_MARKERS_BUILDER_H_

#include <map>
#include <set>
#include <string>

#include "geometry_msgs/Pose.h"
#include "ros/duration.h"
#include "ros/time.h"
#include "std_msgs/ColorRGBA.h"
#include "transform_graph/transform_graph.h"
#include "urdf/model.h"
#include "visualization_msgs/Marker.h"
#include "visualization_msgs/MarkerArray.h"

#include "robot_markers/forward_kinematics.h"

namespace robot_markers {
class Builder {
 public:
  explicit Builder(const urdf::Model& model);

  void Init();

  void SetJointPositions(const std::map<std::string, double> joint_positions);

  void SetFrameId(const std::string& frame_id);

  void SetTime(const ros::Time& stamp);

  void SetNamespace(const std::string& ns);

  void SetPose(const geometry_msgs::Pose& pose);

  void SetColor(float r, float g, float b, float a);

  void SetLifetime(const ros::Duration& lifetime);

  void SetFrameLocked(bool frame_locked);

  void Build(visualization_msgs::MarkerArray* marker_array);

  void Build(const std::set<std::string>& link_names,
             visualization_msgs::MarkerArray* marker_array);

 private:
  // Sets everything in the marker except the pose.
  void BuildMarker(const urdf::Link& link, int id,
                   visualization_msgs::Marker* output);
  std::string NodeName(const std::string& name);

  urdf::Model model_;
  ForwardKinematics fk_;
  transform_graph::Graph tf_graph_;

  // Marker fields
  std::string frame_id_;
  ros::Time stamp_;
  std::string ns_;
  geometry_msgs::Pose pose_;
  std_msgs::ColorRGBA color_;
  ros::Duration lifetime_;
  bool frame_locked_;

  bool has_initialized_;
};
}  // namespace robot_markers

#endif  // _ROBOT_MARKERS_BUILDER_H_