Execute Task
Service
Execute a task as a service.
-
Service Name: autonomy/task/execute_as_srv
-
Service Type: clearpath_task_msgs/srv/ExecuteTask
- Python
- C++
import rclpy
from rclpy.node import Node
from clearpath_task_msgs.srv import ExecuteTask
class ExecuteTask(Node):
def __init__(self):
super().__init__('execute_task_client')
self._srv_client = self.create_client(ExecuteTask, 'autonomy/task/execute_as_srv')
while not self._srv_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('service not available, waiting again...')
self.req = ExecuteTask.Request()
def send_request(self):
# TODO
self.future = self._srv_client.call_async(self.req)
rclpy.spin_until_future_complete(self, self.future)
return self.future.result()
if __name__ == '__main__':
rclpy.init(args=args)
execute_task_client = PauseAutonomy()
response = execute_task_client.send_request()
if response.success:
execute_task_client.get_logger().info('Executing task: '))
else:
execute_task_client.get_logger().error('Failed to execute task: ')
execute_task_client.destroy_node()
rclpy.shutdown()
#include "rclcpp/rclcpp.hpp"
#include "clearpath_task_msgs/srv/execute_task.hpp"
#include <chrono>
#include <cstdlib>
#include <memory>
using namespace std::chrono_literals;
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("execute_task_client");
rclcpp::Client<std_srvs::srv::ExecuteTask>::SharedPtr client =
node->create_client<std_srvs::srv::ExecuteTask>("autonomy/taskexecute_as_srv");
auto request = std::make_shared<std_srvs::srv::ExecuteTask::Request>();
while (!client->wait_for_service(1s)) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
return 0;
}
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again...");
}
auto result = client->async_send_request(request);
// Wait for the result.
if (rclcpp::spin_until_future_complete(node, result) ==
rclcpp::FutureReturnCode::SUCCESS)
{
if (result.get()->success)
{
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Executing task: ", );
}
else
{
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Failed to execute task: ");
}
} else {
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service autonomy/task/execute_as_srv");
}
rclcpp::shutdown();
return 0;
}
Action
Execute a task as an action.
-
Action Name: autonomy/task/execute_as_action
-
Action Type: clearpath_task_msgs/action/ExecuteTask
- Python
- C++
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from clearpath_task_msgs.action import ExecuteTask
class ExecuteTask(Node):
def __init__(self):
super().__init__('execute_task_action_client')
self._action_client = ActionClient(self, ExecuteTask, 'autonomy/task/execute_as_action')
def send_goal(self):
goal_msg = ExecuteTask.Goal()
# TODO: populate goal message
self._action_client.wait_for_server()
self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)
self._send_goal_future.add_done_callback(self.goal_response_callback)
def goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().info('Goal rejected :(')
return
self.get_logger().info('Goal accepted :)')
self._get_result_future = goal_handle.get_result_async()
self._get_result_future.add_done_callback(self.get_result_callback)
def get_result_callback(self, future):
result = future.result().result
self.get_logger().info('Result: {0}'.format(result.sequence))
rclpy.shutdown()
def feedback_callback(self, feedback_msg):
feedback = feedback_msg.feedback
self.get_logger().info('Received feedback: {0}'.format(feedback.partial_sequence))
if __name__ == '__main__':
rclpy.init(args=args)
action_client = ExecuteTask()
action_client.send_goal()
rclpy.spin(action_client)
#include <functional>
#include <future>
#include <memory>
#include <string>
#include <sstream>
#include "clearpath_task_msgs/action/execute_task.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"
namespace execute_task_action_client
{
class ExecuteTaskActionClient : public rclcpp::Node
{
public:
using ExecuteTask = action_tutorials_interfaces::action::ExecuteTask;
using GoalHandleExecuteTask = rclcpp_action::ClientGoalHandle<ExecuteTask>;
explicit ExecuteGoToActionClient(const rclcpp::NodeOptions & options)
: Node("execute_task_action_client", options)
{
this->client_ptr_ = rclcpp_action::create_client<ExecuteTask>(
this, "autonomy/task/execute_as_action");
this->timer_ = this->create_wall_timer(
std::chrono::milliseconds(500),
std::bind(&ExecuteTaskActionClient::send_goal, this));
}
void send_goal()
{
using namespace std::placeholders;
this->timer_->cancel();
if (!this->client_ptr_->wait_for_action_server()) {
RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
rclcpp::shutdown();
}
auto goal_msg = ExecuteTask::Goal();
// TODO: update goal values
RCLCPP_INFO(this->get_logger(), "Sending goal");
auto send_goal_options = rclcpp_action::Client<ExecuteTask>::SendGoalOptions();
send_goal_options.goal_response_callback =
std::bind(&ExecuteTaskActionClient::goal_response_callback, this, _1);
send_goal_options.feedback_callback =
std::bind(&ExecuteTaskActionClient::feedback_callback, this, _1, _2);
send_goal_options.result_callback =
std::bind(&ExecuteTaskActionClient::result_callback, this, _1);
this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
}
private:
rclcpp_action::Client<ExecuteTask>::SharedPtr client_ptr_;
rclcpp::TimerBase::SharedPtr timer_;
void goal_response_callback(std::shared_future<GoalHandleExecuteTask::SharedPtr> future)
{
auto goal_handle = future.get();
if (!goal_handle) {
RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
} else {
RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
}
}
// TODO: update with action feedback
// void feedback_callback(
// GoalHandleExecuteTask::SharedPtr,
// const std::shared_ptr<const ExecuteTask::Feedback> feedback)
// {
// std::stringstream ss;
// ss << "Next number in sequence received: ";
// for (auto number : feedback->partial_sequence) {
// ss << number << " ";
// }
// RCLCPP_INFO(this->get_logger(), ss.str().c_str());
// }
// TODO: update with action result
// void result_callback(const GoalHandleExecuteTask::WrappedResult & result)
// {
// switch (result.code) {
// case rclcpp_action::ResultCode::SUCCEEDED:
// break;
// case rclcpp_action::ResultCode::ABORTED:
// RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
// return;
// case rclcpp_action::ResultCode::CANCELED:
// RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
// return;
// default:
// RCLCPP_ERROR(this->get_logger(), "Unknown result code");
// return;
// }
// std::stringstream ss;
// ss << "Result received: ";
// for (auto number : result.result->sequence) {
// ss << number << " ";
// }
// RCLCPP_INFO(this->get_logger(), ss.str().c_str());
// rclcpp::shutdown();
// }
}; // class ExecuteTaskActionClient
} // namespace execute_task_action_client
RCLCPP_COMPONENTS_REGISTER_NODE(goto_action_client::ExecuteTaskActionClient)