#include <move_base/move_base.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/connection_monitor.h>
#include <visualization_msgs/MarkerArray.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <gazebo_msgs/ModelStates.h>
#include <gazebo_msgs/GetWorldProperties.h>
#include <gazebo_msgs/GetModelProperties.h>
#include <gazebo_msgs/GetModelState.h>
#include <iostream>

int main(int argc, char** argv){
  ROS_INFO_STREAM("starting robot_controller");
  ros::init(argc, argv, "robot_controller");

  ros::NodeHandle n;
  ros::ServiceClient _getWorldPropertiesClient;
  gazebo_msgs::GetWorldProperties _getWorldProperties;
  ros::ServiceClient _getModelPropertiesClient;
  gazebo_msgs::GetModelProperties _getModelProperties;
  ros::ServiceClient _getModelStateClient;
  gazebo_msgs::GetModelState _getModelState;

  _getWorldPropertiesClient = n.serviceClient<gazebo_msgs::GetWorldProperties>("/gazebo/get_world_properties");
  _getModelPropertiesClient = n.serviceClient<gazebo_msgs::GetModelProperties>("/gazebo/get_model_properties");
  _getModelStateClient = n.serviceClient<gazebo_msgs::GetModelProperties>("/gazebo/get_model_state");

  _getWorldPropertiesClient.call(_getWorldProperties);
  std::vector<std::string> model_names = _getWorldProperties.response.model_names;

  for(auto&& model_name: model_names){
    if(model_name!=""){
      _getModelProperties.request.model_name = model_name;
      _getModelPropertiesClient.call(_getModelProperties);
      std::string relative_entity_name = _getModelProperties.response.body_names.at(0);

      _getModelState.request.model_name = model_name;
      _getModelState.request.relative_entity_name = relative_entity_name;
      _getModelStateClient.call(_getModelState);

    }
  }

  while(ros::ok()){
    ros::spin();
  }
}