Writing a Client

The following code defines the inspection path planning problem to be solved by the Structural Inspection Planner (SIP). For a description of each block see below.

#include <ros/ros.h>
#include "koptplanner/inspection.h"
#include "shape_msgs/SolidPrimitive.h"
#include <cstdlib>
#include <visualization_msgs/Marker.h>
#include <fstream>
#include <geometry_msgs/PolygonStamped.h>
#include <nav_msgs/Path.h>
#include <std_msgs/Int32.h>
#include <ros/package.h>
#include "tf/tf.h"

std::vector<nav_msgs::Path> * readSTLfile(std::string name);

int main(int argc, char **argv)
{
  ros::init(argc, argv, "requester");
  ROS_INFO("Requester is alive");
  if (argc != 1)
  {
    ROS_INFO("usage: plan");
    return 1;
  }

  ros::NodeHandle n;
  ros::Publisher obstacle_pub = n.advertise<visualization_msgs::Marker>
    ("scenario", 1);
  ros::Publisher stl_pub = n.advertise<nav_msgs::Path>("stl_mesh", 1);
  ros::ServiceClient client = n.serviceClient<koptplanner::inspection>
    ("inspectionPath");

  ros::Rate r(50.0);
  ros::Rate r2(1.0);
  r2.sleep();

  /* define the bounding box */
  koptplanner::inspection srv;
  srv.request.spaceSize.push_back(1375);
  srv.request.spaceSize.push_back(2165);
  srv.request.spaceSize.push_back(0.001);
  srv.request.spaceCenter.push_back(1375.0/2.0);
  srv.request.spaceCenter.push_back(2165.0/2.0);
  srv.request.spaceCenter.push_back(200.0);

  geometry_msgs::Pose reqPose;

  /* starting pose (comment the push_back if no explicit starting pose
     is desired) */
  reqPose.position.x = 300.0;
  reqPose.position.y = 300.0;
  reqPose.position.z = 200.0;
  tf::Quaternion q = tf::createQuaternionFromRPY(0.0, 0.0, 0.0);
  reqPose.orientation.x = q.x();
  reqPose.orientation.y = q.y();
  reqPose.orientation.z = q.z();
  reqPose.orientation.w = q.w();
  srv.request.requiredPoses.push_back(reqPose);

  /* final pose (remove if no explicit final pose is desired) */
  reqPose.position.x = 400.0;
  reqPose.position.y = 300.0;
  reqPose.position.z = 200.0;
  q = tf::createQuaternionFromRPY(0.0, 0.0, 0.0);
  reqPose.orientation.x = q.x();
  reqPose.orientation.y = q.y();
  reqPose.orientation.z = q.z();
  reqPose.orientation.w = q.w();
  srv.request.requiredPoses.push_back(reqPose);

  /* parameters for the path calculation (such as may change during
     mission) */
  srv.request.incidenceAngle = M_PI/6.0;
  srv.request.minDist = 40.0;
  srv.request.maxDist = 300.0;
  srv.request.numIterations = 20;

  /* read STL file and publish to rviz */
  std::vector<nav_msgs::Path> * mesh = readSTLfile(ros::package::getPath(
    "request")+"/meshes/regularPlanes/rPlane.stl");
  for(std::vector<nav_msgs::Path>::iterator it = mesh->begin();
      it != mesh->end() && ros::ok(); it++)
  {
    stl_pub.publish(*it);
    geometry_msgs::Polygon p;
    geometry_msgs::Point32 p32;
    p32.x = it->poses[0].pose.position.x;
    p32.y = it->poses[0].pose.position.y;
    p32.z = it->poses[0].pose.position.z;
    p.points.push_back(p32);
    p32.x = it->poses[1].pose.position.x;
    p32.y = it->poses[1].pose.position.y;
    p32.z = it->poses[1].pose.position.z;
    p.points.push_back(p32);
    p32.x = it->poses[2].pose.position.x;
    p32.y = it->poses[2].pose.position.y;
    p32.z = it->poses[2].pose.position.z;
    p.points.push_back(p32);
    srv.request.inspectionArea.push_back(p);
    r.sleep();
  }

  /* define obstacle regions as cuboids that are coordinate system aligned */
  shape_msgs::SolidPrimitive body;
  body.type = shape_msgs::SolidPrimitive::BOX;
  body.dimensions.push_back(40.0);
  body.dimensions.push_back(50.0);
  body.dimensions.push_back(4.0);
  srv.request.obstacles.push_back(body);
  geometry_msgs::Pose pose;
  pose.position.x = 600.0;
  pose.position.y = 600.0;
  pose.position.z = 200.0;
  pose.orientation.x = 0.0;
  pose.orientation.y = 0.0;
  pose.orientation.z = 0.0;
  pose.orientation.w = 1.0;
  srv.request.obstaclesPoses.push_back(pose);
  srv.request.obstacleIntransparancy.push_back(0);

  // publish obstacles for rviz 
  visualization_msgs::Marker marker;
  marker.header.frame_id = "/kopt_frame";
  marker.header.stamp = ros::Time::now();
  marker.ns = "obstacles";
  marker.id = 0; // enumerate when adding more obstacles
  marker.type = visualization_msgs::Marker::CUBE;
  marker.action = visualization_msgs::Marker::ADD;

  marker.pose.position.x = pose.position.x;
  marker.pose.position.y = pose.position.y;
  marker.pose.position.z = pose.position.z;
  marker.pose.orientation.x = pose.orientation.x;
  marker.pose.orientation.y = pose.orientation.y;
  marker.pose.orientation.z = pose.orientation.z;
  marker.pose.orientation.w = pose.orientation.w;

  marker.scale.x = body.dimensions[0];
  marker.scale.y = body.dimensions[1];
  marker.scale.z = body.dimensions[2];

  marker.color.r = 0.0f;
  marker.color.g = 0.0f;
  marker.color.b = 1.0f;
  marker.color.a = 0.5;

  marker.lifetime = ros::Duration();
  obstacle_pub.publish(marker);
  r.sleep();

  if (client.call(
  {
    ROS_INFO("Successfully planned inspection path");srv))
  }
  else
  {
    ROS_ERROR("Failed to call service planner");
    return 1;
  }

  return 0;
}

