jockey.h

Go to the documentation of this file.

#ifndef LJ_COSTMAP_JOCKEY_H
#define LJ_COSTMAP_JOCKEY_H

#include <ros/ros.h>

#include <lama_common/place_profile_conversions.h>
#include <lama_interfaces/ActOnMap.h>
#include <lama_interfaces/AddInterface.h>
#include <lama_msgs/DescriptorLink.h>
#include <lama_jockeys/localizing_jockey.h>
#include <lama_msgs/GetPlaceProfile.h>
#include <lama_msgs/SetPlaceProfile.h>
#include <lama_msgs/SetCrossing.h>
#include <place_matcher_msgs/PolygonDissimilarity.h>

#include <crossing_detector/costmap_crossing_detector.h>

namespace lj_costmap
{

class Jockey : public lama_jockeys::LocalizingJockey
{
  public:

    Jockey(std::string name, double frontier_width, double max_frontier_angle=0.785);

    virtual void onGetVertexDescriptor();
    virtual void onLocalizeInVertex();
    virtual void onGetDissimilarity();

    void setDissimilarityServerName(std::string name) {dissimilarity_server_name_ = name;}

  private:

    void initMapPlaceProfileInterface();
    void initMapCrossingInterface();
    void getLiveData();
    void handleMap(const nav_msgs::OccupancyGridConstPtr& msg);

    lama_msgs::DescriptorLink placeProfileDescriptorLink(int32_t id);
    lama_msgs::DescriptorLink crossingDescriptorLink(int32_t id);

    // ROS parameters.
    double range_cutoff_;  

    // Internals.
    bool data_received_;
    bool range_cutoff_set_;

    // Reception and storage of OccupancyGrid and PlaceProfile.
    nav_msgs::OccupancyGrid map_;
    lama_msgs::PlaceProfile profile_;

    // Map interface for PlaceProfile and Crossing descriptors.
    std::string place_profile_interface_name_;
    ros::ServiceClient place_profile_getter_;
    ros::ServiceClient place_profile_setter_;
    std::string crossing_interface_name_;
    ros::ServiceClient crossing_setter_;

    // LocalizeInVertex server.
    std::string localize_service_;
    // Dissimilarity server.
    std::string dissimilarity_server_name_;

    crossing_detector::CostmapCrossingDetector crossing_detector_;
};


} // namespace lj_costmap

#endif // LJ_COSTMAP_JOCKEY_H