The ROS interface¶

The tutorial shows the main services offered by the ROS nodes kautham_node and kautham_node_vis:

Problem services: to open/close the problem to be used.
Robot services to mange the robot(s).
Obstacle services: to modify the environment.
Planner services: to tune the planner and set the queries.
Solution services: to retrieve and analyze the solutions.
Visualization services: to visualize the kautham scene in rviz.

Problem services¶

OpenProblem¶

The kautham_node/OpenProblem service is used to open a problem, given by a problem file and a model folder.

The request and response defined in the OpenProblem.srv file are:

string problem
string[] dir
---
bool response

An example of a function that wraps the call to this service is:

//! Function that wraps the call to the kautham service that opens a problem
bool kauthamOpenProblem( std::string modelFolder, std::string problemFile )
{
   ros::NodeHandle n;
   ros::service::waitForService("/kautham_node/OpenProblem");

   kautham::OpenProblem kthopenproblem_srv;
   kthopenproblem_srv.request.problem = problemFile;
   kthopenproblem_srv.request.dir.resize(1);
   kthopenproblem_srv.request.dir[0] = modelFolder;

   ros::ServiceClient kthopenproblem_client = n.serviceClient<kautham::OpenProblem>("/kautham_node/OpenProblem");
   kthopenproblem_client.call(kthopenproblem_srv);

   if (kthopenproblem_srv.response.response == true) {
       ROS_INFO( "Kautham Problem opened correctly" );
   } else {
       ROS_ERROR( "ERROR Opening Kautham Problem" );
       ROS_ERROR( "models folder: %s", kthopenproblem_srv.request.dir[0].c_str() );
       ROS_ERROR( "problem file: %s", kthopenproblem_srv.request.problem.c_str() );
   }
}

CloseProblem¶

The kautham_node/CloseProblem service is used to close a problem.

The request and response defined in the CloseProblem.srv file are:

---

An example of a function that wraps the call to this service is:

//! Function that wraps the call to the kautham service that closes a problem
void kauthamCloseProblem()
{
   ros::NodeHandle nh;

   //close kautham problem
   ros::service::waitForService("/kautham_node/CloseProblem");
   kautham::CloseProblem kthcloseproblem_srv;
   ros::ServiceClient  kthcloseproblem_client;

   kthcloseproblem_client = nh.serviceClient<kautham::CloseProblem>("/kautham_node/CloseProblem");
   kthcloseproblem_client.call(kthcloseproblem_srv);

   std::cout << "Kautham Problem closed" << std::endl;
}

Robot services¶

SetRobotsConfig¶

The kautham_node/SetRobotsConfig service is used to set the configuration of the robot(s) given by a control vector.

The request and response defined in the SetRobotsConfig.srv file are:

float32[] config
---
bool response

An example of a function that wraps the call to this service is:

//! Function that wraps the call to the kautham service that moves the robot
bool kauthamMoveRobot(std::vector<double> controls)
{
   ros::NodeHandle nh;

   //set robot config
   //Define server and client
   ros::service::waitForService("/kautham_node/SetRobotsConfig");
   kautham::SetRobotsConfig setrobotconfig_srv;
   ros::ServiceClient  setrobotconfig_client;

   setrobotconfig_client = nh.serviceClient<kautham::SetRobotsConfig>("/kautham_node/SetRobotsConfig");
   setrobotconfig_srv.request.config.resize(controls.size());
   for(int i=0; i<controls.size();i++)
       setrobotconfig_srv.request.config[i] = controls[i];
   setrobotconfig_client.call(setrobotconfig_srv);

   if (setrobotconfig_srv.response.response == true)
       std::cout << "Robot moved correctly" << std::endl;
   else
       std::cout << "ERROR failed moveRobot" << std::endl;

   return setrobotconfig_srv.response.response;
}

SetRobControls¶