std::vector<nav_msgs::Path> * readSTLfile(std::string name)
{
  std::vector<nav_msgs::Path> * mesh = new std::vector<nav_msgs::Path>;
  std::fstream f;
  f.open(name.c_str());
  assert(f.is_open());
  int MaxLine = 0;
  char* line;
  double maxX = -DBL_MAX;
  double maxY = -DBL_MAX;
  double maxZ = -DBL_MAX;
  double minX = DBL_MAX;
  double minY = DBL_MAX;
  double minZ = DBL_MAX;
  assert(line = (char *) malloc(MaxLine = 80));
  f.getline(line, MaxLine);
  if(0 != strcmp(strtok(line, " "), "solid"))
  {
    ROS_ERROR("Invalid mesh file! Give in ascii-format.");
    ros::shutdown();
  }
  assert(line = (char *) realloc(line, MaxLine));
  f.getline(line, MaxLine);
  int k = 0;
  while(0 != strcmp(strtok(line, " "), "endsolid") && !ros::isShuttingDown())
  {
    int q = 0;
    nav_msgs::Path p;
    geometry_msgs::PoseStamped v1;
    for(int i = 0; i<7; i++)
    {
      while(line[q] == ' ')
        q++;
      if(line[q] == 'v')
      {
        // used to rotate the mesh before processing
        const double yawTrafo = 0.0;
        // used to scale the mesh before processing
        const double scaleFactor = 1.0;
        // used to offset the mesh before processing
        const double offsetX = 0.0;
        // used to offset the mesh before processing
        const double offsetY = 0.0;
        // used to offset the mesh before processing
        const double offsetZ = 0.0;

        geometry_msgs::PoseStamped vert;
        char* v = strtok(line+q," ");
        v = strtok(NULL," ");
        double xtmp = atof(v)/scaleFactor;
        v = strtok(NULL," ");
        double ytmp = atof(v)/scaleFactor;
        vert.pose.position.x = cos(yawTrafo)*xtmp-sin(yawTrafo)*ytmp;
        vert.pose.position.y =  sin(yawTrafo)*xtmp+cos(yawTrafo)*ytmp;
        v = strtok(NULL," ");
        vert.pose.position.z =  atof(v)/scaleFactor;
        vert.pose.position.x -= offsetX;
        vert.pose.position.y -= offsetY;
        vert.pose.position.z -= offsetZ;
        if(maxX<vert.pose.position.x)
          maxX=vert.pose.position.x;
        if(maxY<vert.pose.position.y)
          maxY=vert.pose.position.y;
        if(maxZ<vert.pose.position.z)
          maxZ=vert.pose.position.z;
        if(minX>vert.pose.position.x)
          minX=vert.pose.position.x;
        if(minY>vert.pose.position.y)
          minY=vert.pose.position.y;
        if(minZ>vert.pose.position.z)
          minZ=vert.pose.position.z;
        vert.pose.orientation.x =  0.0;
        vert.pose.orientation.y =  0.0;
        vert.pose.orientation.z =  0.0;
        vert.pose.orientation.w =  1.0;
        p.poses.push_back(vert);
        if(p.poses.size() == 1)
          v1 = vert;
      }
      assert(line = (char *) realloc(line, MaxLine));
      f.getline(line, MaxLine);
    }
    p.poses.push_back(v1);
    p.header.frame_id = "/kopt_frame";
    p.header.stamp = ros::Time::now();
    p.header.seq = k;
    mesh->push_back(p);
    k++;
  }
  free(line);
  f.close();
  return mesh;
}

