Get data from sensor to control a robot

edit

Hi,

I'm totally new to Gazebo and I'm currently trying to exploit the data from the laser sensor I put on a robot. I noticed this post : http://answers.gazebosim.org/question..., so I tried to read the laser data from the corresponding topic which is "/gazebo/default/roc_robot/hokuyo/link/laser/scan" in my case.

This is my model plugin (used by the robot model) :

namespace gazebo
{
  class RocPlugin : public ModelPlugin
  {
  private:
    physics::ModelPtr           model;
    transport::SubscriberPtr    sptr;

  public:
    void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
    {
      std::string             topicName = "/gazebo/default/roc_robot/hokuyo/link/laser/scan";
      transport::NodePtr      node(new transport::Node());

      model = _parent;
      node->Init();
      node = node->Subscribe(topicName, &RocPlugin::ReadLaserData, this);
    }

   void ReadLaserData(ConstRaySensorPtr &_msg)
   {
      std::cout << _msg->range_max() << std::endl;
   }

    GZ_REGISTER_MODEL_PLUGIN(RocPlugin)
}

And I got this error :

gzserver: /usr/include/boost/smart_ptr/shared_ptr.hpp:687: typename boost::detail::sp_member_access<t>::type boost::shared_ptr<t>::operator->() const [with T = const gazebo::msgs::RaySensor; typename boost::detail::sp_member_access<t>::type = const gazebo::msgs::RaySensor*]: Assertion `px != 0' failed.

Since I aim to move the robot according to the data produced by the laser I haven't tried to write a plugin for the laser because even if I manage to read data from a sensor plugin I need those in my model plugin to control my robot.

Can you give me a tip for doing it ? ( Thank you ! )