66 lines
2.3 KiB
C++
66 lines
2.3 KiB
C++
#ifndef WEBOTS_ROS2_PLUGIN_EXAMPLE_HPP
|
|
#define WEBOTS_ROS2_PLUGIN_EXAMPLE_HPP
|
|
|
|
#include "rclcpp/macros.hpp"
|
|
#include "webots_ros2_driver/PluginInterface.hpp"
|
|
#include "webots_ros2_driver/WebotsNode.hpp"
|
|
|
|
#include "geometry_msgs/msg/twist.hpp"
|
|
#include "rclcpp/rclcpp.hpp"
|
|
#include "/home/sweetai/webots_simulation/install/sweet_msg/include/sweet_msg/msg/movebase.hpp"
|
|
#include "geometry_msgs/msg/pose.hpp"
|
|
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
|
|
#include <webots/Speaker.hpp>
|
|
#include <nav_msgs/msg/path.hpp>
|
|
#include <webots/Node.hpp>
|
|
#include <webots/Field.hpp>
|
|
|
|
#include <array>
|
|
#include <vector>
|
|
|
|
#include <tf2_ros/buffer.h>
|
|
#include <tf2_ros/transform_listener.h>
|
|
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
|
|
|
// #include "sweet_utils/Utils/common_utils.hpp"
|
|
|
|
|
|
namespace webots_robot_driver {
|
|
class WebotsRobotDriver : public webots_ros2_driver::PluginInterface {
|
|
public:
|
|
void step() override;
|
|
void init(webots_ros2_driver::WebotsNode *node,
|
|
std::unordered_map<std::string, std::string> ¶meters) override;
|
|
|
|
private:
|
|
void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
|
|
void twist_callback(const geometry_msgs::msg::Twist::SharedPtr twist_msg);
|
|
void move_base_callback(const sweet_msg::msg::Movebase::SharedPtr move_base_msg);
|
|
void pathCallback(const nav_msgs::msg::Path::SharedPtr msg);
|
|
void getRobotCenterPose(double &x, double &y, const float yaw);
|
|
void pub_current_pose(const double lat, const double lon, const float roll, const float pitch, const float yaw);
|
|
float get_bearing_in_degrees();
|
|
|
|
|
|
rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr rtk_pose_publisher;
|
|
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_cmd_vel_sub;
|
|
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr
|
|
cmd_vel_subscription_;
|
|
rclcpp::Subscription<sweet_msg::msg::Movebase>::SharedPtr move_base_sub;
|
|
|
|
std::vector<std::array<double, 3>> path_points_;
|
|
|
|
geometry_msgs::msg::Twist cmd_vel_msg;
|
|
|
|
|
|
WbDeviceTag right_motor;
|
|
WbDeviceTag left_motor;
|
|
|
|
WbDeviceTag speaker;
|
|
WbDeviceTag gp, imu, compass;
|
|
|
|
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
|
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
|
};
|
|
} // namespace webots_robot_driver
|
|
#endif |