The kautham_node/SetRobControls service is used to set the file that define the controls to be used to move the degrees of freedom of the robot(s). It also sets the query as the initial and goal control vectors.

The request and response defined in the SetRobControls.srv file are:

string controls
float32[] init
float32[] goal
---
bool response

An example of a function that wraps the call to this service is:

//! Function that wraps the call to the kautham service that changes the control file
bool kauthamSetControlFile(std::string controlfilename, std::vector<double> init, std::vector<double> goal)
{
   ros::NodeHandle nh;

   //set controlfile
   //Define server and client
   ros::service::waitForService("kautham_node/SetRobControls");
   kautham::SetRobControls setrobcontrols_srv;
   ros::ServiceClient  setrobcontrols_client;

   setrobcontrols_client = nh.serviceClient<kautham::SetRobControls>("/kautham_node/SetRobControls");
   //Load the filename
   setrobcontrols_srv.request.controls = controlfilename;
   //Load the query request (init and goal)
   setrobcontrols_srv.request.init.resize(init.size());
   setrobcontrols_srv.request.goal.resize(goal.size());
   for(int i=0; i<init.size();i++)
   {
       setrobcontrols_srv.request.init[i] = init[i];
       setrobcontrols_srv.request.goal[i] = goal[i];
   }
   setrobcontrols_client.call(setrobcontrols_srv);


   std::cout << "New control file is" <<controlfilename<<std::endl;
   return setrobcontrols_srv.response.response;
}

SetRobControlsNoQuery¶

The kautham_node/SetRobControlsNoQuery service is used to set the file that define the controls to be used to move the degrees of freedom of the robot(s). Recall to change the query accordingly to the new controls.

The request and response defined in the SetRobControlsNoQuery.srv file are:

string controls
---
bool response

An example of a function that wraps the call to this service is:

//! Function that wraps the call to the kautham service that changes the control file
bool kauthamSetControlFileNoQuery(std::string controlfilename)
{
   ros::NodeHandle nh;

   //set controlfile
   //Define server and client
   ros::service::waitForService("kautham_node/SetRobControlsNoQuery");
   kautham::SetRobControlsNoQuery setrobcontrolsnoquery_srv;
   ros::ServiceClient setrobcontrolsnoquery_client;

   setrobcontrolsnoquery_client = nh.serviceClient<kautham::SetRobControlsNoQuery>("/kautham_node/SetRobControlsNoQuery");
   //Load the filename
   setrobcontrolsnoquery_srv.request.controls = controlfilename;
   setrobcontrolsnoquery_client.call(setrobcontrolsnoquery_srv);


   std::cout << "New control file is" <<controlfilename<<std::endl;
   return setrobcontrolsnoquery_srv.response.response;
}

SetDefaultRobControls¶

The kautham_node/SetDefaultRobControls service is used to reset the controls to the default ones (one per d.o.f.), and also sets the new init and goal control values.

The request and response defined in the SetDefaultRobControls.srv file are:

float32[] init
float32[] goal
---
bool response

SetRobotPos / GetRobpos¶

The kautham_node/SetRobotPos and kautham_node/GetRobotPos services are used to set/get the pose of the robot (x,y,z,qx,qy,qz,qw).

The request and response are defined in the ObsPos.srv file as:

int32 index
float32[] setPos
---
bool response
float32[] getPos


   ros::ServiceServer service41 = n.advertiseService("kautham_node/GetRobotHomePos",srvGetRobHomePos);
   ros::ServiceServer service38 = n.advertiseService("kautham_node/FindIK",srvFindIK);

An example of a function that wraps the call to this service is:

