How to make a realtime controller get information

edit

Hi guys

So I have a robot simulated in Gazebo that I can control with the realtime controllers I made thanks to the tutorials. I also added a service and a PID controller as in the tutorials (which doesnt work good for every controller, but that's another topic!).

Since I want to have a "smart navigation" with the robot, I tried getting information from the ros topic published by gazebo : ModelStates. It gives you position, orientation, velocity... So I made my controller subscribe to that topic the following way (no need to give the whole code).

#include "leftwheelcontroller/leftwcontrol.h"
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <gazebo_msgs/ModelStates.h>
#include <boost/shared_ptr.hpp>

namespace controller_ns {

bool lwControllerClass::init(pr2_mechanism_model::RobotState *robot,  ros::NodeHandle &n)
{}

void lwControllerClass::starting()
{}

void lwControllerClass::update()
{    
   ROS_INFO("lefwcontrol : lWEffort= %f",  lWEffort);
}

void lwControllerClass::stopping()
{}

/// Service call to set effort applied on the wheel
bool lwControllerClass::setEffort(leftwheelcontroller::SetEffort::Request& req,leftwheelcontroller::SetEffort::Response& resp)
{}

void lwControllerClass::chatterCallback(const gazebo_msgs::ModelStates::ConstPtr& msg)
{
    lWEffort=msg->pose[1].position.x;
    ROS_INFO("lwController heard this effort set: %f", lWEffort);
}

} // namespace

int main(int argc, char **argv)
{
  ros::init(argc, argv, "lwController");
  ros::NodeHandle n;

  lw_controller_ns::lwControllerClass *p= new lw_controller_ns::lwControllerClass;
  ros::Subscriber sub = n.subscribe("gazebo/model_states", (uint)1000, &lw_controller_ns::lwControllerClass::chatterCallback,p);

  ros::spin();

  return 0;
}

So the callback output gives me the position of the robot (as it is supposed to do), but the update function loop give me "-0.0". I just guess that such subscription can not be done in real time...?

In a way or another, I'd like to get navigation information. I looked at a few things :

• Navigation stack / Odometry which I don't know if it works in realtime
• realtime_tools that seems to enable a controller to publish topics, but not to subscribe...
• GazeboRosIMU plugin and the following answer, but I tried and it seems it just publishes a topic.
• It seems there is "getModelState" service from Gazebo that may give information such as the "modelstate" topic, but i dont know how to get it, nor how to send it to my RT controller.

Any answer to these questions, or any suggestion is very welcome!

Update : I may build a non real time controller, but in this case (if RT is a dead-end), what tools should I use? pr2_controller_manager or pr2_mechanism_model are realtime specific, and a solution involving topics subscribed by gazebo might be a bit low-level.

UP