The Code Explained

Set up publisher for obstacles and mesh visualization. The service client is used to call the SIP.

  ros::NodeHandle n;
  ros::Publisher obstacle_pub = n.advertise<visualization_msgs::Marker>
    ("scenario", 1);
  ros::Publisher stl_pub = n.advertise<nav_msgs::Path>("stl_mesh", 1);
  ros::ServiceClient client = n.serviceClient<koptplanner::inspection>
    ("inspectionPath");

Delay publishing of display elements to avoid buffer overflow.

  ros::Rate r(50.0);
  ros::Rate r2(1.0);
  r2.sleep();

Instantiate the datastructure to call the service with.

  koptplanner::inspection srv;

Define the bounding box of the operating space.

  srv.request.spaceSize.push_back(1375);
  srv.request.spaceSize.push_back(2165);
  srv.request.spaceSize.push_back(0.001);
  srv.request.spaceCenter.push_back(1375.0/2.0);
  srv.request.spaceCenter.push_back(2165.0/2.0);
  srv.request.spaceCenter.push_back(200.0);

Fill the vector srv.request.requiredPoses with all fixed poses that should be included in the path. The first pose is interpreted as starting point, while the last (if more than one is provided) is the end pose of the inspection path. At least one pose has to be provided.

  geometry_msgs::Pose reqPose;

  /* starting pose */
  reqPose.position.x = 300.0;
  reqPose.position.y = 300.0;
  reqPose.position.z = 200.0;
  tf::Quaternion q = tf::createQuaternionFromRPY(0.0, 0.0, 0.0);
  reqPose.orientation.x = q.x();
  reqPose.orientation.y = q.y();
  reqPose.orientation.z = q.z();
  reqPose.orientation.w = q.w();
  srv.request.requiredPoses.push_back(reqPose);

  /* final pose */
  reqPose.position.x = 400.0;
  reqPose.position.y = 300.0;
  reqPose.position.z = 200.0;
  q = tf::createQuaternionFromRPY(0.0, 0.0, 0.0);
  reqPose.orientation.x = q.x();
  reqPose.orientation.y = q.y();
  reqPose.orientation.z = q.z();
  reqPose.orientation.w = q.w();
  srv.request.requiredPoses.push_back(reqPose);

Four inspection planning parameters have to be chosen at this point, corresponding to the minimum incidence angle, the minimal and maximal inspection distances, as well as the number of iterations of the planner.

  srv.request.incidenceAngle = M_PI/6.0;
  srv.request.minDist = 40.0;
  srv.request.maxDist = 300.0;
  srv.request.numIterations = 20;

Read the mesh and publish it to rviz for visualization.

  std::vector<nav_msgs::Path> * mesh = readSTLfile(ros::package::getPath(
    "request")+"/meshes/regularPlanes/rPlane.stl");
  for(std::vector<nav_msgs::Path>::iterator it = mesh->begin();
      it != mesh->end() && ros::ok(); it++)
  {
    stl_pub.publish(*it);
    geometry_msgs::Polygon p;
    geometry_msgs::Point32 p32;
    p32.x = it->poses[0].pose.position.x;
    p32.y = it->poses[0].pose.position.y;
    p32.z = it->poses[0].pose.position.z;
    p.points.push_back(p32);
    p32.x = it->poses[1].pose.position.x;
    p32.y = it->poses[1].pose.position.y;
    p32.z = it->poses[1].pose.position.z;
    p.points.push_back(p32);
    p32.x = it->poses[2].pose.position.x;
    p32.y = it->poses[2].pose.position.y;
    p32.z = it->poses[2].pose.position.z;
    p.points.push_back(p32);
    srv.request.inspectionArea.push_back(p);
    r.sleep();
  }

