Hello there,
I have the following error inthe 6.5 quiz ros2 basics c++
It tells me that the distance didn’t compute correctly.
The distance is calculated with:
float get_distance_travelled() {
float distance_travelled =
sqrt(pow(this->current_pos_x - this->start_pos_x, 2) +
pow(this->current_pos_y - this->start_pos_y, 2));
return distance_travelled;
}
there are 20 feedbacks and 20 messages in /total_distance published in 20 secs.
Can somebody tell me what have I missed?
Thank you in advance.
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include <functional>
#include <memory>
#include <thread>
#include "actions_quiz_msg/action/distance.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "std_msgs/msg/float64.hpp"
//#include "nav_msgs/msg/detail/odometry__struct.hpp"
class MyActionServer : public rclcpp::Node {
public:
using Distance = actions_quiz_msg::action::Distance;
using GoalHandleMove = rclcpp_action::ServerGoalHandle<Distance>;
explicit MyActionServer(
const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
: Node("my_action_quiz_server", options) {
using namespace std::placeholders;
this->action_server_ = rclcpp_action::create_server<Distance>(
this, "distance_as",
std::bind(&MyActionServer::handle_goal, this, _1, _2),
std::bind(&MyActionServer::handle_cancel, this, _1),
std::bind(&MyActionServer::handle_accepted, this, _1));
subscription_ = this->create_subscription<nav_msgs::msg::Odometry>(
"odom", 10, std::bind(&MyActionServer::odom_callback, this, _1));
publisher_ =
this->create_publisher<std_msgs::msg::Float64>("total_distance", 10);
}
private:
rclcpp_action::Server<Distance>::SharedPtr action_server_;
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr publisher_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subscription_;
float start_pos_x = 0;
float start_pos_y = 0;
float current_pos_x = 0;
float current_pos_y = 0;
void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
this->current_pos_x = msg->pose.pose.position.x;
this->current_pos_y = msg->pose.pose.position.y;
}
rclcpp_action::GoalResponse
handle_goal(const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const Distance::Goal> goal) {
RCLCPP_INFO(this->get_logger(), "Received goal request with secs %d",
goal->seconds);
(void)uuid;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse
handle_cancel(const std::shared_ptr<GoalHandleMove> goal_handle) {
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void handle_accepted(const std::shared_ptr<GoalHandleMove> goal_handle) {
using namespace std::placeholders;
// this needs to return quickly to avoid blocking the executor, so spin up a
// new thread
std::thread{std::bind(&MyActionServer::execute, this, _1), goal_handle}
.detach();
}
float get_distance_travelled() {
float distance_travelled =
sqrt(pow(this->current_pos_x - this->start_pos_x, 2) +
pow(this->current_pos_y - this->start_pos_y, 2));
return distance_travelled;
}
void execute(const std::shared_ptr<GoalHandleMove> goal_handle) {
RCLCPP_INFO(this->get_logger(), "Executing goal");
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<Distance::Feedback>();
auto ¤t_distance_feedback = feedback->current_dist;
auto result = std::make_shared<Distance::Result>();
std_msgs::msg::Float64 distance_msg;
rclcpp::Rate loop_rate(1);
float distance_travelled = 0;
for (int i = 0; (i < goal->seconds) && rclcpp::ok(); ++i) {
// Check if there is a cancel request
if (goal_handle->is_canceling()) {
result->status = false;
result->total_dist = distance_travelled;
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "Goal canceled");
return;
}
// get distance traveled and send feedback
distance_travelled = get_distance_travelled();
current_distance_feedback = distance_travelled;
distance_msg.data = distance_travelled;
publisher_->publish(distance_msg);
goal_handle->publish_feedback(feedback);
RCLCPP_INFO(this->get_logger(), "Publish feedback");
loop_rate.sleep();
}
// Check if goal is done
if (rclcpp::ok()) {
result->status = true;
result->total_dist = distance_travelled;
// total_distance_publisher->publish(distance_msg);
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Goal succeeded");
}
}
}; // class MyActionServer
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
auto action_server = std::make_shared<MyActionServer>();
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(action_server);
executor.spin();
executor.remove_node(action_server);
rclcpp::shutdown();
return 0;
}
Action node client quiz
#include <inttypes.h>
#include <memory>
#include <string>
#include <iostream>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "actions_quiz_msg/action/distance.hpp"
#include "std_msgs/msg/float64.hpp"
class MyActionClient : public rclcpp::Node
{
public:
using Distance = actions_quiz_msg::action::Distance;
using GoalHandleMove = rclcpp_action::ClientGoalHandle<Distance>;
explicit MyActionClient(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions())
: Node("my_action_quiz_client", node_options), goal_done_(false)
{
this->client_ptr_ = rclcpp_action::create_client<Distance>(
this->get_node_base_interface(),
this->get_node_graph_interface(),
this->get_node_logging_interface(),
this->get_node_waitables_interface(),
"distance_as");
this->timer_ = this->create_wall_timer(
std::chrono::milliseconds(500),
std::bind(&MyActionClient::send_goal, this));
}
bool is_goal_done() const
{
return this->goal_done_;
}
void send_goal()
{
using namespace std::placeholders;
this->timer_->cancel();
this->goal_done_ = false;
if (!this->client_ptr_) {
RCLCPP_ERROR(this->get_logger(), "Action client not initialized");
}
if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))) {
RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
this->goal_done_ = true;
return;
}
auto goal_msg = Distance::Goal();
goal_msg.seconds = 20;
RCLCPP_INFO(this->get_logger(), "Sending goal");
auto send_goal_options = rclcpp_action::Client<Distance>::SendGoalOptions();
send_goal_options.goal_response_callback =
std::bind(&MyActionClient::goal_response_callback, this, _1);
send_goal_options.feedback_callback =
std::bind(&MyActionClient::feedback_callback, this, _1, _2);
send_goal_options.result_callback =
std::bind(&MyActionClient::result_callback, this, _1);
auto goal_handle_future = this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
}
private:
rclcpp_action::Client<Distance>::SharedPtr client_ptr_;
rclcpp::TimerBase::SharedPtr timer_;
bool goal_done_;
void goal_response_callback(const GoalHandleMove::SharedPtr & goal_handle)
{
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");
}
}
void feedback_callback(
GoalHandleMove::SharedPtr,
const std::shared_ptr<const Distance::Feedback> feedback)
{
RCLCPP_INFO(
this->get_logger(), "Feedback received: %f", feedback->current_dist);
}
void result_callback(const GoalHandleMove::WrappedResult & result)
{
this->goal_done_ = true;
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;
}
RCLCPP_INFO(this->get_logger(), "Result status received: %d", result.result->status);
RCLCPP_INFO(this->get_logger(), "Distance received: %f", result.result->total_dist);
}
}; // class MyActionClient
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto action_client = std::make_shared<MyActionClient>();
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(action_client);
while (!action_client->is_goal_done()) {
executor.spin_once();
}
executor.remove_node(action_client);
rclcpp::shutdown();
return 0;
}
Following are my different shells while making the robot go forward.
Shell 1 action server
user:~/ros2_ws$ ros2 launch actions_quiz actions_quiz_server.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2023-07-03-14-25-50-071580-4_xterm-22512
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [action_quiz_server_node-1]: process started with pid [22513]
[action_quiz_server_node-1] [INFO] [1688394420.270499813] [my_action_quiz_server]: Received goal request with secs 20
[action_quiz_server_node-1] [INFO] [1688394420.270880178] [my_action_quiz_server]: Executing goal
[action_quiz_server_node-1] [INFO] [1688394420.271080877] [my_action_quiz_server]: Publish feedback
[action_quiz_server_node-1] [INFO] [1688394421.271168110] [my_action_quiz_server]: Publish feedback
[action_quiz_server_node-1] [INFO] [1688394422.271304598] [my_action_quiz_server]: Publish feedback
[action_quiz_server_node-1] [INFO] [1688394423.271211568] [my_action_quiz_server]: Publish feedback
[action_quiz_server_node-1] [INFO] [1688394424.271231409] [my_action_quiz_server]: Publish feedback
[action_quiz_server_node-1] [INFO] [1688394425.271231018] [my_action_quiz_server]: Publish feedback
[action_quiz_server_node-1] [INFO] [1688394426.271239781] [my_action_quiz_server]: Publish feedback
[action_quiz_server_node-1] [INFO] [1688394427.271836915] [my_action_quiz_server]: Publish feedback
[action_quiz_server_node-1] [INFO] [1688394428.271193733] [my_action_quiz_server]: Publish feedback
[action_quiz_server_node-1] [INFO] [1688394429.271223898] [my_action_quiz_server]: Publish feedback
[action_quiz_server_node-1] [INFO] [1688394430.271201994] [my_action_quiz_server]: Publish feedback
[action_quiz_server_node-1] [INFO] [1688394431.271201209] [my_action_quiz_server]: Publish feedback
[action_quiz_server_node-1] [INFO] [1688394432.271210343] [my_action_quiz_server]: Publish feedback
[action_quiz_server_node-1] [INFO] [1688394433.271117438] [my_action_quiz_server]: Publish feedback
[action_quiz_server_node-1] [INFO] [1688394434.271235867] [my_action_quiz_server]: Publish feedback
[action_quiz_server_node-1] [INFO] [1688394435.271191260] [my_action_quiz_server]: Publish feedback
[action_quiz_server_node-1] [INFO] [1688394436.271247630] [my_action_quiz_server]: Publish feedback
[action_quiz_server_node-1] [INFO] [1688394437.271198158] [my_action_quiz_server]: Publish feedback
[action_quiz_server_node-1] [INFO] [1688394438.271240933] [my_action_quiz_server]: Publish feedback
[action_quiz_server_node-1] [INFO] [1688394439.271197938] [my_action_quiz_server]: Publish feedback
[action_quiz_server_node-1] [INFO] [1688394440.271253272] [my_action_quiz_server]: Goal succeeded
Shell 2 action client
user:~/ros2_ws$ ros2 launch actions_quiz actions_quiz_client.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2023-07-03-14-26-59-526966-4_xterm-22674
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [action_quiz_client_node-1]: process started with pid [22675]
[action_quiz_client_node-1] [INFO] [1688394420.270001602] [my_action_quiz_client]: Sending goal
[action_quiz_client_node-1] [INFO] [1688394420.271100552] [my_action_quiz_client]: Goal accepted by server, waiting for result
[action_quiz_client_node-1] [INFO] [1688394420.271599083] [my_action_quiz_client]: Feedback received: 0.000003
[action_quiz_client_node-1] [INFO] [1688394421.273842201] [my_action_quiz_client]: Feedback received: 0.000003
[action_quiz_client_node-1] [INFO] [1688394422.271418979] [my_action_quiz_client]: Feedback received: 0.057028
[action_quiz_client_node-1] [INFO] [1688394423.271316661] [my_action_quiz_client]: Feedback received: 0.388979
[action_quiz_client_node-1] [INFO] [1688394424.271371746] [my_action_quiz_client]: Feedback received: 0.720939
[action_quiz_client_node-1] [INFO] [1688394425.271325985] [my_action_quiz_client]: Feedback received: 1.052899
[action_quiz_client_node-1] [INFO] [1688394426.271360680] [my_action_quiz_client]: Feedback received: 1.384438
[action_quiz_client_node-1] [INFO] [1688394427.271977662] [my_action_quiz_client]: Feedback received: 1.713078
[action_quiz_client_node-1] [INFO] [1688394428.271306666] [my_action_quiz_client]: Feedback received: 2.051677
[action_quiz_client_node-1] [INFO] [1688394429.271331373] [my_action_quiz_client]: Feedback received: 2.380317
[action_quiz_client_node-1] [INFO] [1688394430.271571706] [my_action_quiz_client]: Feedback received: 2.715597
[action_quiz_client_node-1] [INFO] [1688394431.271364874] [my_action_quiz_client]: Feedback received: 3.047557
[action_quiz_client_node-1] [INFO] [1688394432.271389264] [my_action_quiz_client]: Feedback received: 3.382836
[action_quiz_client_node-1] [INFO] [1688394433.271304852] [my_action_quiz_client]: Feedback received: 3.704837
[action_quiz_client_node-1] [INFO] [1688394434.271364860] [my_action_quiz_client]: Feedback received: 4.036798
[action_quiz_client_node-1] [INFO] [1688394435.271289107] [my_action_quiz_client]: Feedback received: 4.372077
[action_quiz_client_node-1] [INFO] [1688394436.272632823] [my_action_quiz_client]: Feedback received: 4.700717
[action_quiz_client_node-1] [INFO] [1688394437.271323760] [my_action_quiz_client]: Feedback received: 5.035996
[action_quiz_client_node-1] [INFO] [1688394438.271502087] [my_action_quiz_client]: Feedback received: 5.367957
[action_quiz_client_node-1] [INFO] [1688394439.271786366] [my_action_quiz_client]: Feedback received: 5.703236
[action_quiz_client_node-1] [INFO] [1688394440.271496141] [my_action_quiz_client]: Result status received: 1
[action_quiz_client_node-1] [INFO] [1688394440.271549273] [my_action_quiz_client]: Distance received: 5.703236
[INFO] [action_quiz_client_node-1]: process has finished cleanly [pid 22675]
Shell 3 ros2 topic
user:~$ ros2 topic echo /total_distance
data: 3.1116537684283685e-06
---
data: 3.1116537684283685e-06
---
data: 0.05702805146574974
---
data: 0.38897904753685
---
data: 0.7209389805793762
---
data: 1.0528990030288696
---
data: 1.384437918663025
---
data: 1.7130779027938843
---
data: 2.0516769886016846
---
data: 2.380317211151123
---
data: 2.715596914291382
---
data: 3.0475566387176514
---
data: 3.382836103439331
---
data: 3.7048373222351074
---
data: 4.036797523498535
---
data: 4.372076988220215
---
data: 4.700716972351074
---
data: 5.035996437072754
---
data: 5.367956638336182
---
data: 5.703236103057861
---
ros2 action list
user:~$ ros2 action list
/distance_as