68 lines
2.5 KiB
C++
68 lines
2.5 KiB
C++
#include "geometry_msgs/msg/twist.hpp"
|
|
#include "ros2/ros2.hpp"
|
|
#include "tasks/packet_typedef.hpp"
|
|
#include <rclcpp/subscription.hpp>
|
|
#include <wust_vl/common/drivers/serial_driver.hpp>
|
|
#include <wust_vl/common/utils/logger.hpp>
|
|
#include <wust_vl/common/utils/timer.hpp>
|
|
using namespace wust_vision;
|
|
class Nav: public rclcpp::Node {
|
|
public:
|
|
Nav(): Node("wust_vision_global_node") {
|
|
cmd_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
|
|
"cmd_vel",
|
|
10,
|
|
std::bind(&Nav::twistCb, this, std::placeholders::_1)
|
|
);
|
|
serial_ = std::make_shared<wust_vl::common::drivers::SerialDriver>();
|
|
wust_vl::common::drivers::SerialDriver::SerialPortConfig cfg {
|
|
/*baud*/ 115200,
|
|
/*csize*/ 8,
|
|
boost::asio::serial_port_base::parity::none,
|
|
boost::asio::serial_port_base::stop_bits::one,
|
|
boost::asio::serial_port_base::flow_control::none
|
|
};
|
|
serial_->init_port("/dev/ttyACM0", cfg);
|
|
serial_->set_receive_callback(std::bind(
|
|
&Nav::serialCallback,
|
|
this,
|
|
std::placeholders::_1,
|
|
std::placeholders::_2
|
|
|
|
));
|
|
serial_->set_error_callback([&](const boost::system::error_code& ec) {
|
|
WUST_ERROR("serial") << "serial error: " << ec.message();
|
|
});
|
|
}
|
|
void serialCallback(const uint8_t* data, std::size_t len) {}
|
|
void twistCb(const geometry_msgs::msg::Twist::SharedPtr msg) {
|
|
NavRobotCmdData send_data;
|
|
send_data.cmd_ID = ID_NAV_CMD;
|
|
send_data.packet_type = ID_NAV_CONTROL;
|
|
send_data.time_stamp =
|
|
static_cast<uint32_t>(std::chrono::duration_cast<std::chrono::milliseconds>(
|
|
wust_vl::common::utils::time_utils::now().time_since_epoch()
|
|
)
|
|
.count());
|
|
send_data.vx = -msg->linear.y;
|
|
send_data.vy = msg->linear.x;
|
|
send_data.wz = msg->angular.z;
|
|
if (serial_) {
|
|
serial_->write(std::move(wust_vl::common::drivers::toVector(send_data)));
|
|
}
|
|
}
|
|
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
|
|
std::shared_ptr<wust_vl::common::drivers::SerialDriver> serial_;
|
|
};
|
|
int main(int argc, char** argv) {
|
|
rclcpp::init(argc, argv);
|
|
|
|
{
|
|
auto node = rclcpp::Node::make_shared("wust_vision_global_node");
|
|
rclcpp::spin(node);
|
|
// rclcpp::shutdown();
|
|
}
|
|
|
|
return 0;
|
|
}
|