Hello all,
I am not able to send out the action goal result . May I ask you for help to provide some guidance?
@The error I received
:[record_odom_action_server_node-1] /home/user/ros2_ws/install/record_odom/lib/record_odom/record_odom_action_server_node: symbol lookup error: /home/user/ros2_ws/install/custom_interfaces/lib/libcustom_interfaces__rosidl_typesupport_fastrtps_cpp.so: undefined symbol:_ZN13geometry_msgs3msg24typesupport_fastrtps_cpp19get_serialized_sizeERKNS0_8Point32_ISaIvEEEm
@action_msg:
---
geometry_msgs/Point32[] list_of_odoms
---
float32 current_total
@Code of the action server:
#include “geometry_msgs/msg/detail/point32__struct.hpp”
#include “geometry_msgs/msg/detail/quaternion__struct.hpp”
#include “geometry_msgs/msg/point32.hpp”
#include “rclcpp/rclcpp.hpp”
#include “rclcpp_action/rclcpp_action.hpp”
#include “functional”
#include “memory”
#include “thread”
#include “custom_interfaces/action/odom_record.hpp”
#include “geometry_msgs/msg/twist.hpp”
#include “nav_msgs/msg/odometry.hpp”
class OdomActionServer : public rclcpp::Node {
public:
using OdomRecord = custom_interfaces::action::OdomRecord;
using GoalHandleOdomRecord = rclcpp_action::ServerGoalHandle;
//std::vector<geometry_msgs::msg::Point32> list_of_odoms;
explicit OdomActionServer(
const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
: Node(“record_odom”, options) {
using namespace std::placeholders;
this->action_server_ = rclcpp_action::create_server<OdomRecord>(
this, "record_odom_as",
std::bind(&OdomActionServer::handle_goal, this, _1),
std::bind(&OdomActionServer::handle_cancel, this, _1),
std::bind(&OdomActionServer::handle_accepted, this, _1));
publisher_ =
this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);
subscription_ = this->create_subscription<nav_msgs::msg::Odometry>(
"odom", 10, std::bind(&OdomActionServer::odom_callback, this, _1));
}
private:
rclcpp_action::Server::SharedPtr action_server_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subscription_;
geometry_msgs::msg::Point32 actual_coords;
std::vector<geometry_msgs::msg::Point32> odom_coords;
float DistanceFromStart;
float distance_total;
float distance_current;
void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
// RCLCPP_INFO(this->get_logger(), "Odometry=['%f','%f','%f']",
// msg->pose.pose.position.x, msg->pose.pose.position.y,
// msg->pose.pose.position.z);
this->actual_coords.x = msg->pose.pose.position.x;
this->actual_coords.y = msg->pose.pose.position.y;
this->actual_coords.z = msg->pose.pose.position.z;
// rclcpp::sleep_for(std::chrono::milliseconds(50));
}
// How do we make empty goal? Without setting the goal argument
rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &uuid) {
RCLCPP_INFO(this->get_logger(), “Received goal: Record Odometry”);
(void)uuid;
// distance_current = 0.0;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse
handle_cancel(const std::shared_ptr 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 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(&OdomActionServer::execute, this, _1), goal_handle}
.detach();
}
void execute(const std::shared_ptr goal_handle) {
RCLCPP_INFO(this->get_logger(), “Executing goal”);
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<OdomRecord::Feedback>();
auto &fb_message = feedback->current_total;
fb_message = 0.0;
auto result = std::make_shared<OdomRecord::Result>();
rclcpp::Rate loop_rate(1);
odom_coords.push_back(actual_coords);
goal_handle->publish_feedback(feedback);
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
if (rclcpp::ok()) {
result->list_of_odoms = odom_coords; ;
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Goal succeeded");
}
}
}; // class OdomActionServer
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
auto action_server = std::make_shared();
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(action_server);
executor.spin();
rclcpp::shutdown();
return 0;
}
@Method Tried:
cd /path/to/your/ros2_workspace
rm -r build install
colcon build --symlink-install