//! Function that wraps the call to the kautham service thatsets robot home pos
bool kauthamSetRobotPos(int index,  std::vector <float> pos)
{
   // activate the setRobotPos service
   ros::NodeHandle node;
   ros::service::waitForService("/kautham_node/SetRobotPos");
   ros::ServiceClient set_robot_client = node.serviceClient<kautham::ObsPos>("/kautham_node/SetRobotPos");

   kautham::ObsPos set_robot_srv;

   set_robot_srv.request.index = index;
   set_robot_srv.request.setPos = pos;

   //call kautham service to set the obstacle pos
   set_robot_client.call(set_robot_srv);

   // Evaluate the set obstacle service and perform rest of process
   if (set_robot_srv.response.response == false)
   {
       ROS_ERROR("SetRobotPos service has not been performed. ");
       return false;
   }
   else
   {
       ROS_INFO("SetRobotPos service has been performed !");
       return true;
   }
}

Obstacle services¶

AddObstacle¶

The kautham_node/AddObstacle service is used to add a new obstacle to the scene.

The request and response defined in the AddObstacle.srv file are:

string obsname
float32 scale
float32[] home
---
int64 response

RemoveObstacle¶

The kautham_node/RemoveObstacle service is used to remove an obstacle from the scene.

The request and response defined in the RemoveObstacle.srv file are:

string obsname
---
bool response

SetObstaclePos / GetObstaclePos¶

The kautham_node/SetObstaclePos and kautham_node/GetObstaclePos services are used to set/get the pose of an object.

The request and response are defined in the ObsPos.srv file as:

string obsname
float32[] setPos
---
bool response
float32[] getPos

AttachObstacle2RobotLink¶

The kautham_node/AttachObstacle2RobotLink service is used to attach an object to a link of a robot.

The request and response are defined in the AttachObstacle2RobotLink.srv file as:

int64 robot
int64 link
string obs
---
bool response

An example of a function that wraps the call to this service is:

//! Function that wraps the call to the kautham service that attaches an object to a given link
bool kauthamAttachObject(int robotnumber, int linknumber, std::string objectname)
{
  ros::NodeHandle nh;

  //attach obstacle to robot link
  ros::service::waitForService("/kautham_node/AttachObstacle2RobotLink");
  kautham::AttachObstacle2RobotLink attachobstacle2robotlink_srv;
  ros::ServiceClient  attachobstacle2robotlink_client;

  attachobstacle2robotlink_client = nh.serviceClient<kautham::AttachObstacle2RobotLink>("/kautham_node/AttachObstacle2RobotLink");
  attachobstacle2robotlink_srv.request.robot = robotnumber;
  attachobstacle2robotlink_srv.request.link = linknumber;
  attachobstacle2robotlink_srv.request.obs = objectname;
  attachobstacle2robotlink_client.call(attachobstacle2robotlink_srv);

  std::cout << "Attached object "<< objectname << " to link " << linknumber<< " of robot "<< robotnumber<< std::endl;

  return attachobstacle2robotlink_srv.response.response;
}

DetachObstacle¶

The kautham_node/DetachObstacle service is used to detach an object from where it was attached.

The request and response are defined in the DetachObstacle.srv file as:

string obsname
---
bool response

An example of a function that wraps the call to this service is:

//! Function that wraps the call to the kautham service that dettaches an attached object
bool kauthamDetachObject(std::string objectname)
{
  ros::NodeHandle nh;

  //detach obstacle to robot link
  ros::service::waitForService("/kautham_node/AttachObstacle2RobotLink");
  kautham::DetachObstacle detachobstacle_srv;
  ros::ServiceClient  detachobstacle_client;

  detachobstacle_client = nh.serviceClient<kautham::DetachObstacle>("/kautham_node/DetachObstacle");
  detachobstacle_srv.request.obsname = objectname;
  detachobstacle_client.call(detachobstacle_srv);

  std::cout << "Detached object "<< objectname << std::endl;

  return detachobstacle_srv.response.response;
}

Planner services¶

SetInit¶

The kautham_node/SetInit service is used to set the initial configuration of the path planning query, as a set of control values.

The request and response are defined in the SetInit.srv file as:

float32[] init
---
bool response

SetGoal¶

