Paste: 1
| Author: | 1 |
|---|
| Mode: | factor |
|---|
| Date: | Sun, 4 Feb 2024 13:02:16 |
|---|
Plain Text |
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <serial/serial.h>
#include <thread>
#include <iostream>
#include <cmath>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf/transform_broadcaster.h>
#include "mavlink2ros/ServoCtrl.h"
#include "FishCom/mavlink.h"
#include <mavlink2ros/Optlimit.h>//new
#include "mavlink2ros/FunctionCtrl.h"//new
#define SERVO_DUTY_CYCLE_MIN 499
#define SERVO_DUTY_CYCLE_MAX 2499
#define SERVO_DUTY_CYCLE_INIT 1500
class RobotController {
public:
RobotController(const std::string& port, int baud_rate) :
serial_device(port, baud_rate, serial::Timeout::simpleTimeout(1000)),
chassis_odom_pub_(nh.advertise<nav_msgs::Odometry>("odom", 50)),
optlimit_pub(nh.advertise<mavlink2ros::Optlimit>("optlimit_info", 50))
{
if (!serial_device.isOpen()) {
ROS_ERROR_STREAM("Failed to open serial port.");
}
cmd_ctrl_sub_ = nh.subscribe("cmd_vel", 100, &RobotController::cmdVelCallback, this);
servo_ctrl_sub_ = nh.subscribe("servo_ctrl", 100, &RobotController::servoCtrlCallback, this);
//
receive_thread = std::thread(&RobotController::receiveData, this);
manage_timer = nh.createTimer(ros::Duration(1.0), &RobotController::sendManageInfoRegularly, this);
control_timer = nh.createTimer(ros::Duration(0.01), &RobotController::sendCtrlInfoRegularly, this);
servos_timer = nh.createTimer(ros::Duration(0.01), &RobotController::sendServosInfoRegularly, this);
chassis_odom_pub_=nh.advertise<nav_msgs::Odometry>("odom",1);
odom_.header.frame_id = "odom";
odom_.child_frame_id = "base_link";
odom_tf_.header.frame_id = "odom";
odom_tf_.child_frame_id = "base_link";
optlimit_pub = nh.advertise<mavlink2ros::Optlimit>("optlimit_info",1);//new
optlimit_sub = nh.subscribe("optlimit_info", 100, &RobotController::optlimitCallback, this);//new
// optlimit_msg.pattern = gpio_read_level;//initialize the message
function_sub = nh.subscribe("function_id", 100, &RobotController::functionCtrlCallback, this);//new functionid
}
~RobotController() {
if (receive_thread.joinable()) {
receive_thread.join();
}
}
void runTask(){
int functionid = function_id.functionid;
if(functionid == 1)
{
upper_to_lower();
clp_close();
lower_to_upper();
clp_open();
printf("run\n");
}
if(functionid == 2)
{
servo_ctrl_r(4, 1.7);
servo_ctrl_r(4,0.3);
}
if(functionid == 3)
{
upper_to_lower();
servo_ctrl_r(2, 0.65);
lower_to_upper();
}
if(functionid == 4)
{
upper_to_lower();
servo_ctrl_r(2, 0.0);
lower_to_upper();
}
if(functionid == 5)
{
servos_positions[5] = 1.65 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
}
if(functionid == 6)
{
servos_positions[5] = 1.45 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
}
if(functionid == 7)
{
servos_positions[0] = 3.14 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
}//check id and give the diploma
if(functionid == 8)
{
servos_positions[0] = 0 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
}//check id and give the diploma
}
void sendManageInfoRegularly(const ros::TimerEvent&) {
sendManageInfo(1, 1, 1);
}
void sendCtrlInfoRegularly(const ros::TimerEvent&) {
std::lock_guard<std::mutex> lock(twist_mutex); // 确保线程安全
sendCtrlInfo(current_twist.linear.x, current_twist.linear.y, current_twist.angular.z);
}
void sendServosInfoRegularly(const ros::TimerEvent&){
std::lock_guard<std::mutex> lock(servos_mutex);
sendServosInfo(servos_positions[0],servos_positions[1],servos_positions[2],servos_positions[3],
servos_positions[4],servos_positions[5],servos_positions[6]);
}
void sendManageInfo(bool enable_chassis,bool enable_servos,bool reset_quaternion){
mavlink_message_t *msg = (mavlink_message_t *)malloc(sizeof(mavlink_message_t));
memset(msg, 0, sizeof(mavlink_message_t));
uint16_t Txlen = 0;
uint8_t *txbuf = (uint8_t *)malloc(MAVLINK_MSG_ID_CHS_MANAGE_INFO_LEN + 12);
mavlink_msg_chs_manage_info_pack(
CHS_SYSTEM_ID::CHS_ID_ORANGE,
CHS_SYSTEM_ID::CHS_ID_ORANGE,
msg, enable_chassis, enable_servos, reset_quaternion);
Txlen = mavlink_msg_to_send_buffer(txbuf, msg);
serial_device.write(txbuf, Txlen);
free(msg);
free(txbuf);
}
void sendCtrlInfo(float vx, float vy, float vz){
mavlink_message_t *msg = (mavlink_message_t *)malloc(sizeof(mavlink_message_t));
memset(msg, 0, sizeof(mavlink_message_t));
uint16_t Txlen = 0;
uint8_t *txbuf = (uint8_t *)malloc(MAVLINK_MSG_ID_CHS_CTRL_INFO_LEN + 12);
mavlink_msg_chs_ctrl_info_pack(
CHS_SYSTEM_ID::CHS_ID_ORANGE,
CHS_SYSTEM_ID::CHS_ID_ORANGE,
msg,
-vy, vx, vz);
Txlen = mavlink_msg_to_send_buffer(txbuf, msg);
serial_device.write(txbuf, Txlen);
free(msg);
free(txbuf);
}
void sendServosInfo(uint16_t servo1, uint16_t servo2, uint16_t servo3,
uint16_t servo4, uint16_t servo5, uint16_t servo6, uint16_t servo7){
mavlink_message_t *msg = (mavlink_message_t *)malloc(sizeof(mavlink_message_t));
memset(msg, 0, sizeof(mavlink_message_t));
uint16_t Txlen = 0;
uint8_t *txbuf = (uint8_t *)malloc(MAVLINK_MSG_ID_CHS_SERVOS_INFO_LEN + 12);
uint16_t servos[7] = {servo1, servo2, servo3, servo4, servo5, servo6, servo7};
mavlink_msg_chs_servos_info_pack(
CHS_SYSTEM_ID::CHS_ID_ORANGE,
CHS_SYSTEM_ID::CHS_ID_ORANGE,
msg,
servos);
Txlen = mavlink_msg_to_send_buffer(txbuf, msg);
serial_device.write(txbuf, Txlen);
free(msg);
free(txbuf);
}
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg) {
current_twist = *msg;
}
void servoCtrlCallback(const mavlink2ros::ServoCtrl::ConstPtr& msg) {
std::lock_guard<std::mutex> lock(servos_mutex);//这里使用了互斥锁（mutex），目的是确保在多线程环境中对servos_positions的访问是安全的。锁定在函数的生命周期内，以防止其他线程同时修改servos_positions。
int servo_id = msg->servo_id;
if (servo_id >= 0 && servo_id < 7) {
double servo_angle = msg->servo_angle;
servos_positions[servo_id] = static_cast<int>(servo_angle * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI) + SERVO_DUTY_CYCLE_MIN;//将上一步计算的结果强制转换为整数。这可能导致小数部分被截断，因为舵机位置通常以整数值表示。
ROS_INFO("updated! Servo ID: %d, New Position: %d", servo_id, servos_positions[servo_id]);
}
std::stringstream ss;
ss << "Current Servo Positions: ";
for(int i = 0; i < 7; i++) {
ss << servos_positions[i] << " ";
}
ROS_INFO_STREAM(ss.str());
}
void optlimitCallback(const mavlink2ros::Optlimit::ConstPtr& msg){
int optlimit_position = msg->pattern;
if(optlimit_position == 1)
{
// sendServosInfo(servos_positions[0], servos_positions[1], servos_positions[2], servos_positions[3], servos_positions[4], 1400, servos_positions[6]);
servos_positions[5] = 1499;//need to check
}
}
void upper_to_lower()
{
servos_positions[1] = 1.3 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
ros::Duration(1).sleep();
servos_positions[0] = 2.8 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
ros::Duration(1).sleep();
servos_positions[1] = 2.75 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
ros::Duration(1).sleep();
servos_positions[0] = 2.5 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
ros::Duration(1.5).sleep();
}
void lower_to_upper()
{
servos_positions[0] = 2.8 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
ros::Duration(1).sleep();
servos_positions[1] = 1.3 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
ros::Duration(1).sleep();
servos_positions[0] = 3.1 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
ros::Duration(1).sleep();
servos_positions[1] = 0.55 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
ros::Duration(1).sleep();
}
void clp_open()
{
servos_positions[2] = 0.0 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
ros::Duration(0.8).sleep();
}
void clp_close()
{
servos_positions[2] = 0.65 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
ros::Duration(0.8).sleep();
}
void servo_ctrl_r(int servo_Id, double servo_Angle)
{
servos_positions[servo_Id] = servo_Angle * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
printf("%d",servos_positions[servo_Id]);
ros::Duration(0.8).sleep();
}
void functionCtrlCallback(const mavlink2ros::FunctionCtrl::ConstPtr& msg)
{
function_id = *msg;
std::thread taskThread(&RobotController::runTask, this);
taskThread.detach();
}//new
void receiveData() {
while (ros::ok()) {
if (serial_device.available()) {
mavlink_message_t *msg = (mavlink_message_t *)malloc(sizeof(mavlink_message_t));
memset(msg, 0, sizeof(mavlink_message_t));
mavlink_status_t status;
uint8_t rxbuf[512];
size_t RxLen = serial_device.read(rxbuf, sizeof(rxbuf));
mavlink_chs_odom_info_t odom_info;
mavlink_chs_ctrl_info_t ctrl_info;
mavlink_chs_motor_info_t motor_info;
mavlink_chs_servos_info_t servos_info;
mavlink_chs_manage_info_t manage_info;
mavlink_chs_remoter_info_t remoter_info;
mavlink_chs_gpio_read_t gpio_read;
for (size_t i = 0; i < RxLen; ++i) {
ROS_DEBUG_STREAM("Byte " << i << ": 0x" << std::hex << static_cast<int>(rxbuf[i]));
if (mavlink_parse_char(MAVLINK_COMM_0, rxbuf[i], msg, &status)) {
ROS_INFO_STREAM("第 " << i << " 个包，解包成功：" << "Message ID: " << msg->msgid);
switch (msg->msgid) {
case MAVLINK_MSG_ID_CHS_ODOM_INFO: {
mavlink_msg_chs_odom_info_decode(msg, &odom_info);
ROS_DEBUG_STREAM("received odom info! vx: " << odom_info.vx << ", vy: " << odom_info.vy << ", vz: " << odom_info.vw);
std::stringstream ss;
ss << "quaternion: ";
for (int i = 0; i < 4; i++) ss << odom_info.quaternion[i] << " ";
ROS_DEBUG_STREAM(ss.str());
std::swap(odom_info.vx,odom_info.vy);
odom_info.vy = -odom_info.vy;
current_time = ros::Time::now();
double dt = (current_time - last_time).toSec();
double delta_x = (odom_info.vx * cos(th) - odom_info.vy * sin(th)) * dt;
double delta_y = (odom_info.vx * sin(th) + odom_info.vy * cos(th)) * dt;
double delta_th = odom_info.vw * dt;
x += delta_x;
y += delta_y;
th += delta_th;
odom_.header.stamp = current_time;
odom_.twist.twist.linear.x = odom_info.vx;
odom_.twist.twist.linear.y = odom_info.vy;
odom_.twist.twist.angular.z = odom_info.vw;
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
odom_.pose.pose.position.x = x;
odom_.pose.pose.position.y = y;
odom_.pose.pose.position.z = 0.0;
odom_.pose.pose.orientation = odom_quat;
chassis_odom_pub_.publish(odom_);
odom_tf_.header.stamp = current_time;
odom_tf_.transform.translation.x = x;
odom_tf_.transform.translation.y = y;
odom_tf_.transform.translation.z = 0.0;
odom_tf_.transform.rotation = odom_quat;
tf_broadcaster_.sendTransform(odom_tf_);
last_time = current_time;
break;
}
case MAVLINK_MSG_ID_CHS_REMOTER_INFO: {
mavlink_msg_chs_remoter_info_decode(msg, &remoter_info);
//printf("%d,",remoter_info.channel_0);
ROS_INFO_STREAM("received remoter_info: switch_message: " << remoter_info.switch_messgae <<
", channel_0: " << remoter_info.channel_0 << ", channel_1: " << remoter_info.channel_1 <<
", channel_2: " << remoter_info.channel_2 << ", channel_3: " << remoter_info.channel_3 <<
", wheel: " << remoter_info.wheel);
geometry_msgs::Twist cmd_vel;
cmd_vel.angular.z = static_cast<float>(remoter_info.channel_2) / -660.0;
cmd_vel.linear.x = static_cast<float>(remoter_info.channel_0) / 660.0;
cmd_vel.linear.y = static_cast<float>(remoter_info.channel_2) / -660.0;
cmd_vel_pub_.publish(cmd_vel);
int switch_messgae_1 = remoter_info.switch_messgae;
if(switch_messgae_1%2 != 0)
{
uint8_t switch_messgae_2 = remoter_info.switch_messgae; // 二进制表示为 10001110
// 读取第0位
//uint8_t zerothBit = & 0x01;
// 读取第1和第2位
uint8_t firstTwoBits = ( switch_messgae_2>> 1) & 0x03;
// 读取第3和第4位
uint8_t nextTwoBits = (switch_messgae_2 >> 3) & 0x03;
int channel_0_p = remoter_info.channel_0;
int channel_1_p = remoter_info.channel_1;
int channel_2_p = remoter_info.channel_2;
int channel_3_p = remoter_info.channel_3;
//start to check
//判断GPS，ATTI，ATTI的状态并作出反应
switch (nextTwoBits) {
case 0b00:
clp_open();
printf("%s\n", "clp_open");
break;
case 0b01:
printf("%s\n", "nothing");
break;
case 0x02:
clp_close();
printf("%s\n", "clp_close");
break;
default:
sprintf("%s\n", "fuck");
}
if(channel_0_p >= 100)
{
sendCtrlInfo(0, 1.0, 0);
}
if(channel_0_p <= -100)
{
sendCtrlInfo(0, -1.0, 0);
}
if(channel_1_p >= 100)//需要实验是否是这个方向
{
sendCtrlInfo(1.0, 0, 0);
}
if(channel_1_p <= -100)
{
sendCtrlInfo(-1.0, 0, 0);
}
if(channel_3_p >= 100)//需要实验是否是这个方向
{
sendCtrlInfo(0, 0, 1/M_PI);
}
if(channel_3_p <= -100)
{
sendCtrlInfo(0, 0, -1/M_PI);
}
if(channel_2_p >= 100)//需要实验是否是这个方向
{
upper_to_lower();
printf("%s\n", "down");
}
if(channel_2_p <= -100)
{
lower_to_upper();
printf("%s\n", "up");
}
}
break;
}
case MAVLINK_MSG_ID_CHS_GPIO_READ:{
mavlink_msg_chs_gpio_read_decode(msg, &gpio_read);
optlimit_msg.pattern = gpio_read.level;
// printf("%d\n",optlimit_msg.pattern);
optlimit_pub.publish(optlimit_msg);//new
break;
}
}
}
}
free(msg);
}
usleep(1000);
}
}
private:
ros::NodeHandle nh;
serial::Serial serial_device;
std::thread receive_thread;
geometry_msgs::Twist current_twist;
ros::Timer manage_timer;
ros::Timer control_timer;
ros::Timer servos_timer;
std::mutex twist_mutex;
std::mutex servos_mutex;
ros::Subscriber cmd_ctrl_sub_;
ros::Subscriber servo_ctrl_sub_;
ros::Subscriber optlimit_sub;//new
ros::Publisher cmd_vel_pub_;
tf::TransformBroadcaster tf_broadcaster_;//! ros chassis odometry tf broadcaster
ros::Publisher chassis_odom_pub_;//! ros odometry message publisher
//self defined message
geometry_msgs::TransformStamped odom_tf_;//! ros chassis odometry tf
nav_msgs::Odometry odom_;//! ros odometry message
mavlink2ros::Optlimit optlimit_msg;//define the message of optical limit
ros::Publisher optlimit_pub;//new
mavlink2ros::FunctionCtrl function_id;//newnew message
ros::Subscriber function_sub;//newnew subscriber
// mavlink2ros::FunctionCtrl function_ctrl;
// int gpio_read_level;//new
double x = 0.0;
double y = 0.0;
double th = 0.0;
ros::Time current_time = ros::Time::now(), last_time = ros::Time::now();
int servos_positions[7] = {2474, 849, 499,
SERVO_DUTY_CYCLE_INIT, 499, SERVO_DUTY_CYCLE_INIT, SERVO_DUTY_CYCLE_INIT};
};
int main(int argc, char** argv) {
ros::init(argc, argv, "robot_controller");
RobotController controller("/dev/robomaster", 115200);
ros::spin();
return 0;
}
New Annotation