The ROS interface
The tutorial introduces the main services provided by the kautham_ros_node, explaining their purpose and how they can be used:
Problem services: to open or close the problem to be solved.
Robot services: to manage the robot(s).
Obstacle services: to modify the environment.
Planner services: to configure the planner and define queries.
Constraints services: to modify the defined constraints.
Solution services: to retrieve and analyze solutions.
Visualization services: to visualize the Kautham scene in RViz.
The code examples are part of the kautham_rclcpp_client class, which wraps service calls into independent methods. This allows the class to be inherited and its methods reused in a custom class without the need for reimplementation. The examples do not show the creation and initialization of the service object, since it is declared directly as a class instance and initialized in its constructor.
Problem services
OpenProblem
The kautham_node/OpenProblem service is used to open a problem defined by a problem file and an associated model folder.
The request and response are defined in the OpenProblem.srv file as follows:
string problem
string[] dir
---
bool response
Below is an example of a function that wraps the call to this service:
bool KauthamRCLCPPClient::kauthamOpenProblem(const std::string& _model_folder, const std::string& _problem_file )
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::OpenProblem::Request>();
srv_request->problem = _problem_file;
srv_request->dir = {_model_folder};
auto future_result = this->client_kthm_open_problem->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_open_problem->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (!result->response) {
RCLCPP_WARN(this->get_logger(),"The Kautham problem (%s) cannot be opened.", srv_request->problem.c_str());
}
return result->response;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service.", this->client_kthm_open_problem->get_service_name());
return false;
}
}
ProblemOpened
The kautham_node/ProblemOpened service is used to check if a problem has been opened.
The request and response are defined in the ProblemOpened.srv file as follows:
---
bool response
Below is an example of a function that wraps the call to this service:
bool KauthamRCLCPPClient::kauthamProblemOpened()
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::ProblemOpened::Request>();
auto future_result = this->client_kthm_problem_opened->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_problem_opened->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (!result->response) {
RCLCPP_WARN(this->get_logger(),"There is no Kautham problem opened.");
}
return result->response;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service.", this->client_kthm_problem_opened->get_service_name());
return false;
}
}
CloseProblem
The kautham_node/CloseProblem service is used to close the opened problem.
The request and response are defined in the CloseProblem.srv file as follows:
---
Below is an example of a function that wraps the call to this service:
void KauthamRCLCPPClient::kauthamCloseProblem()
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::CloseProblem::Request>();
auto future_result = this->client_kthm_close_problem->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_close_problem->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
return true;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service.", this->client_kthm_close_problem->get_service_name());
return false;
}
}
Robot services
UpdateRobotDOFOffsetByName
The kautham_node/UpdateRobotDOFOffsetByName service is used to set/update the predefined DOF offsets of the robot(s) specified in the control file.
The request and response are defined in the UpdateRobotDOFOffsetByName.srv file as follows:
string robot_name
string[] dof_names
float64[] dof_values
---
bool success
Below is an example of a function that wraps the call to this service:
bool KauthamRCLCPPClient::kauthamUpdateRobotDOFOffsetByName(const std::string& _robot_name, const std::vector<std::string>& _dof_names, const std::vector<double>& _dof_values)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::UpdateRobotDOFOffsetByName::Request>();
srv_request->robot_name = _robot_name;
srv_request->dof_names = _dof_names;
srv_request->dof_values = _dof_values;
auto future_result = this->client_kthm_update_robot_dof_offset_by_name->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_update_robot_dof_offset_by_name->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (!result->success) {
RCLCPP_WARN(this->get_logger(),"The DOF offsets cannot be set.");
}
return result->success;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service.", this->client_kthm_update_robot_dof_offset_by_name->get_service_name());
return false;
}
return true;
}
SetRobotsConfig
The kautham_node/SetRobotsConfig service allows setting a robot (or multiple robots) to a specific configuration, different from the initial or goal states, by using the defined controls. The service then returns this configuration in joint space.
The request and response are defined in the SetRobotsConfig.srv file as follows:
float64[] controls
---
kautham_ros_interfaces/Robconf[] config
bool response
Below is an example of a function that wraps the call to this service:
bool KauthamRCLCPPClient::kauthamMoveRobot(const std::vector<double>& _controls, std::vector<geometry_msgs::msg::Pose>& robots_poses_, std::vector<std::vector<double>>& robots_joints_)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::SetRobotsConfig::Request>();
srv_request->controls = _controls;
auto future_result = this->client_kthm_move_robot->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_move_robot->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (!result->response) {
RCLCPP_WARN(this->get_logger(),"The requested configuration cannot be set.");
}
robots_poses_ = config.base;
robots_joints_ = config.joints;
return result->response;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service.", this->client_kthm_move_robot->get_service_name());
return false;
}
return true;
}
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 RobPos.srv file as follows:
int64 index
float64[] setpos
---
bool response
float64[] getpos
Below is an example of a function that wraps the call to this service:
bool KauthamRCLCPPClient::kauthamGetRobotPos(const uint _index, Eigen::AffineCompact3d& robot_pose_)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::RobPos::Request>();
srv_request->index = _index;
auto future_result = this->client_kthm_get_robot_pose->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_get_robot_pose->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (!result->response) {
RCLCPP_WARN(this->get_logger(),"The requested robot pose cannot be get.");
}
robot_pose_ = Eigen::Translation3d(getpos[0], getpos[1], getpos[2]) * Eigen::Quaterniond(getpos[6], getpos[3], getpos[4], getpos[5]);
return result->response;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service.", this->client_kthm_get_robot_pose->get_service_name());
return false;
}
return true;
}
bool KauthamRCLCPPClient::kauthamSetRobotPos(const uint _index, const Eigen::AffineCompact3d& _robot_pose)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::RobPos::Request>();
srv_request->index = _index;
setpos[0] = _robot_pose.translation.x();
setpos[1] = _robot_pose.translation.y();
setpos[2] = _robot_pose.translation.z();
setpos[3] = _robot_pose.quaterniond.x();
setpos[4] = _robot_pose.quaterniond.y();
setpos[5] = _robot_pose.quaterniond.z();
setpos[6] = _robot_pose.quaterniond.w();
auto future_result = this->client_kthm_set_robot_pose->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_set_robot_pose->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (!result->response) {
RCLCPP_WARN(this->get_logger(),"The requested robot pose cannot be set.");
}
return result->response;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service.", this->client_kthm_set_robot_pose->get_service_name());
return false;
}
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
float64 scale
float64[] 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
geometry_msgs/Pose set_pose
---
bool response
geometry_msgs/Pose get_pose
Below is an example of a function that wraps the call to this service:
bool KauthamRCLCPPClient::kauthamGetObstaclePos(const uint _obstacle_name, Eigen::AffineCompact3d& obstacle_pose_)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::ObsPos::Request>();
srv_request->obsname = _obstacle_name;
auto future_result = this->client_kthm_get_obstacle_pose->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_get_obstacle_pose->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (!result->response) {
RCLCPP_WARN(this->get_logger(),"The requested obstacle pose cannot be get.");
}
obstacle_pose_ = Eigen::Translation3d(get_pose.position.x, get_pose.position.y, get_pose.position.z) * Eigen::Quaterniond(get_pose.orientation.w, get_pose.orientation.x, get_pose.orientation.y, get_pose.orientation.z);
return result->response;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service.", this->client_kthm_get_obstacle_pose->get_service_name());
return false;
}
return true;
}
bool KauthamRCLCPPClient::kauthamSetObstaclePos(const uint _obstacle_name, const Eigen::AffineCompact3d& _obstalce_pose)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::RobPos::Request>();
srv_request->obsname = _obstacle_name;
set_pose.position.x = _obstalce_pose.translation.x();
set_pose.position.y = _obstalce_pose.translation.y();
set_pose.position.z = _obstalce_pose.translation.z();
set_pose.orientation.x = _obstalce_pose.quaterniond.x();
set_pose.orientation.y = _obstalce_pose.quaterniond.y();
set_pose.orientation.z = _obstalce_pose.quaterniond.z();
set_pose.orientation.w = _obstalce_pose.quaterniond.w();
auto future_result = this->client_kthm_set_obstalce_pose->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_set_obstalce_pose->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (!result->response) {
RCLCPP_WARN(this->get_logger(),"The requested obstalce pose cannot be set.");
}
return result->response;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service.", this->client_kthm_set_obstalce_pose->get_service_name());
return false;
}
return true;
}
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 follows:
string obstacle_name
string robot_name
string robot_link_name
---
bool response
Below is an example of a function that wraps the call to this service:
bool KauthamRCLCPPClient::kauthamAttachObstacle2RobotLink(const std::string& _robot_name, const std::string& _robot_link_name, const std::string& _obstacle_name)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::AttachObstacle2RobotLink::Request>();
srv_request->obstacle_name = requested_obstacle_name;
srv_request->robot_name = requested_robot_name;
srv_request->robot_link_name = requested_robot_link_name;
auto future_result = this->client_kthm_attach_obstacle_2_robot_link->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_attach_obstacle_2_robot_link->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (!result->response) {
RCLCPP_WARN(this->get_logger(),"The attach cannot be done.");
}
return result->response;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service.", this->client_kthm_attach_obstacle_2_robot_link->get_service_name());
return false;
}
return true;
}
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 follows:
string obstacle_name
---
bool response
Below is an example of a function that wraps the call to this service:
bool KauthamRCLCPPClient::kauthamDetachObject(const std::string& _obstacle_name)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::DetachObstacle::Request>();
srv_request->obstacle_name = requested_obstacle_name;
auto future_result = this->client_kthm_detach_obstacle->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_detach_obstacle->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (!result->response) {
RCLCPP_WARN(this->get_logger(),"The detach cannot be done.");
}
return result->response;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service.", this->client_kthm_detach_obstacle->get_service_name());
return false;
}
return true;
}
Planner services
SetRobotControlsNoQuery
The kautham_node/SetRobotControlsNoQuery service is used to set/change 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 are defined in the SetRobControlsNoQuery.srv file as follows:
string controls
---
bool response
Below is an example of a function that wraps the call to this service:
bool KauthamRCLCPPClient::kauthamSetRobotControlsNoQuery(const std::string& _control_filename)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::SetRobControlsNoQuery::Request>();
srv_request->controls = "controls/" + _control_filename;
auto future_result = this->client_kthm_set_robot_control_no_query->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_set_robot_control_no_query->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (!result->response) {
RCLCPP_WARN(this->get_logger(),"Not possible to set the robot (no query) controls: %s", srv_request->controls.c_str());
}
return result->response;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service.", this->client_kthm_set_robot_control_no_query->get_service_name());
return false;
}
}
SetControlsQuery
The kautham_node/SetQueryByControlName service is used to specify one or more control values of the path planning query by referring to their control names. It allows setting the initial configuration, the goal configuration, or both, and can be applied to all or only a subset of the controls in either configuration.
The request and response are defined in the SetControlsQuery.srv file as follows:
string[] control_names
float64[] init
float64[] goal
---
bool response
Below is an example of a function that wraps the call to this service:
bool KauthamRCLCPPClient::kauthamSetControlsQuery(const std::vector<std::string>& _control_names, const std::vector<double>& _init, const std::vector<double>& _goal)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::SetRobControlsNoQuery::Request>();
srv_request->control_names = _control_names;
srv_request->init = _init;
srv_request->goal = _goal;
auto future_result = this->client_kthm_set_control_query->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service to set the new control query.", this->client_kthm_set_control_query->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (!result->response) {
RCLCPP_WARN(this->get_logger(),"The new control query cannot be set.");
}
return result->response;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service to set the new control query.", this->client_kthm_set_control_query->get_service_name());
return false;
}
}
SetQuery [DEPRECATED, USE SetControlsQuery]
The kautham_node/SetQuery service is used to specify one or more control values of the path planning query. It allows setting the initial configuration, the goal configuration, or both. Must be applied to all the controls following the .cntr definition order.
The request and response are defined in the SetQuery.srv file as follows:
float64[] init
float64[] goal
---
bool response
Below is an example of a function that wraps the call to this service:
bool KauthamRCLCPPClient::kauthamSetQuery(const std::vector<double>& _init, const std::vector<double>& _goal)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::SetQuery::Request>();
srv_request->init = _init;
srv_request->goal = _goal;
auto future_result = this->client_kthm_set_query->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service to set the new query.", this->client_kthm_set_query->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (!result->response) {
RCLCPP_WARN(this->get_logger(),"The new query cannot be set.");
}
return result->response;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service to set the new query.", this->client_kthm_set_query->get_service_name());
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
Below is an example of a function that wraps the call to this service:
bool KauthamRCLCPPClient::kauthamSetPlannerByName(const std::string& _planner_name)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::SetPlannerByName::Request>();
srv_request->name = _planner_name;
auto future_result = this->client_kthm_set_planner_by_name->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service to set the new control query.", this->client_kthm_set_planner_by_name->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (!result->response) {
RCLCPP_WARN(this->get_logger(),"The new planner cannot be set.");
}
return result->response;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service to set the new control query.", this->client_kthm_set_planner_by_name->get_service_name());
return false;
}
}
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
Below is an example of a function that wraps the call to this service:
bool KauthamRCLCPPClient::kauthamSetPlannerParameter(const std::string& _parameter_name, const std::string& _parameter_value)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::SetPlannerParameter::Request>();
srv_request->parameter = _parameter_name;
srv_request->value = _parameter_value;
auto future_result = this->client_kthm_set_planner_param->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service to set the new control query.", this->client_kthm_set_planner_param->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (!result->response) {
RCLCPP_WARN(this->get_logger(),"The parameter cannot be set.");
}
return result->response;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service to set the new control query.", this->client_kthm_set_planner_param->get_service_name());
return false;
}
}
Constraints services
UpdateOrientationConstraint
The kautham_node/UpdateOrientationConstraint service is used modify some of the parameters of an existing orientation constraint.
The request and response are defined in the UpdateOrientationConstraint.srv file as follows:
# MAIN:
string robot # Name of the robot that use the constraint.
string id # Unique constraint identifier.
bool enabled # A constraint could be defined, but not used.
string target_link # The frame where the target is set.
# ORIENTATION:
geometry_msgs/Quaternion target_orientation # The desired orientation to maintain.
float64 tolerance_value # The tolerance of the constraint.
bool tolerance_variable # If the tolerance is fixed or variable in function of how far is the target_link from the goal.
float64 tolerance_grandient # The gradient in rad/m if the tolerance is variable.
geometry_msgs/Vector3 free_movement_axes # (x,y,z) bool axes, that indicates which axis will be free to move (when the constraint is used).
---
bool success # True or False
string msg # Indicates the error if the service fails; otherwise, it is empty.
Below is an example of a function that wraps the call to this service:
bool KauthamRCLCPPClient::kauthamUpdateOrientationConstraint(const std::string& _robot_name, const std::string& _constraint_identifier, const bool _enable, const std::string& _target_link, const geometry_msgs::msgs::Quaternion& _target_orientation, const double _tolerance_value, const bool _tolerance_variable, const double _tolerance_gradient, const geometry_msgs::msg::Vector3& _free_movement_axes)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::UpdateOrientationConstraint::Request>();
srv_request->robot = _robot_name;
srv_request->id = _constraint_identifier;
srv_request->enabled = _enable;
srv_request->target_link = _target_link;
srv_request->target_orientation = _target_orientation;
srv_request->tolerance_value = _tolerance_value;
srv_request->tolerance_variable = _tolerance_variable;
srv_request->tolerance_grandient = _tolerance_grandient;
srv_request->free_movement_axes = _free_movement_axes;
auto future_result = this->client_kthm_update_orientation_constraint->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_update_orientation_constraint->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (!result->success) {
RCLCPP_WARN(this->get_logger(),"Error when trying to update the orientation constraint %s from the robot %s: %s.", _constraint_identifier, _robot_name, result->msg.c_str());
}
return result->success;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service.", this->client_kthm_update_orientation_constraint->get_service_name());
return false;
}
}
UpdateGeometricConstraint
The kautham_node/UpdateGeometricConstraint service is used modify some of the parameters of an existing geometric constraint.
The request and response are defined in the UpdateGeometricConstraint.srv file as follows:
# MAIN:
string robot # Name of the robot that use the constraint.
string id # Unique constraint identifier.
bool enabled # A constraint could be defined, but not used.
string target_link # The frame where the target is set.
# GEOMETRIC:
geometry_msgs/Pose origin # The desired origin of the geometric constraint wrt the the reference frame link.
---
bool success # True or False
string msg # Indicates the error if the service fails; otherwise, it is empty.
Below is an example of a function that wraps the call to this service:
bool KauthamRCLCPPClient::kauthamUpdateGeometricConstraint(const std::string& _robot_name, const std::string& _constraint_identifier, const bool _enable, const std::string& _target_link, const geometry_msgs::msg::Pose& _origin)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::UpdateGeometricConstraint::Request>();
srv_request->robot = _robot_name;
srv_request->id = _constraint_identifier;
srv_request->enabled = _enable;
srv_request->target_link = _target_link;
srv_request->origin = _origin;
auto future_result = this->client_kthm_update_geometric_constraint->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_update_geometric_constraint->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (!result->success) {
RCLCPP_WARN(this->get_logger(),"Error when trying to update the geometric constraint %s from the robot %s: %s.", _constraint_identifier, _robot_name, result->msg.c_str());
}
return result->success;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service.", this->client_kthm_update_geometric_constraint->get_service_name());
return false;
}
}
Solution services
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:
float64[] config
---
bool response
bool collisionFree
string msg
uint32 collidingRob
string collidedObs
Below is an example of a function that wraps the call to this service:
bool KauthamRCLCPPClient::kauthamIsCollisionFree(const std::vector<double>& _controls)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::CheckCollision::Request>();
srv_request->config = _controls;
auto future_result = this->client_kthm_check_collision->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service to set the new control query.", this->client_kthm_check_collision->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (!result->response) {
RCLCPP_WARN(this->get_logger(),"The collision check cannot be performed.");
}
if (!result->collisionFree) {
RCLCPP_WARN(this->get_logger(),"Collision detected: %s", result->msg);
}
return result->collisionFree;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service to set the new control query.", this->client_kthm_check_collision->get_service_name());
return false;
}
}
Solve
The kautham_node/Solve service is used to solve the problem.
The request and response are defined in the Solve.srv file as follows:
---
bool is_solved
Below is an example of a function that wraps the call to this service:
bool KauthamRCLCPPClient::kauthamSolve()
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::Solve::Request>();
auto future_result = this->client_kthm_solve->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_solve->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (!result->is_solved) {
RCLCPP_WARN(this->get_logger(),"Kautham have not found a solution.");
}
return result->is_solved;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service.", this->client_kthm_solve->get_service_name());
return false;
}
}
GetPath
The kautham_node/GetPath service is used to get the path of the solution. Returns a vector of vectors message, with each configuration in each waypoint of the path.
The request and response are defined in the GetPath.srv file as follows:
bool only_controlled_joints
---
PathVector[] response
Where only_controlled_joints allows to get the path related onyl to the joints of the current controls.
The response variable is defined as:
float64[] v
An example of a function that wraps the call to this service is:
bool KauthamRCLCPPClient::kauthamGetPath(const bool _only_controlled_joints, std::vector<std::vector<double>>& kthm_path_)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::GetTrajectory::Request>();
srv_request->only_controlled_joints = _only_controlled_joints;
auto future_result = this->client_kthm_get_path->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_get_path->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (result->response.size() == 0) {
RCLCPP_WARN(this->get_logger(),"No valid path found.");
return false;
}
kthm_path_ = result->response;
return true;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service.", this->client_kthm_get_path->get_service_name());
return false;
}
}
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 follows:
---
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']
GetTrajectory
The kautham_node/GetTrajectory service is used to get the trajectory of the solution. Returns a complete trajectory_msgs/JointTrajectory message, ready to be used with ros_control.
The request and response are defined in the GetTrajectory.srv file as follows:
bool only_controlled_joints
float64 path_tolerance
float64 robot_freq
float64[] velocity_ratio
float64[] acceleration_ratio
---
trajectory_msgs/JointTrajectory traj
bool valid_trajectory_found
Where only_controlled_joints allows to compute the trajectgory related onyl to the joints of the current controls.
An example of a function that wraps the call to this service is:
bool KauthamRCLCPPClient::kauthamGetTrajecotry(const double _robot_freq, const std::vector<double>& _velocity_ratio, const std::vector<double>& _acceleration_ratio, trajectory_msgs::msg::JointTrajectory& kthm_computed_traj_)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::GetTrajectory::Request>();
srv_request->only_controlled_joints = true;
srv_request->path_tolerance = 0.01;
srv_request->robot_freq = _robot_freq;
srv_request->velocity_ratio = _velocity_ratio;
srv_request->acceleration_ratio = _acceleration_ratio;
auto future_result = this->client_kthm_get_traj->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_get_traj->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
kthm_computed_traj_ = result->traj;
if (!result->valid_trajectory_found) {
RCLCPP_WARN(this->get_logger(),"No valid trajectory found.");
}
return result->valid_trajectory_found;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service.", this->client_kthm_get_traj->get_service_name());
return false;
}
}
Visualization services
The Kautham scene and their solutions can be visualized through RViz.
Kautham GUI and Kautham RViz Tiago-Kitchen scenes comparation:
Kautham GUI and Kautham RViz Chesslab scenes comparation:
KauthamRVIZ
The kautham_node/visualizeRVIZ service is used to visualize the kautham scene in rviz.
The request and response are defined in the KauthamRVIZ.srv file as follows:
string rviz_config_file_path
uint32 rviz_freq
---
bool entities_loaded_successfully
An example of a function that wraps the call to this service is:
bool KauthamRCLCPPClient::kauthamViewSceneRVIZ(const std::string& _rviz_config_file_path)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::KauthamRVIZ::Request>();
srv_request->rviz_freq = 1; // Hz (Default)
srv_request->rviz_config_file_path = rviz_config_file_path;
auto future_result = this->client_kthm_view_scene_rviz->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_view_scene_rviz->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
return true;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service.", this->client_kthm_view_scene_rviz->get_service_name());
return false;
}
}
VisualizeSampleRVIZ
The kautham_node/visualizeSampleRVIZ service is used to visualize an specific kautham scene.
The request and response are defined in the VisualizeSampleRVIZ.srv file as follows:
string sample_type
uint32 sample_index
---
bool success
- Where sample_type could be:
path: To visualize an specific sample of the solved problem, then the sample_index must be set.
init: To visualize the initial query.
goal: To visualize the goal query.
An example of a function that wraps the call to this service is:
bool KauthamRCLCPPClient::kauthamVisualizeSampleRVIZ(const std::string& _sample_type, const int _sample_index)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::VisualizeSampleRVIZ::Request>();
srv_request->sample_type = _sample_type;
srv_request->sample_index = _sample_index;
auto future_result = this->client_kthm_visualize_sample_rviz->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_visualize_sample_rviz->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
return result->success;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccessful response from %s service.", this->client_kthm_visualize_sample_rviz->get_service_name());
return false;
}
}
VisualizePathMotionRVIZ
The kautham_node/visualizePathMotionRVIZ service is used to visualize the solved path motion.
The request and response are defined in the VisualizePathMotionRVIZ.srv file as follows:
bool run_motion
uint32 sampling_time_ms
string[] drawn_links
---
bool success
Where drawn_links are the TF2 link names that the user wants to visualize in their historical trajectory by displaying their paths with RViz/ROS2 markers, in addition to the URDF model’s motion in RViz.
An example of a function that wraps the call to this service is:
bool KauthamRCLCPPClient::kauthamVisualizePathMotionRVIZ(const bool _run_motion, const uint _sampling_time_ms, const std::vector<std::string>& _drawn_links)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::VisualizePathMotionRVIZ::Request>();
srv_request->run_motion = _run_motion;
srv_request->sampling_time_ms = _sampling_time_ms;
srv_request->drawn_links = _drawn_links;
auto future_result = this->client_kthm_visualize_path_motion_rviz->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_visualize_path_motion_rviz->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
return true;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service.", this->client_kthm_visualize_path_motion_rviz->get_service_name());
return false;
}
}
VisualizeGeoConstraintRVIZ
The kautham_node/visualizeGeoConstraintRVIZ service is used to visualize any geometric constraint associated to a robot.
The request and response are defined in the VisualizeGeoConstraintRVIZ.srv file as follows:
# This service is used to visualize a geometric constraint in RVIZ.
# It is not possible to create a constraint through the ROS service; the constraint must be created in the XML problem file.
string robot # Name of the robot that use the constraint.
string id # Unique constraint identifier.
---
bool success # True or False
string msg # Indicates the error if the service fails; otherwise, it is empty.
An example of a function that wraps the call to this service is:
bool KauthamRCLCPPClient::kauthamVisualizeGeoConstraintRVIZ(const std::string& _robot_name, const std::string& _constraint_identifier)
{
auto srv_request = std::make_shared<kautham_ros_interfaces::srv::VisualizeGeoConstraintRVIZ::Request>();
srv_request->robot = _robot_name;
srv_request->id = _constraint_identifier;
auto future_result = this->client_kthm_visualize_geo_constraint->async_send_request(srv_request);
RCLCPP_INFO(this->get_logger(),"Successfully requested %s service.", this->client_kthm_visualize_geo_constraint->get_service_name());
// Wait for the result:
if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
if (!result->success) {
RCLCPP_WARN(this->get_logger(),"Error when trying to visualize the geometric constraint %s from the robot %s: %s.", _constraint_identifier, _robot_name, result->msg.c_str());
}
return true;
} else {
RCLCPP_ERROR(this->get_logger(),"Unsuccess response of %s service.", this->client_kthm_visualize_geo_constraint->get_service_name());
return false;
}
}