The kautham_node/SetGoal service is used to set the goal configuration of the path planning query, as a set of control values.

The request and response are defined in the SetGoal.srv file as:

float32[] goal
---
bool response

SetQuery¶

The kautham_node/SetQuery service is used to set the initial and goal configuration of the path planning query, as sets of control values.

The request and response are defined in the SetQuery.srv file as:

float32[] init
float32[] goal
---
bool response

An example of a function that wraps the call to this service is:

//! Function that wraps the call to the kautham service that sets a new query to be solved
bool kauthamSetQuery(std::vector<double> init, std::vector<double> goal)
{
   //Define the handle
   ros::NodeHandle nh;

   //Set query
   //Define server and client
   ros::service::waitForService("/kautham_node/SetQuery");
   kautham::SetQuery setquery_srv;
   ros::ServiceClient  setquery_client;
   setquery_client = nh.serviceClient<kautham::SetQuery>("/kautham_node/SetQuery");

   //Load the query request (init and goal)
   setquery_srv.request.init.resize(init.size());
   setquery_srv.request.goal.resize(goal.size());
   for(int i=0; i<init.size();i++)
   {
       setquery_srv.request.init[i] = init[i];
       setquery_srv.request.goal[i] = goal[i];
   }
   //Call the query service
   setquery_client.call(setquery_srv);

   //Listen the response
   if(setquery_srv.response.response==false)
   {
       std::cout<<"Query has not been set\n";
       return false;
   }
}

SetPlannerByName¶

The kautham_node/SetPlannerByName service is used to set the initial and goal configuration of the path planning query, as sets of control values.

The request and response are defined in the SetPlannerByName.srv file as:

string name
---
bool response

An example of a function that wraps the call to this service is:

//! Function that wraps the call to the kautham service that sets a planner to solve the opened problem
bool kauthamSetPlannerByName(std::string plannername)
{
   ros::NodeHandle nh;

   //set planner name
   ros::service::waitForService("/kautham_node/SetPlannerByName");
   kautham::SetPlannerByName setplannerbyname_srv;
   ros::ServiceClient  setplannerbyname_client;

   setplannerbyname_client = nh.serviceClient<kautham::SetPlannerByName>("/kautham_node/SetPlannerByName");
   setplannerbyname_srv.request.name = plannername;
   setplannerbyname_client.call(setplannerbyname_srv);

   return setplannerbyname_srv.response.response;
}

SetPlannerParameter¶

The kautham_node/SetPlannerParameter service is used to set the planner parameters.

The request and response are defined in the SetPlannerParameter.srv file as:

string parameter
string value
---
bool response

An example of a function that wraps the call to this service is:

//! Function that wraps the call to the kautham service that sets the planner parameters
bool kauthamSetPlannerParameter(std::string parametername,std::string parametervalue)
{
   ros::NodeHandle nh;

   //set planner parameter
   //Define server and client
   ros::service::waitForService("/kautham_node/SetPlannerParameter");
   kautham::SetPlannerParameter setplannerparameter_srv;
   ros::ServiceClient  setplannerparameter_client;

   setplannerparameter_client = nh.serviceClient<kautham::SetPlannerParameter>("/kautham_node/SetPlannerParameter");
   setplannerparameter_srv.request.parameter = parametername;
   setplannerparameter_srv.request.value = parametervalue;
   setplannerparameter_client.call(setplannerparameter_srv);

   return setplannerparameter_srv.response.response;
}

PathDofNames¶

The kautham_node/PathDofNames service is used to get information on the configrations of a solution path obtained by the GetPath service.

The request and response are defined in the PathDofNames.srv file as:

---
string[] dofnames

e.g. for the Tiago we have that each configuration of the path has 23 values according to:

dofnames =  ['x', 'y', 'z', 'qx', 'qy', 'qz', 'qw', 'wheel_right_joint', 'wheel_left_joint', 'torso_fixed_joint', 'torso_lift_joint', 'head_1_joint', 'head_2_joint', 'arm_1_joint', 'arm_2_joint', 'arm_3_joint', 'arm_4_joint', 'arm_5_joint', 'arm_6_joint', 'arm_7_joint', 'gripper_joint', 'gripper_right_finger_joint', 'gripper_left_finger_joint']

GetPath¶

The kautham_node/GetPath service is used to get the solution to a query.

The request and response are defined in the Solve.srv file as:

---
fVector[] response

Where fVector is defined in fVector.msg file as:

float32[] v

An example of a function that wraps the call to this service is:

//! Function that wraps the call to the kautham service that solves a problem
bool kauthamGetPath(int printpath=0)
{
   //Define the handle
   ros::NodeHandle nh;

   //Find path
   //Define server and client
   ros::service::waitForService("/kautham_node/GetPath");
   kautham::GetPath getpath_srv;
   ros::ServiceClient  getpath_client;
   getpath_client = nh.serviceClient<kautham::GetPath>("/kautham_node/GetPath");

   //Call the getpath service
   getpath_client.call(getpath_srv);

   //Listen the response
   if(getpath_srv.response.response.size()==0)
   {
       std::cout<<"No path found\n";
       return false;
   }
   else
   {
       std::cout<<"Path found:";
       if(printpath){
         for(int i=0; i<getpath_srv.response.response.size(); i++)
         {
           std::cout<<"( ";
           for(int j=0;j<getpath_srv.response.response[i].v.size();j++)
           {
               std::cout<<getpath_srv.response.response[i].v[j]<<" ";
           }
           std::cout<<")\n";
         }
       }
       return true;
   }
}

CheckCollision¶

The kautham_node/CheckCollision service is used to check the collisions between the robot(s) and any obstacle. If a collision is found the service reports the objects that are in collision.

The request and response are defined in the CheckCollision.srv file as:

float32[] config
---
bool response
bool collisionFree
string msg
uint32 collidingRob
string collidedObs

An example of a function that wraps the call to this service is:

//! Function that wraps the call to the kautham service that checks for collision
bool kauthamIsCollisionFree(std::vector<double> controls)
{
   ros::NodeHandle nh;

   //check for collision
   //Define server and client
   ros::service::waitForService("/kautham_node/CheckCollision");
   kautham::CheckCollision checkcollision_srv;
   ros::ServiceClient  checkcollision_client;

   checkcollision_client = nh.serviceClient<kautham::CheckCollision>("/kautham_node/CheckCollision");
   checkcollision_srv.request.config.resize(controls.size());
   for(int i=0; i<controls.size();i++)
       checkcollision_srv.request.config[i] = controls[i];
   checkcollision_client.call(checkcollision_srv);

   return checkcollision_srv.response.collisionFree;
}

Connect¶

The kautham_node/Connect service is used to check if two configurations (sample1 and samle2 defined by its control vectors) can be connected by a collision-free rectilinear segment in Csapce.

The request and response are defined in the Connect.srv file as:

float32[] sample1
float32[] sample2
---
bool response

Solution services¶

GetLastPlanComputationTime¶

The kautham_node/GetLastPlanComputationTime service is used to get the computational time of the last call to the planner.

The request and response are defined in the Connect.srv file as:

---
float64 time

An example of a function that wraps the call to this service is:

//! Function that wraps the call to the kautham service that gets the computational time spent solving a query
bool kauthamGetPlannerComputationTime()
{
   ros::NodeHandle nh;

   //get computational time
   //Define server and client
   ros::service::waitForService("/kautham_node/GetLastPlanComputationTime");
   kautham::GetLastPlanComputationTime getlastplancomputationtime_srv;
   ros::ServiceClient  getlastplancomputationtime_client;

   getlastplancomputationtime_client = nh.serviceClient<kautham::GetLastPlanComputationTime>("/kautham_node/GetLastPlanComputationTime");
   getlastplancomputationtime_client.call(getlastplancomputationtime_srv);


   std::cout << "Computation Time = " << getlastplancomputationtime_srv.response.time <<std::endl;

   return getlastplancomputationtime_srv.response.time;
}

