Adding Subscriber to custom Costmap
I am currently writing my own custom costmap plugin for use with the navigation2 stack. I'm trying to have the costmap layer take the navigation goal into account when updating costs near it. I've been trying to do this by adding a subscriber to the custom costmap layer that will pull the position of the navigation goal when it is set.
However, I haven't found a good way to do this.
I've tried to use the existing Lifecycle node within the nav2_costmap_2d::Layer class, but no matter what I do, I simply cannot get the data types to match. When I first tried to create the subscriber within the onInitialize method I wasn't able to build the plugin due to colcon complaining that it could not find the subscriber (error looked like this):
error: no matching function for call to ‘std::function<void(const geometry_msgs::msg::Pose2D_<std::allocator<void> >&)>::function(std::_Bind<void (jackstand_objective_costmap_plugin::JSLayer::*(std::shared_ptr<rclcpp_lifecycle::LifecycleNode>, std::_Placeholder<1>))(const geometry_msgs::msg::Pose2D_<std::allocator<void> >&)>&)’ 394 | callback_variant_ = static_cast<typename scbth::callback_type>(callback);
I figured this was because the Lifecycle node needed to be in the process of being configured to create a subscriber. So the next thing I attempted to do was define an on_configure callback to the node within the Layer class. I attempted to do this via the register_on_configure method. I could not get my types to match though. When I made a func function the method always complained that it needed a type std::function<func>.
I tried making a separate regular node that I could set as a shared pointer within my costmap layer which also didn't work. This was due to the plugin not even recognizing the node I had made within my code.
Is there a way to add a subscriber to a custom costmap layer plugin? If so, would anyone please tell me how?
