How to transport the ESDF? Are here any planners with examples? about voxblox HOT 15 CLOSED

Maybe as a quick hint to those trying to do the same thing. Here's some very rough snippets from a planner using voxblox ESDF for collision checking. Planner not included yet. ;)

class RrtPlannerVoxblox {
 public:
  RrtPlannerVoxblox(const ros::NodeHandle& nh,
                    const ros::NodeHandle& nh_private);
  virtual ~RrtPlannerVoxblox() {}
  double getMapDistance(const Eigen::Vector3d& position) const;
 private:
  ros::NodeHandle nh_;
  ros::NodeHandle nh_private_;

  // Map!
  voxblox::EsdfServer voxblox_server_;
};

There's also now a traversability pointcloud you can enable/disable, that if you set the radius to, can show you parts of the map the planner thinks are traversable in a pointcloud:

RrtPlannerVoxblox::RrtPlannerVoxblox(const ros::NodeHandle& nh,
                                     const ros::NodeHandle& nh_private)
    : nh_(nh),
      nh_private_(nh_private),
      voxblox_server_(nh_, nh_private_) {
  // Optionally load a map saved with the save_map service call in voxblox.
  std::string input_filepath;
  nh_private_.param("voxblox_path", input_filepath, input_filepath);
  if (!input_filepath.empty()) {
    if (!voxblox_server_.loadMap(input_filepath)) {
      ROS_ERROR("Couldn't load ESDF map!");
    }
  }
  double robot_radius = 1.0;
  voxblox_server_.setTraversabilityRadius(robot_radius);
  voxblox_server_.publishTraversable();
}

Then to check for collisions you can just do this (compare map distance to your robot radius):

double RrtPlannerVoxblox::getMapDistance(
    const Eigen::Vector3d& position) const {
  if (!voxblox_server_.getEsdfMapPtr()) {
    return 0.0;
  }
  double distance = 0.0;
  if (!voxblox_server_.getEsdfMapPtr()->getDistanceAtPosition(position,
                                                              &distance)) {
    return 0.0;
  }
  return distance;
}

from voxblox.

Good Day,

is it better to subscribe to the TSDF topic and create the ESDF in the planner or directly subscribe to the ESDF topic?
Which way is better?

best regards

from voxblox.

Hi @schnph,
I'm currently working on cleaning out my planning pipeline and open-sourcing it, just getting unfortunately side-tracked all the time.
The short answers are:

• Have a separate mapping and planning node.
• Your mapping node should do both TSDF and ESDF calculations, as it needs to accurately keep track of what's been updated in the TSDF to do the ESDF computations efficiently.
• Private vs. public nodehandle is explained here: http://wiki.ros.org/Names Subscribers are public node handles, ros params and advertisers are private nodehandles.

Doing incremental ESDF map sending is also in the pipeline, but will require some refactoring. In general we haven't had much issue with the transmission, but we also run the mapper and the planner always on the same machine so it's just a local socket connection.

from voxblox.

Hi @helenol,

That I need the mapping node separately from the planning node is clear.
The planning node should subscribe to the esdf_map_out and tsdf_map_out or is the esdf_map_out topic all it does need to set up the esdf_server in the planning node?
Is that the right way or should it be done differently?
I hope my question is now more clear.

I think along this code:

voxblox::EsdfServer server;
voxblox::EsdfMap map = server.getEsdfMapPtr();

ros_sub = nh.subscribe("esdf_map_out", 1,&Planner::esdfMapCallback, this);

Planner::esdfMapCallback(const voxblox_msgs::Layer& layer_msg)
{
bool success =
voxblox::deserializeMsgToLayervoxblox::EsdfVoxel(layer_msg, map.getEsdfLayerPtr());

if (!success) {
ROS_ERROR_THROTTLE(10, "Got an invalid ESDF map message!");
} else {
ROS_INFO_ONCE("Got an ESDF map from ROS topic!");
}
}

from voxblox.