Define obstacle regions. They can be given as cuboids. Also publish the obstacles to rviz for visualization.

  shape_msgs::SolidPrimitive body;
  body.type = shape_msgs::SolidPrimitive::BOX;
  body.dimensions.push_back(40.0);
  body.dimensions.push_back(50.0);
  body.dimensions.push_back(4.0);
  srv.request.obstacles.push_back(body);
  geometry_msgs::Pose pose;
  pose.position.x = 600.0;
  pose.position.y = 600.0;
  pose.position.z = 200.0;
  pose.orientation.x = 0.0;
  pose.orientation.y = 0.0;
  pose.orientation.z = 0.0;
  pose.orientation.w = 1.0;
  srv.request.obstaclesPoses.push_back(pose);
  srv.request.obstacleIntransparancy.push_back(0);

  // publish obstacles for rviz 
  visualization_msgs::Marker marker;
  marker.header.frame_id = "/kopt_frame";
  marker.header.stamp = ros::Time::now();
  marker.ns = "obstacles";
  marker.id = 0; // enumerate when adding more obstacles
  marker.type = visualization_msgs::Marker::CUBE;
  marker.action = visualization_msgs::Marker::ADD;

  marker.pose.position.x = pose.position.x;
  marker.pose.position.y = pose.position.y;
  marker.pose.position.z = pose.position.z;
  marker.pose.orientation.x = pose.orientation.x;
  marker.pose.orientation.y = pose.orientation.y;
  marker.pose.orientation.z = pose.orientation.z;
  marker.pose.orientation.w = pose.orientation.w;

  marker.scale.x = body.dimensions[0];
  marker.scale.y = body.dimensions[1];
  marker.scale.z = body.dimensions[2];

  marker.color.r = 0.0f;
  marker.color.g = 0.0f;
  marker.color.b = 1.0f;
  marker.color.a = 0.5;

  marker.lifetime = ros::Duration();
  obstacle_pub.publish(marker);
  r.sleep();

Call the SIP service with the prepared datastructure srv.

  if (client.call(srv))
  {
    ROS_INFO("Successfully planned inspection path");
  }
  else
  {
    ROS_ERROR("Failed to call service planner");
    return 1;
  }

Use the following function to read a non-binary .stl mesh-file.

std::vector<nav_msgs::Path> * readSTLfile(std::string name)
{
  std::vector<nav_msgs::Path> * mesh = new std::vector<nav_msgs::Path>;
  std::fstream f;
  f.open(name.c_str());
  assert(f.is_open());
  int MaxLine = 0;
  char* line;
  double maxX = -DBL_MAX;
  double maxY = -DBL_MAX;
  double maxZ = -DBL_MAX;
  double minX = DBL_MAX;
  double minY = DBL_MAX;
  double minZ = DBL_MAX;
  assert(line = (char *) malloc(MaxLine = 80));
  f.getline(line, MaxLine);
  if(0 != strcmp(strtok(line, " "), "solid"))
  {
    ROS_ERROR("Invalid mesh file! Give in ascii-format.");
    ros::shutdown();
  }
  assert(line = (char *) realloc(line, MaxLine));
  f.getline(line, MaxLine);
  int k = 0;
  while(0 != strcmp(strtok(line, " "), "endsolid") && !ros::isShuttingDown())
  {
    int q = 0;
    nav_msgs::Path p;
    geometry_msgs::PoseStamped v1;
    for(int i = 0; i<7; i++)
    {
      while(line[q] == ' ')
        q++;
      if(line[q] == 'v')
      {
        // used to rotate the mesh before processing
        const double yawTrafo = 0.0;
        // used to scale the mesh before processing
        const double scaleFactor = 1.0;
        // used to offset the mesh before processing
        const double offsetX = 0.0;
        // used to offset the mesh before processing
        const double offsetY = 0.0;
        // used to offset the mesh before processing
        const double offsetZ = 0.0;

        geometry_msgs::PoseStamped vert;
        char* v = strtok(line+q," ");
        v = strtok(NULL," ");
        double xtmp = atof(v)/scaleFactor;
        v = strtok(NULL," ");
        double ytmp = atof(v)/scaleFactor;
        vert.pose.position.x = cos(yawTrafo)*xtmp-sin(yawTrafo)*ytmp;
        vert.pose.position.y =  sin(yawTrafo)*xtmp+cos(yawTrafo)*ytmp;
        v = strtok(NULL," ");
        vert.pose.position.z =  atof(v)/scaleFactor;
        vert.pose.position.x -= offsetX;
        vert.pose.position.y -= offsetY;
        vert.pose.position.z -= offsetZ;
        if(maxX<vert.pose.position.x)
          maxX=vert.pose.position.x;
        if(maxY<vert.pose.position.y)
          maxY=vert.pose.position.y;
        if(maxZ<vert.pose.position.z)
          maxZ=vert.pose.position.z;
        if(minX>vert.pose.position.x)
          minX=vert.pose.position.x;
        if(minY>vert.pose.position.y)
          minY=vert.pose.position.y;
        if(minZ>vert.pose.position.z)
          minZ=vert.pose.position.z;
        vert.pose.orientation.x =  0.0;
        vert.pose.orientation.y =  0.0;
        vert.pose.orientation.z =  0.0;
        vert.pose.orientation.w =  1.0;
        p.poses.push_back(vert);
        if(p.poses.size() == 1)
          v1 = vert;
      }
      assert(line = (char *) realloc(line, MaxLine));
      f.getline(line, MaxLine);
    }
    p.poses.push_back(v1);
    p.header.frame_id = "/kopt_frame";
    p.header.stamp = ros::Time::now();
    p.header.seq = k;
    mesh->push_back(p);
    k++;
  }
  free(line);
  f.close();
  return mesh;
}