Hi David,<div><br></div><div>First off, supporting custom cost values would be a feature that I'd love to see in the navigation stack. I'd be happy to incorporate a patch that implements this in a clean way. I'll respond to your questions below, and throw in my 2 cents for good measure.<br>
<br><div class="gmail_quote">On Tue, Feb 21, 2012 at 3:52 PM, David Lu!! <span dir="ltr"><<a href="mailto:davidvlu@gmail.com">davidvlu@gmail.com</a>></span> wrote:<br><blockquote class="gmail_quote" style="margin:0 0 0 .8ex;border-left:1px #ccc solid;padding-left:1ex">
<div>Hey all,</div><div><br></div><div>I have a design question. Following the question and discussion had on ROS Answers (<a href="http://answers.ros.org/question/12569/custom-costmap-values" target="_blank">http://answers.ros.org/question/12569/custom-costmap-values</a>) I am modifying costmap_2d to allow further types of input. I was originally passing in a custom message with the information I wanted, but I realized that this probably isn't the best solution. The question I open for discussion is what the best/most useful change would be.  </div>
</blockquote><blockquote class="gmail_quote" style="margin:0 0 0 .8ex;border-left:1px #ccc solid;padding-left:1ex">

<div><br></div><div>1) Split move_base into several separate nodes such that the arrows in this diagram (<a href="http://ros.org/wiki/move_base#move_base-1" target="_blank">http://ros.org/wiki/move_base#move_base-1</a>) are messages, as opposed to the whole thing being one giant binary with the data only available within the C++ environment. I presume there's a good reason why it's not written this way. However, this would allow another node to subscribe to the costmap and publish a new one with modifications made. </div>
</blockquote><div><br></div><div>The move_base node is a single binary because it was built long before ROS had a good way to pass data efficiently between nodes. At the time, if you wanted to avoid a TCP connection and data copies, throwing things into a single node was the only way to go. In place of nodelets, move_base uses pluginlib (<a href="http://ros.org/wiki/pluginlib">http://ros.org/wiki/pluginlib</a>) and ROS wrappers (<a href="http://ros.org/wiki/navigation/ROS_Wrappers">http://ros.org/wiki/navigation/ROS_Wrappers</a>) to try to keep things reasonably modular. </div>
<div><br></div><div>Re-factoring move_base to use nodelets would take a significant effort to do well, but there's no technical reason that it cannot be done. However, I don't think that it will actually make custom cost values any easier to implement. You can already modify the costmap ROS wrapper to subscribe to new topics. The real benefits of nodelets for the navigation stack would be improved introspection along with the ability to run components of move_base in separate processes when desired.</div>
<div><br></div><blockquote class="gmail_quote" style="margin:0 0 0 .8ex;border-left:1px #ccc solid;padding-left:1ex">

<div><br></div><div>2) Leave move_base as one node and modify it to use the channels of the PointCloud to pass in a type variable. For instance if type==0 or type is not specified, the current inflation rules are used. However, if the type==1 say, then use gaussian inflation rules (with variance and amplitude specified by the parameter server). </div>
</blockquote><div><br></div><div>Overloading the PointCloud message for custom cost values doesn't make much sense to me. I think that it will only serve to confuse people and will likely make for some difficult code to read. It would be far better to modify the costmap to subscribe to a different message type for custom cost values. That message would probably just be an OccupancyGrid.</div>
<blockquote class="gmail_quote" style="margin:0 0 0 .8ex;border-left:1px #ccc solid;padding-left:1ex">

<div><br></div><div>3) Use the channel values to specify all parameters for the inflation rules (i.e. not static from the parameter server)</div></blockquote><div><br></div><div>I don't like this for the same reason as 2.</div>
<div> </div><blockquote class="gmail_quote" style="margin:0 0 0 .8ex;border-left:1px #ccc solid;padding-left:1ex"><div><br></div><div>4) Use a custom service to populate the cost map. </div></blockquote><div><br></div><div>
This option makes the most sense to me. I could see a service that allows users to set cost values on cells that are guaranteed not to be overwritten by any observation source. This service could consist of an OccupancyGrid that encodes the desired custom cost values. </div>
<div><br></div><div>If you're planning on supporting "I'd rather not go there" costs in addition to "never go there" costs, you should also consider implementing an inflation policy of taking the highest cost seen for a cell rather than just ignoring data from sensors in custom areas. Otherwise, the robot will probably hit things.</div>
<div><br></div><div>Hope this helps, I'll be interested to see what you end up with,</div><div><br></div><div>Eitan</div><div><br></div><blockquote class="gmail_quote" style="margin:0 0 0 .8ex;border-left:1px #ccc solid;padding-left:1ex">


<div><br></div><div>There are a couple additional issues with computation time and whether to precompute and cache the inflation functions or not, but hopefully this is enough to start the conversation. </div><span class="HOEnZb"><font color="#888888"><div>
<br></div>

<div>-David!!</div>
</font></span><br>_______________________________________________<br>
ros-users mailing list<br>
<a href="mailto:ros-users@code.ros.org">ros-users@code.ros.org</a><br>
<a href="https://code.ros.org/mailman/listinfo/ros-users" target="_blank">https://code.ros.org/mailman/listinfo/ros-users</a><br>
<br></blockquote></div><br></div>