Ahh ok, I see the question. If you only use ESDF for collision checks (i.e., with EsdfMap::getDistanceAtPosition()), then you don't need to send the TSDF as well, just ESDF will work fine for the planner.
What I always do is just have an EsdfServer as a member in the planner as well. This way you can visualize what's in the ESDF map to debug, and all you have to do is remap the esdf_map_in topic to match the esdf_map_out topic of the mapping node.
The one thing you have to be careful about is making sure that the tsdf_voxel_size and tsdf_voxels_per_side parameters match between the two nodes (yes, even though you're not subscribing to or receiving the tsdf map).

from voxblox.

So both nodes should use the same parameter set, ok. Thank you for your reply! With this I can create a "client" for only the ESDF. So both EsdfMap::getDistanceAtPosition() and EsdfMap::getDistanceAndGradientAtPositionAtPosition() should work fine with this.

Thank you for your reply, did help me to clear things. Now I will code it ^^...

from voxblox.

Good Day @helenol ,

I think I got it working, but here is something, which isn't right I think.

I did let it show me the distance and gradient for different points. Every single time it does return the same gradient. The gradient should be a vector away or towards the obstacle, but it doesn't do that and it is always the same value, which it shouldn't be or am I worng? I only query observed points and non not observed ones.

[ INFO] [1535022858.430601778]: point = 0.35 , 0.10 , 0.15
[ INFO] [1535022858.430622881]: distance = 0.30
v =
6.95264e-310
-4.60876e+300
6.95264e-310

[ INFO] [1535022858.430679299]: point = 0.35 , 0.10 , 0.20
[ INFO] [1535022858.430700920]: distance = 0.34
v =
6.95264e-310
-4.60876e+300
6.95264e-310

[ INFO] [1535022858.430767599]: point = 0.35 , 0.10 , 0.25
[ INFO] [1535022858.430790065]: distance = 0.39
v =
6.95264e-310
-4.60876e+300
6.95264e-310

[ INFO] [1535022858.431099204]: point = 0.40 , 0.00 , 0.15
[ INFO] [1535022858.431120842]: distance = 0.27
v =
6.95264e-310
-4.60876e+300
6.95264e-310

[ INFO] [1535022858.432125707]: point = 0.40 , 0.10 , 0.30
[ INFO] [1535022858.432147255]: distance = 0.41
v =
6.95264e-310
-4.60876e+300
6.95264e-310

[ INFO] [1535022858.433048020]: point = 0.45 , 0.05 , 0.30
[ INFO] [1535022858.433068770]: distance = 0.37
v =
6.95264e-310
-4.60876e+300
6.95264e-310

[ INFO] [1535022858.433671359]: point = 0.50 , 0.00 , 0.10
[ INFO] [1535022858.433693553]: distance = 0.20
v =
6.95264e-310
-4.60876e+300
6.95264e-310

[ INFO] [1535022858.452588910]: point = 0.50 , 0.10 , 0.30
[ INFO] [1535022858.452615059]: distance = 0.35
v =
6.95264e-310
-4.60876e+300
6.95264e-310

[ INFO] [1535022858.452941665]: point = 0.50 , 0.20 , 0.10
[ INFO] [1535022858.452968633]: distance = 0.21
v =
6.95264e-310
-4.60876e+300
6.95264e-310

from voxblox.

I repeated the set-up with different objects, it never did change the gradient. Why?

best regards
schnph

from voxblox.

Those values you are getting back look like uninitialized memory to me. Which function are you using to get the gradient and are you requesting the interpolated gradient? Also are you checking the return value to make sure getting the gradient retrieval was successful? If it's returning it's valid and giving those values we may have a bug.

Otherwise when getting an interpolated gradient the function requires both a voxel's neighbors and neighbor's neighbors to be observed, which can fail in many cases.

from voxblox.

Good Day @helenol and @ZacharyTaylor,

I did initialized memory, but it isn't the problems core. I made a simple case code.
I use the getDistanceAndGradientAtPosition(position, &distance, &gradient) function.
the function itself sets interpolation to true, so yes I do interpolate. I check the return value, see code.
code:
testplanner.txt
testplanner_node .txt
testplannerh.txt
output:
voxblox_gradient.txt

Input signal is a xtion, I set a box as the object to avoid into the room at about 80cm, the distance is return but not the gradient.

from voxblox.

When looking at your function it makes sense that it never works, your function prototype is
double getMapDistanceAndGradientAtPosition(const Eigen::Vector3d position, Eigen::Vector3d gradient) const;

As gradient is passed by value, nothing that happens in the function will affect its value I guess you meant either Eigen::Vector3d& gradient or Eigen::Vector3d* gradient

Also when using Eigen, passing fixed size matricies by value can cause memory alignment issues https://eigen.tuxfamily.org/dox/group__TopicPassingByValue.html

from voxblox.

ok thanks

from voxblox.

It is functional now, I overlooked that, I'm sorry for the trouble I made.

from voxblox.

@helenol , @ZacharyTaylor

One last question. How did you set the radius around the robot at start time to be known/observed?

from voxblox.

clear_sphere_for_planning param set to true.
Looks like no way to set the clear sphere radiuses from ros params at the moment.. I'll put it on the to-do list. (one smaller inner sphere sets unknown -> unoccupied, a larger outside sphere sets unknown -> occupied).

from voxblox.