waypoint_provider.cpp

Go to the documentation of this file.

/*
   Way point Provider

   inspired by yocs_waypoints_navi

   LICENSE : BSD - https://raw.github.com/yujinrobot/yujin_ocs/license/LICENSE

   Author : Jihoon Lee
   Date   : Dec 2013
 */

#include "yocs_waypoint_provider/waypoint_provider.hpp"

namespace yocs {
  WaypointProvider::WaypointProvider(ros::NodeHandle& n,
                                     yocs_msgs::WaypointList& wps,
                                     yocs_msgs::TrajectoryList& trajs) : nh_(n)
  {

    // setup pub
    waypoints_pub_ = nh_.advertise<yocs_msgs::WaypointList>("waypoints", 5, true);
    trajectories_pub_ = nh_.advertise<yocs_msgs::TrajectoryList>("trajectories", 5, true);
    waypoints_marker_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("waypoint_markers", 5, true);
    trajectory_marker_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("trajectory_markers", 5, true);
    // setup srv server
    waypoints_srv_ = nh_.advertiseService("request_waypoints", &WaypointProvider::processWaypointsService, this);

    waypoints_ = wps;
    trajectories_ = trajs;
    generateWaypointMarkers(waypoints_, waypoint_markers_);
    generateTrajectoryMarkers(trajectories_, trajectory_markers_);

    marker_index_ = 1000;
    label_index_ = 2000;
  }


  WaypointProvider::~WaypointProvider() {}

  bool WaypointProvider::processWaypointsService(yocs_msgs::WaypointListService::Request& request,
                                                 yocs_msgs::WaypointListService::Response& response)
  {
    ROS_INFO("Waypoint Manager : Received request");
    if(!initialized_) // return false if node is not initialized with points
    {
      response.success = false;
    }
    else {
      response.waypoints = this->waypoints_;
      response.success = true;
    }
    return true;
  }

  void WaypointProvider::generateWaypointMarkers(const yocs_msgs::WaypointList& wps,
                                                 visualization_msgs::MarkerArray& wp_viz)
  {
    wp_viz.markers.clear();

    unsigned int i;

    for(i = 0; i < wps.waypoints.size(); i++)
    {
      visualization_msgs::Marker marker;
      visualization_msgs::Marker label;

      createMarkerArrow(i, wps.waypoints[i], marker);
      createMarkerLabel(wps.waypoints[i].header.frame_id,
                        i,
                        "waypoint_labels",
                        wps.waypoints[i].name,
                        wps.waypoints[i].pose,
                        label);

      wp_viz.markers.push_back(marker);
      wp_viz.markers.push_back(label);
    }
  }

  void WaypointProvider::generateTrajectoryMarkers(const yocs_msgs::TrajectoryList& trajs,
                                                   visualization_msgs::MarkerArray& traj_markers)
  {
    traj_markers.markers.clear();
    for(unsigned int traj = 0; traj < trajs.trajectories.size(); ++traj)
    {
      visualization_msgs::Marker marker;
      createMarkerLineStrip(traj, trajs.trajectories[traj], marker);
      traj_markers.markers.push_back(marker);
      // no label, since way points with labels are already generated by for the waypoint marker publisher
    }
  }

  void WaypointProvider::createMarkerArrow(const int i,
                                           const yocs_msgs::Waypoint& wp,
                                           visualization_msgs::Marker& marker)
  {
    marker.header.frame_id = wp.header.frame_id;
    marker.header.stamp = ros::Time();
    marker.ns = "waypoints";
    marker.id = i + marker_index_;
    marker.pose = wp.pose;
    marker.type = visualization_msgs::Marker::ARROW;
    marker.action = visualization_msgs::Marker::ADD;
    marker.color.r = 0.0f;
    marker.color.g = 1.0f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0f;
    marker.scale.x = 0.1;
    marker.scale.y = 0.1;
    marker.scale.z = 0.1;

  }

  void WaypointProvider::createMarkerLabel(const std::string frame_id,
                                           const int id,
                                           const std::string ns,
                                           const std::string wp_name,
                                           const geometry_msgs::Pose wp_pose,
                                           visualization_msgs::Marker& marker)
  {
    marker.header.frame_id = frame_id;
    marker.header.stamp = ros::Time();
    marker.ns = ns;
    marker.id = id + label_index_;
    marker.scale.x = 0.1;
    marker.scale.y = 0.1;
    marker.scale.z = 0.1;
    marker.color.r = 1.0f;
    marker.color.g = 1.0f;
    marker.color.b = 1.0f;
    marker.color.a = 1.0f;
    marker.pose = wp_pose;
    marker.pose.position.z = marker.pose.position.z + marker.scale.z / 2.0 + 0.05;  // just above the marker
    marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    marker.action = visualization_msgs::Marker::ADD;
    marker.text = wp_name;
  }

  void WaypointProvider::createMarkerLineStrip(const int i,
                                               const yocs_msgs::Trajectory& traj,
                                               visualization_msgs::Marker& marker)
  {
    marker.header.frame_id = traj.waypoints[0].header.frame_id;
    marker.header.stamp = ros::Time();
    marker.ns = "trajectories";
    marker.id = i + marker_index_;
    marker.pose = geometry_msgs::Pose();
    marker.type = visualization_msgs::Marker::LINE_STRIP;
    marker.action = visualization_msgs::Marker::ADD;
    marker.color.r = 0.0f;
    marker.color.g = 0.0f;
    marker.color.b = 1.0f;
    marker.color.a = 1.0f;
    marker.scale.x = 0.1;
    marker.scale.y = 0.1;
    marker.scale.z = 0.1;
    for(unsigned int wp = 0; wp < traj.waypoints.size(); ++wp)
    {
      marker.points.push_back(traj.waypoints[wp].pose.position);
      marker.colors.push_back(marker.color);
    }
  }

  void WaypointProvider::spin() {
    waypoints_pub_.publish(waypoints_);
    trajectories_pub_.publish(trajectories_);
    waypoints_marker_pub_.publish(waypoint_markers_);
    trajectory_marker_pub_.publish(trajectory_markers_);
    initialized_ = true;
    ros::spin();
  }
}