GetNumEdges / GetNumVertices¶

The kautham_node/GetNumEdges and kautham_node/GetNumVertices services are used to get the number of edges/vertices of the planner data structure.

The request and response are defined in the GetNumEdges.srv and GetNumVertices.srv files as:

---
int32 num

Visualization services¶

The kautham_node/srvVisualizeScene service is used to visualize the kautham scene in rviz.

The request and response are defined in the VisualizeScene.srv file as:

bool guisliders
string rvizfile
---
bool response

Python client files and launch files have been prepared to test the visualization features for the Chess demo and the Tiago-kitchen demo.

A configuration file has to be added in the folder of the demo to configure the problem file, the demo folder and the rviz configuration file, e.g. for the Chess demo:

<?xml version="1.0"?>
<Config>
   <Problemfiles>
       <kautham name="OMPL_RRTconnect_chess_ur3_gripper_2_simple.xml" />
       <directory name="/demos/OMPL_geo_demos/chess/"/>
       <rviz name="config/kautham_chess.rviz" />
   </Problemfiles>
</Config>

You can try it with the following demos:

$ roslaunch kautham kautham_tiago_kitchen_right.launch

$ roslaunch kautham kautham_chess.launch

An example¶

The following main function uses the functions shown above that wrap the different services

//! main function of kautham-client.
int main (int argc, char **argv)
{
   ROS_INFO("**** starting kautham_client_node ****");
   ros::init(argc, argv, "kautham_client_node_tiago");
   ros::NodeHandle n;

   //open problem
   std::string kauthamclient_path = ros::package::getPath("kauthamclient");
   std::string modelFolder = kauthamclient_path + "/demos/models/";
   std::string problemFile = kauthamclient_path + "/demos/OMPL_geo_demos/Tiago-kitchen/tiago_mobile_counterB_counterA.xml";
   kauthamOpenProblem(modelFolder, problemFile);

   //solve query
   std::cout << std::endl;
   std::cout << "TESTING getPath service." <<std::endl;
   std::cout<<"\nPRESS A KEY TO START..."<<std::endl;
   std::cin.get();

   std::cout << std::endl;
   std::cout << "Solving problem. Waiting for the solution" <<std::endl;
   kauthamGetPath(1);

   //compute time
   kauthamGetPlannerComputationTime();

   //change controlfile with query included
   std::cout << std::endl;
   std::cout << "TESTING setControlFile service to change the control file" <<std::endl;
   std::cout << "New control file moves the arm" <<std::endl;
   std::cout << "\nPRESS A KEY TO CONTINUE..."<<std::endl;
   std::cin.get();

   std::vector<double> arminit = {0.459, 0.023, 0.628, 0.688, 0.820, 0.083, 0.974, 0.553, 1.000};
   std::vector<double> armgoal = {0.846, 0.944, 0.861, 0.786, 0.594, 0.226, 0.850, 0.429, 1.000};
   kauthamSetControlFileNoQuery("controls/tiago_simple_only_arm_gripper_counterA.cntr");
   kauthamSetQuery(arminit,armgoal);

   std::cout << std::endl;
   std::cout<<"\nPRESS A KEY TO START GETPATH..."<<std::endl;
   std::cin.get();

   std::cout << "Solving problem. Waiting for the solution" <<std::endl;
   kauthamGetPath(1);

   std::cout << std::endl;
   std::cout<<"\nPRESS A KEY TO CLOSE..."<<std::endl;
   std::cin.get();

   //compute time
   kauthamGetPlannerComputationTime();

   //close problem
   kauthamCloseProblem();

   //THE END
   return 0;
}