Ask Your Question
0

Sending data to plugin

asked 2012-07-23 04:46:11 -0500

Febo gravatar image

updated 2012-07-23 12:13:13 -0500

Hello,

I have created my modified path planner based on carrot_planner. But for creating a plan I need some aditional information that I get from some rostopic. Generally you create nod in main and register your subscriber tehre, but as far as I know it is impossible to create main function inside plugin.

Is there a way I could listen to some topic from inside a plugin? Or any other way I could send my plan from (example:) "my_makeplan.cpp" to my "plugin my_navigator"? Where my_makeplan.cpp would be regular executable with main().

EDIT: here is my initialize method, strangely if I write publisher there is no error while compiling, but I don't see them in I write rostopic list. If I add subscriber I get this error:

error: no matching function for call to ‘ros::NodeHandle::subscribe(const char [5], int, <unresolved overloaded="" function="" type="">)’

Here is my initialize method:

    void MyNavigator::my_plan(const nav_msgs::OccupancyGrid::ConstPtr& msg){

    }

  void MyNavigator::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
    if(!initialized_){
      costmap_ros_ = costmap_ros;
      ros::NodeHandle private_nh("~/" + name);
      private_nh.param("step_size", step_size_, costmap_ros_->getResolution());
      private_nh.param("min_dist_from_robot", min_dist_from_robot_, 0.10);
      costmap_ros_->getCostmapCopy(costmap_);
      world_model_ = new base_local_planner::CostmapModel(costmap_); 
      //we'll get the parameters for the robot radius from the costmap we're associated with
      inscribed_radius_ = costmap_ros_->getInscribedRadius();
      circumscribed_radius_ = costmap_ros_->getCircumscribedRadius();
      footprint_spec_ = costmap_ros_->getRobotFootprint();

      initialized_ = true;

            ros::Subscriber sub1 = private_nh.subscribe("/map",1,my_plan);
            ros::Publisher pub1 = private_nh.advertise<nav_msgs::OccupancyGrid> ("moja_mapa", 1);
            ros::Publisher path_publisher1 = private_nh.advertise<nav_msgs::Path> ("moj_plan_path", 1);

    }
    else
      ROS_WARN("This planner has already been initialized... doing nothing");
  }

and here is my .h file (without includes):

namespace my_navigator
{
    class MyNavigator : public nav_core::BaseGlobalPlanner
        {
            public:
                MyNavigator();
                MyNavigator(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
                void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
                bool makePlan(const geometry_msgs::PoseStamped& start, 
          const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);
                void my_plan(const nav_msgs::OccupancyGrid::ConstPtr& msg);
            private:
                costmap_2d::Costmap2DROS* costmap_ros_;
                double step_size_, min_dist_from_robot_;
                costmap_2d::Costmap2D costmap_;
                base_local_planner::WorldModel* world_model_;
                double inscribed_radius_, circumscribed_radius_, inflation_radius_;
                double footprintCost(double x_i, double y_i, double theta_i);
                std::vector<geometry_msgs::Point> footprint_spec_;
                bool initialized_;
        };

};

#endif

What am I doing wrong?

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
4

answered 2012-07-23 10:07:39 -0500

Lorenz gravatar image

updated 2012-07-23 22:03:16 -0500

Generally, in C++ you create a class that contains subscribers and publishers which are initialized in the constructor. You can subscribe to topics and read ROS parameters either in your plugin's constructor or in the (virtual) method initialize.

Edit: The problem with your code is that the NodeHandle and the subscribers and publishers are local to your init method. As soon as this method exits, the stack is unwound and the objects destructed. Make them member variables of your class and everything should work.

I actually don't think your subscriber works as expected. The compiler should complain unless you have a global method my_plan somewhere. To use my_plan of your class, you need to pass a class instance:

sub1 = private_nh.subscribe("/map", 1, &MyNavigator::my_plan, this);

Alternatively you can use boost::bind. You can find more information on callbacks here.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2012-07-23 04:46:11 -0500

Seen: 827 times

Last updated: Jul 23 '12