验证后 #12
|
|
@ -0,0 +1,53 @@
|
||||||
|
cmake_minimum_required(VERSION 3.0.2)
|
||||||
|
project(FollowingCar)
|
||||||
|
|
||||||
|
find_package(catkin REQUIRED COMPONENTS
|
||||||
|
roscpp
|
||||||
|
rospy
|
||||||
|
std_msgs
|
||||||
|
message_generation
|
||||||
|
geometry_msgs
|
||||||
|
pibot_msgs
|
||||||
|
)
|
||||||
|
|
||||||
|
# 寻找OpenCV库
|
||||||
|
find_package(OpenCV REQUIRED)
|
||||||
|
# 查找 Boost 库
|
||||||
|
find_package(Boost REQUIRED)
|
||||||
|
|
||||||
|
# catkin_package(
|
||||||
|
# # INCLUDE_DIRS include
|
||||||
|
# # LIBRARIES FollowingCar
|
||||||
|
# # CATKIN_DEPENDS roscpp rospy std_msgs
|
||||||
|
# # DEPENDS system_lib
|
||||||
|
# CATKIN_DEPENDS message_runtime std_msgs geometry_msgs
|
||||||
|
# )
|
||||||
|
|
||||||
|
|
||||||
|
include_directories(
|
||||||
|
# include
|
||||||
|
${OpenCV_INCLUDE_DIRS}
|
||||||
|
${catkin_INCLUDE_DIRS}
|
||||||
|
include
|
||||||
|
)
|
||||||
|
|
||||||
|
add_library(${PROJECT_NAME} SHARED
|
||||||
|
|
||||||
|
src/uwb.cpp
|
||||||
|
src/mapping.cpp
|
||||||
|
src/align.cpp
|
||||||
|
src/Mat.cpp
|
||||||
|
src/lighthouse.cpp
|
||||||
|
include/senddata.h src/senddata.cpp)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
generate_messages(DEPENDENCIES std_msgs geometry_msgs)
|
||||||
|
|
||||||
|
catkin_package(CATKIN_DEPENDS message_runtime std_msgs geometry_msgs)
|
||||||
|
add_executable(${PROJECT_NAME}_node src/main.cpp)
|
||||||
|
|
||||||
|
|
||||||
|
target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES} pthread ${PROJECT_NAME})
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,83 @@
|
||||||
|
/******************** (C) COPYRIGHT 2022 Geek************************************
|
||||||
|
* File Name : Mat.h
|
||||||
|
* Current Version : V1.0
|
||||||
|
* Author : logzhan
|
||||||
|
* Date of Issued : 2022.09.14
|
||||||
|
* Comments : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
********************************************************************************/
|
||||||
|
/* Header File Including -----------------------------------------------------*/
|
||||||
|
#ifndef _H_MAT_
|
||||||
|
#define _H_MAT_
|
||||||
|
|
||||||
|
#define MAT_MAX 15 //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܴ<EFBFBD><DCB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
|
||||||
|
|
||||||
|
#include <string.h>
|
||||||
|
#define _USE_MATH_DEFINES
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
class Mat
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
Mat();
|
||||||
|
Mat(int setm,int setn,int kind);//kind=1<><31>λ<EFBFBD><CEBB>kind=0<><30><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>ݡ<EFBFBD>
|
||||||
|
void Init(int setm,int setn,int kind);//kind=1<><31>λ<EFBFBD><CEBB>kind=0<><30><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>ݡ<EFBFBD>
|
||||||
|
|
||||||
|
void Zero(void);
|
||||||
|
//<2F><>Щ<EFBFBD>ؼ<EFBFBD><D8BC><EFBFBD><EFBFBD><EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD>Ϊprivate<74>ġ<EFBFBD><C4A1><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>˷<EFBFBD><CBB7>㣬<EFBFBD><E3A3AC>Ҳ<EFBFBD><D2B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>public
|
||||||
|
int m;//<2F><><EFBFBD><EFBFBD>
|
||||||
|
int n;//<2F><><EFBFBD><EFBFBD>
|
||||||
|
double mat[MAT_MAX][MAT_MAX];//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
|
||||||
|
//<2F><><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD>
|
||||||
|
Mat SubMat(int a,int b,int lm,int ln);//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
||||||
|
void FillSubMat(int a,int b,Mat s);//<2F><><EFBFBD><EFBFBD>Ӿ<EFBFBD><D3BE><EFBFBD>
|
||||||
|
|
||||||
|
//<2F><><EFBFBD><EFBFBD>ר<EFBFBD><D7A8>
|
||||||
|
double absvec();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ij<EFBFBD><C4B3>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>Ǹ<EFBFBD><C7B8><EFBFBD>Ԫ<EFBFBD>صľ<D8B5><C4BE><EFBFBD>ֵ<EFBFBD><D6B5>
|
||||||
|
double Sqrt();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD>ƽ<EFBFBD><C6BD>
|
||||||
|
friend Mat operator ^(Mat a,Mat b);//<2F><><EFBFBD>
|
||||||
|
|
||||||
|
//<2F><><EFBFBD><EFBFBD>
|
||||||
|
friend Mat operator *(double k,Mat a);
|
||||||
|
friend Mat operator *(Mat a,double k);
|
||||||
|
friend Mat operator /(Mat a,double k);
|
||||||
|
friend Mat operator *(Mat a,Mat b);
|
||||||
|
friend Mat operator +(Mat a,Mat b);
|
||||||
|
friend Mat operator -(Mat a,Mat b);
|
||||||
|
friend Mat operator ~(Mat a);//ת<><D7AA>
|
||||||
|
friend Mat operator /(Mat a,Mat b);//a*inv(b)
|
||||||
|
friend Mat operator %(Mat a,Mat b);//inv(a)*b
|
||||||
|
|
||||||
|
//MAT inv();//<2F><><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
|
||||||
|
private:
|
||||||
|
// Ϊ<><CEAA><EFBFBD>ø<EFBFBD>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һЩ<D2BB><D0A9><EFBFBD><EFBFBD>
|
||||||
|
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
void RowExchange(int a, int b);
|
||||||
|
// ijһ<C4B3>г<EFBFBD><D0B3><EFBFBD>ϵ<EFBFBD><CFB5>
|
||||||
|
void RowMul(int a,double k);
|
||||||
|
// <20><>ijһ<C4B3>мӼ<D0BC><D3BC><EFBFBD>һ<EFBFBD>еı<D0B5><C4B1><EFBFBD>
|
||||||
|
void RowAdd(int a,int b, double k);
|
||||||
|
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
void ColExchange(int a, int b);
|
||||||
|
// ijһ<C4B3>г<EFBFBD><D0B3><EFBFBD>ϵ<EFBFBD><CFB5>
|
||||||
|
void ColMul(int a,double k);
|
||||||
|
// <20><>ijһ<C4B3>мӼ<D0BC><D3BC><EFBFBD>һ<EFBFBD>еı<D0B5><C4B1><EFBFBD>
|
||||||
|
void ColAdd(int a,int b,double k);
|
||||||
|
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,44 @@
|
||||||
|
#include <cmath>
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include <nav_msgs/Odometry.h>
|
||||||
|
#include <utility>
|
||||||
|
#include <queue>
|
||||||
|
#include <fstream>
|
||||||
|
#include "pibot_msgs/RawImu.h"
|
||||||
|
#include "type.h"
|
||||||
|
#include "uwb.h"
|
||||||
|
#include "lighthouse.h"
|
||||||
|
#include "Mat.h"
|
||||||
|
|
||||||
|
#ifndef ALIGN_H
|
||||||
|
#define AlIGN_H
|
||||||
|
namespace uwb_slam{
|
||||||
|
class Align
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
Align(){
|
||||||
|
|
||||||
|
};
|
||||||
|
void Run();
|
||||||
|
void wheel_odomCB(const nav_msgs::Odometry& wheel_odom);
|
||||||
|
void imuCB(const pibot_msgs::RawImu& imu);
|
||||||
|
void odomCB(const nav_msgs::Odometry& odom);
|
||||||
|
|
||||||
|
public:
|
||||||
|
ros::NodeHandle nh_;
|
||||||
|
ros::Subscriber wheel_odom_sub_;
|
||||||
|
ros::Subscriber imu_sub_;
|
||||||
|
ros::Subscriber odom_sub_;
|
||||||
|
Imu_odom_pose_data imu_odom_;
|
||||||
|
Uwb_data uwb_data_;
|
||||||
|
ros::Time imuDataRxTime, uwbDataRxTime, odomDataRxTime;
|
||||||
|
ros::Time odom_tmp_ ;
|
||||||
|
bool write_data_ = false;
|
||||||
|
cv::Mat img1;
|
||||||
|
std::queue<std::pair<Imu_odom_pose_data,Uwb_data>> data_queue;
|
||||||
|
std::shared_ptr<uwb_slam::Uwb> uwb_;
|
||||||
|
std::shared_ptr<uwb_slam::Lighthouse> lighthouse_;
|
||||||
|
};
|
||||||
|
};
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,29 @@
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <mutex>
|
||||||
|
#include <boost/asio.hpp>
|
||||||
|
#include <iostream>
|
||||||
|
#include <string>
|
||||||
|
#include <cstdint>
|
||||||
|
#include "type.h"
|
||||||
|
#include <queue>
|
||||||
|
#include <chrono>
|
||||||
|
#ifndef __LIGHTHOUSE_H__
|
||||||
|
#define __LIGHTHOUSE_H__
|
||||||
|
|
||||||
|
namespace uwb_slam{
|
||||||
|
class Lighthouse{
|
||||||
|
public:
|
||||||
|
Lighthouse();
|
||||||
|
~Lighthouse();
|
||||||
|
void Run();
|
||||||
|
void UDPRead();
|
||||||
|
// Listen PORT
|
||||||
|
int PORT = 12345;
|
||||||
|
int UdpSocket = -1;
|
||||||
|
|
||||||
|
LightHouseData data;
|
||||||
|
|
||||||
|
std::mutex mMutexLighthouse;
|
||||||
|
};
|
||||||
|
};
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,40 @@
|
||||||
|
#include <queue>
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include <mutex>
|
||||||
|
#include "uwb.h"
|
||||||
|
#include "align.h"
|
||||||
|
|
||||||
|
#ifndef MAPPING_H
|
||||||
|
#define MAPPING_H
|
||||||
|
|
||||||
|
|
||||||
|
namespace uwb_slam{
|
||||||
|
class Mapping
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
const double PIXEL_SCALE = 1.0; //xiangsudian cm
|
||||||
|
const int AREA_SIZE = 1000; //map size cm
|
||||||
|
Mapping() {};
|
||||||
|
void Run();
|
||||||
|
bool checkQueue();
|
||||||
|
void feedPos(const cv::Point2d & imuPosData, const cv::Point2d & uwbPosData, const cv::Point2d & syncPosData);
|
||||||
|
void process();
|
||||||
|
std::mutex mMutexMap;
|
||||||
|
std::shared_ptr<uwb_slam::Uwb> uwb_;
|
||||||
|
std::shared_ptr<uwb_slam::Align> align_;
|
||||||
|
|
||||||
|
private:
|
||||||
|
int realWidth, realHeight;
|
||||||
|
std::queue<std::vector<cv::Point2d>> posDataQueue;
|
||||||
|
std::vector<cv::Point2d> posData = std::vector<cv::Point2d>(3, {-1, -1});
|
||||||
|
//cv::Point2d imuPoint = {-1,-1};
|
||||||
|
// std::queue<cv::Point2d> posDataQueue;
|
||||||
|
|
||||||
|
|
||||||
|
bool readPos = false;
|
||||||
|
cv::Mat img;
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,50 @@
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <nav_msgs/Odometry.h>
|
||||||
|
#include <geometry_msgs/Point.h>
|
||||||
|
#include <geometry_msgs/Quaternion.h>
|
||||||
|
#include <tf2/LinearMath/Quaternion.h>
|
||||||
|
#include <mutex>
|
||||||
|
#include "uwb.h"
|
||||||
|
#include <geometry_msgs/Twist.h>
|
||||||
|
#include "pibot_msgs/dis_info.h"
|
||||||
|
#include "pibot_msgs/dis_info_array.h"
|
||||||
|
|
||||||
|
#ifndef SENDDATA_H
|
||||||
|
#define SENDDATA_H
|
||||||
|
|
||||||
|
|
||||||
|
namespace uwb_slam{
|
||||||
|
|
||||||
|
class Senddata
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
Senddata(){};
|
||||||
|
void publishOdometry( std::shared_ptr<uwb_slam::Uwb>uwb);
|
||||||
|
void Run(std::shared_ptr<uwb_slam::Uwb>uwb);
|
||||||
|
void odomCB(const nav_msgs::Odometry& odom);
|
||||||
|
void stereoCB(const pibot_msgs::dis_info_array::ConstPtr& stereo);
|
||||||
|
|
||||||
|
std::shared_ptr<uwb_slam::Uwb> uwb_;
|
||||||
|
|
||||||
|
|
||||||
|
std::mutex mMutexSend;
|
||||||
|
|
||||||
|
private:
|
||||||
|
ros::Publisher position_pub_;
|
||||||
|
ros::Publisher cmd_vel_pub_;
|
||||||
|
ros::Subscriber odom_sub_;
|
||||||
|
ros::Subscriber obstacles_sub_;
|
||||||
|
ros::NodeHandle nh_;
|
||||||
|
|
||||||
|
nav_msgs::Odometry odom_;//odom的消息类型
|
||||||
|
nav_msgs::Odometry sub_odom_;//odom的消息类型
|
||||||
|
geometry_msgs::Twist cmd_vel_;
|
||||||
|
|
||||||
|
bool flag_ = 0;
|
||||||
|
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,50 @@
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <ros/time.h>
|
||||||
|
#ifndef TYPE_H
|
||||||
|
#define TYPE_H
|
||||||
|
|
||||||
|
namespace uwb_slam{
|
||||||
|
struct Imu_data
|
||||||
|
{
|
||||||
|
ros::Time imu_t_;
|
||||||
|
double a_[3];
|
||||||
|
double g_[3];
|
||||||
|
double m_[3];
|
||||||
|
Imu_data(){};
|
||||||
|
Imu_data(double ax,double ay,double az, double gx, double gy, double gz, double mx, double my, double mz)
|
||||||
|
:a_{ax,ay,az},g_{gx,gy,gz},m_{mx,my,mz}{};
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
struct Imu_odom_pose_data
|
||||||
|
{
|
||||||
|
Imu_data imu_data_;
|
||||||
|
double pose_[3];
|
||||||
|
double quat_[4];
|
||||||
|
double vxy_;
|
||||||
|
double angle_v_;
|
||||||
|
Imu_odom_pose_data(){};
|
||||||
|
Imu_odom_pose_data( Imu_data imu_data,double x,double y,double z, double qw, double qx, double qy, double qz,double vxy, double angle_v):imu_data_(imu_data),pose_{x,y,z},quat_{qw,qx,qy,qz},vxy_(vxy),angle_v_(angle_v){};
|
||||||
|
};
|
||||||
|
|
||||||
|
struct Uwb_data
|
||||||
|
{
|
||||||
|
float x_,y_;
|
||||||
|
ros::Time uwb_t_;
|
||||||
|
Uwb_data(){};
|
||||||
|
Uwb_data(float x,float y,float t):x_(x),y_(y),uwb_t_(t){};
|
||||||
|
};
|
||||||
|
|
||||||
|
struct LightHouseData
|
||||||
|
{
|
||||||
|
float x_,y_,z_;
|
||||||
|
float qw_,qx_,qy_,qz_;
|
||||||
|
ros::Time lighthouse_t_;
|
||||||
|
LightHouseData(){};
|
||||||
|
LightHouseData(float x,float y,float z, float qw, float qx, float qy, float qz, float t):
|
||||||
|
x_(x),y_(y),z_(z),qw_(qw), qx_(qx), qy_(qy), qz_(qz), lighthouse_t_(t){};
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,60 @@
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <mutex>
|
||||||
|
#include <boost/asio.hpp>
|
||||||
|
#include <iostream>
|
||||||
|
#include <string>
|
||||||
|
#include <cstdint>
|
||||||
|
#include "type.h"
|
||||||
|
#include <queue>
|
||||||
|
#include <chrono>
|
||||||
|
#ifndef UWB_H
|
||||||
|
#define UWB_H
|
||||||
|
|
||||||
|
#define PI acos(-1)
|
||||||
|
namespace uwb_slam{
|
||||||
|
|
||||||
|
class Uwb
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
Uwb();
|
||||||
|
void Run();
|
||||||
|
bool checknewdata();
|
||||||
|
void feed_imu_odom_pose_data();
|
||||||
|
void Serread();
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
public:
|
||||||
|
int pre_seq = -1;
|
||||||
|
int cur_seq = -1;
|
||||||
|
int AnchorNum = 3;
|
||||||
|
int AnchorPos[3][3]={
|
||||||
|
-240, 400, 113,\
|
||||||
|
240, 400, 113,\
|
||||||
|
-400, -240, 113
|
||||||
|
};//基站坐标，序号+三维坐标
|
||||||
|
int d[3];
|
||||||
|
int aoa[3];
|
||||||
|
|
||||||
|
// std::queue<Imu_odom_pose_data> v_buffer_imu_odom_pose_data_;
|
||||||
|
|
||||||
|
|
||||||
|
Uwb_data uwb_data_;
|
||||||
|
// ros_merge_test::RawImu sub_imu_;
|
||||||
|
// std::queue<Imu_odom_pose_data > imu_odom_queue_;
|
||||||
|
// std::queue<Uwb_data> uwb_data_queue_;
|
||||||
|
std::mutex mMutexUwb;
|
||||||
|
//boost::asio::io_service io;
|
||||||
|
//boost::asio::serial_port s_port;
|
||||||
|
|
||||||
|
// Imu_odom_pose_data imu_odom_pose_data_;
|
||||||
|
};
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,75 @@
|
||||||
|
<?xml version="1.0"?>
|
||||||
|
<package format="2">
|
||||||
|
<name>FollowingCar</name>
|
||||||
|
<version>0.0.0</version>
|
||||||
|
<description>The FollowingCar package</description>
|
||||||
|
|
||||||
|
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||||
|
<!-- Example: -->
|
||||||
|
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||||
|
<maintainer email="luoruidi@todo.todo">luoruidi</maintainer>
|
||||||
|
|
||||||
|
|
||||||
|
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||||
|
<!-- Commonly used license strings: -->
|
||||||
|
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||||
|
<license>TODO</license>
|
||||||
|
|
||||||
|
|
||||||
|
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||||
|
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||||
|
<!-- Example: -->
|
||||||
|
<!-- <url type="website">http://wiki.ros.org/FollowingCar</url> -->
|
||||||
|
|
||||||
|
|
||||||
|
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||||
|
<!-- Authors do not have to be maintainers, but could be -->
|
||||||
|
<!-- Example: -->
|
||||||
|
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||||
|
|
||||||
|
|
||||||
|
<!-- The *depend tags are used to specify dependencies -->
|
||||||
|
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||||
|
<!-- Examples: -->
|
||||||
|
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
||||||
|
<!-- <depend>roscpp</depend> -->
|
||||||
|
<!-- Note that this is equivalent to the following: -->
|
||||||
|
<!-- <build_depend>roscpp</build_depend> -->
|
||||||
|
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||||
|
<!-- Use build_depend for packages you need at compile time: -->
|
||||||
|
<!-- <build_depend>message_generation</build_depend> -->
|
||||||
|
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||||
|
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||||
|
<!-- Use buildtool_depend for build tool packages: -->
|
||||||
|
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||||
|
<!-- Use exec_depend for packages you need at runtime: -->
|
||||||
|
<!-- <exec_depend>message_runtime</exec_depend> -->
|
||||||
|
<!-- Use test_depend for packages you need only for testing: -->
|
||||||
|
<!-- <test_depend>gtest</test_depend> -->
|
||||||
|
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||||
|
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||||
|
<buildtool_depend>catkin</buildtool_depend>
|
||||||
|
<build_depend>roscpp</build_depend>
|
||||||
|
<build_depend>rospy</build_depend>
|
||||||
|
<build_depend>std_msgs</build_depend>
|
||||||
|
<build_depend>pibot_msgs</build_depend>
|
||||||
|
|
||||||
|
<build_depend>geometry_msgs</build_depend>
|
||||||
|
<build_depend>message_generation</build_depend>
|
||||||
|
|
||||||
|
<build_export_depend>roscpp</build_export_depend>
|
||||||
|
<build_export_depend>rospy</build_export_depend>
|
||||||
|
<build_export_depend>std_msgs</build_export_depend>
|
||||||
|
<exec_depend>roscpp</exec_depend>
|
||||||
|
<exec_depend>rospy</exec_depend>
|
||||||
|
<exec_depend>std_msgs</exec_depend>
|
||||||
|
<exec_depend>message_runtime</exec_depend>
|
||||||
|
<exec_depend>geometry_msgs</exec_depend>
|
||||||
|
|
||||||
|
|
||||||
|
<!-- The export tag contains other, unspecified, tags -->
|
||||||
|
<export>
|
||||||
|
<!-- Other tools can request additional information be placed here -->
|
||||||
|
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
|
|
@ -0,0 +1,368 @@
|
||||||
|
/******************** (C) COPYRIGHT 2022 Geek************************************
|
||||||
|
* File Name : Mat.cpp
|
||||||
|
* Current Version : V1.0
|
||||||
|
* Author : logzhan
|
||||||
|
* Date of Issued : 2022.09.14
|
||||||
|
* Comments : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
********************************************************************************/
|
||||||
|
/* Header File Including -----------------------------------------------------*/
|
||||||
|
#include "Mat.h"
|
||||||
|
|
||||||
|
double mind(double a,double b)
|
||||||
|
{
|
||||||
|
double c = a;
|
||||||
|
if(b < c){
|
||||||
|
c = b;
|
||||||
|
}
|
||||||
|
return c;
|
||||||
|
}
|
||||||
|
|
||||||
|
int mini(int a,int b)
|
||||||
|
{
|
||||||
|
int c=a;
|
||||||
|
if(b<c)
|
||||||
|
{
|
||||||
|
c=b;
|
||||||
|
}
|
||||||
|
return c;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
//<2F><>Ҫ<EFBFBD>ڳ<EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5>ù<EFBFBD><C3B9>캯<EFBFBD><ECBAAF>
|
||||||
|
Mat::Mat()
|
||||||
|
{
|
||||||
|
Init(1,1,0);
|
||||||
|
}
|
||||||
|
|
||||||
|
Mat::Mat(int setm,int setn,int kind)
|
||||||
|
{
|
||||||
|
Init(setm,setn,kind);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Mat::Init(int setm,int setn,int kind)
|
||||||
|
{
|
||||||
|
this->m = setm;
|
||||||
|
this->n = setn;
|
||||||
|
if((kind==0)||(kind==1))
|
||||||
|
{
|
||||||
|
memset(mat,0,MAT_MAX*MAT_MAX*sizeof(double));
|
||||||
|
}
|
||||||
|
|
||||||
|
if(kind==1)
|
||||||
|
{
|
||||||
|
int x;
|
||||||
|
//Cԭ<43>е<EFBFBD>max min<69>ᵼ<EFBFBD><E1B5BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>и<EFBFBD><D0B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ķ<EFBFBD><C4B6><EFBFBD><EFBFBD><EFBFBD>Ҫֱ<D2AA>ӷŵ<D3B7>max<61><78><EFBFBD>档
|
||||||
|
int xend = mini(this->m, this->n);
|
||||||
|
for(x=0;x < xend;x++){
|
||||||
|
mat[x][x] = 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
void Mat::Zero() {
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
Mat Mat::SubMat(int a,int b,int lm,int ln)
|
||||||
|
{
|
||||||
|
|
||||||
|
int aend=a+lm-1;
|
||||||
|
int bend=b+ln-1;
|
||||||
|
|
||||||
|
|
||||||
|
Mat s(lm,ln,-1);
|
||||||
|
int x,y;
|
||||||
|
for(x=0;x<lm;x++)
|
||||||
|
{
|
||||||
|
for(y=0;y<ln;y++)
|
||||||
|
{
|
||||||
|
s.mat[x][y]=mat[a+x][b+y];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return s;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void Mat::FillSubMat(int a,int b,Mat s)
|
||||||
|
{
|
||||||
|
int x,y;
|
||||||
|
for(x=0;x<s.m;x++)
|
||||||
|
{
|
||||||
|
for(y=0;y<s.n;y++)
|
||||||
|
{
|
||||||
|
mat[a+x][b+y]=s.mat[x][y];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
Mat operator *(double k, Mat a)
|
||||||
|
{
|
||||||
|
Mat b(a.m,a.n,-1);
|
||||||
|
int x,y;
|
||||||
|
for(x=0;x<a.m;x++)
|
||||||
|
{
|
||||||
|
for(y=0;y<a.n;y++)
|
||||||
|
{
|
||||||
|
b.mat[x][y]=k*a.mat[x][y];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return b;
|
||||||
|
}
|
||||||
|
Mat operator *(Mat a,double k)
|
||||||
|
{
|
||||||
|
return k*a;
|
||||||
|
}
|
||||||
|
Mat operator /(Mat a,double k)
|
||||||
|
{
|
||||||
|
return (1/k)*a;
|
||||||
|
}
|
||||||
|
Mat operator *(Mat a,Mat b)
|
||||||
|
{
|
||||||
|
Mat c(a.m,b.n,-1);
|
||||||
|
int x,y,z;
|
||||||
|
double s;
|
||||||
|
for(x=0;x<a.m;x++)
|
||||||
|
{
|
||||||
|
for(y=0;y<b.n;y++)
|
||||||
|
{
|
||||||
|
s=0;
|
||||||
|
for(z=0;z<a.n;z++)
|
||||||
|
{
|
||||||
|
s=s+a.mat[x][z]*b.mat[z][y];
|
||||||
|
}
|
||||||
|
c.mat[x][y]=s;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return c;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
Mat operator +(Mat a,Mat b)
|
||||||
|
{
|
||||||
|
|
||||||
|
Mat c=a;
|
||||||
|
int x,y;
|
||||||
|
for(x=0;x<c.m;x++)
|
||||||
|
{
|
||||||
|
for(y=0;y<c.n;y++)
|
||||||
|
{
|
||||||
|
c.mat[x][y]+=b.mat[x][y];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return c;
|
||||||
|
}
|
||||||
|
Mat operator -(Mat a,Mat b)
|
||||||
|
{
|
||||||
|
|
||||||
|
Mat c=a;
|
||||||
|
int x,y;
|
||||||
|
for(x=0;x<c.m;x++)
|
||||||
|
{
|
||||||
|
for(y=0;y<c.n;y++)
|
||||||
|
{
|
||||||
|
c.mat[x][y]-=b.mat[x][y];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return c;
|
||||||
|
}
|
||||||
|
Mat operator ~(Mat a)
|
||||||
|
{
|
||||||
|
Mat b(a.n,a.m,-1);
|
||||||
|
int x,y;
|
||||||
|
for(x=0;x<a.m;x++)
|
||||||
|
{
|
||||||
|
for(y=0;y<a.n;y++)
|
||||||
|
{
|
||||||
|
b.mat[y][x]=a.mat[x][y];
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return b;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
Mat operator /(Mat a,Mat b)
|
||||||
|
{
|
||||||
|
//<2F><>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA>
|
||||||
|
|
||||||
|
int x,xb;
|
||||||
|
for(x=0;x<b.n;x++)
|
||||||
|
{
|
||||||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>ѵ<EFBFBD><D1B5>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD>ʼԪ<CABC><D4AA><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
double s=0;
|
||||||
|
int p=x;
|
||||||
|
double sxb;
|
||||||
|
for(xb=x;xb<b.n;xb++)
|
||||||
|
{
|
||||||
|
sxb=fabs(b.mat[x][xb]);
|
||||||
|
if(sxb>s)
|
||||||
|
{
|
||||||
|
p=xb;
|
||||||
|
s=sxb;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
//ͬʱ<CDAC>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
if(x!=p)
|
||||||
|
{
|
||||||
|
a.ColExchange(x,p);
|
||||||
|
b.ColExchange(x,p);
|
||||||
|
}
|
||||||
|
|
||||||
|
//<2F><>һ<EFBFBD>й<EFBFBD>һ
|
||||||
|
double k=1/b.mat[x][x];//<2F><>һ<EFBFBD>䲻ҪǶ<D2AA><EFBFBD><D7B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>²<EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
a.ColMul(x,k);
|
||||||
|
b.ColMul(x,k);
|
||||||
|
|
||||||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><D0B9><EFBFBD>
|
||||||
|
for(xb=0;xb<b.n;xb++)
|
||||||
|
{
|
||||||
|
if(xb!=x)
|
||||||
|
{
|
||||||
|
k=(-b.mat[x][xb]);
|
||||||
|
a.ColAdd(xb,x,k);
|
||||||
|
b.ColAdd(xb,x,k);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return a;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
Mat operator %(Mat a,Mat b)
|
||||||
|
{
|
||||||
|
//<2F><>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA>
|
||||||
|
int x,xb;
|
||||||
|
for(x=0;x<a.m;x++)
|
||||||
|
{
|
||||||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>ѵ<EFBFBD><D1B5>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD>ʼԪ<CABC><D4AA><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
double s=0;
|
||||||
|
int p=x;
|
||||||
|
double sxb;
|
||||||
|
for(xb=x;xb<a.m;xb++)
|
||||||
|
{
|
||||||
|
sxb=fabs(a.mat[xb][x]);
|
||||||
|
if(sxb>s)
|
||||||
|
{
|
||||||
|
p=xb;
|
||||||
|
s=sxb;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
//ͬʱ<CDAC>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
if(x!=p)
|
||||||
|
{
|
||||||
|
a.RowExchange(x,p);
|
||||||
|
b.RowExchange(x,p);
|
||||||
|
}
|
||||||
|
|
||||||
|
//<2F><>һ<EFBFBD>й<EFBFBD>һ
|
||||||
|
double k=1/a.mat[x][x];//<2F><>һ<EFBFBD>䲻ҪǶ<D2AA><EFBFBD><D7B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>²<EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
a.RowMul(x,k);
|
||||||
|
b.RowMul(x,k);
|
||||||
|
|
||||||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><D0B9><EFBFBD>
|
||||||
|
for(xb=0;xb<a.m;xb++)
|
||||||
|
{
|
||||||
|
if(xb!=x)
|
||||||
|
{
|
||||||
|
k=(-a.mat[xb][x]);
|
||||||
|
a.RowAdd(xb,x,k);
|
||||||
|
b.RowAdd(xb,x,k);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return b;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void Mat::RowExchange(int a, int b)
|
||||||
|
{
|
||||||
|
double s[MAT_MAX];
|
||||||
|
int ncpy=n*sizeof(double);
|
||||||
|
memcpy(s,mat[a],ncpy);
|
||||||
|
memcpy(mat[a],mat[b],ncpy);
|
||||||
|
memcpy(mat[b],s,ncpy);
|
||||||
|
}
|
||||||
|
void Mat::RowMul(int a,double k)
|
||||||
|
{
|
||||||
|
int y;
|
||||||
|
for(y=0;y<n;y++)
|
||||||
|
{
|
||||||
|
mat[a][y]= mat[a][y]*k;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
void Mat::RowAdd(int a,int b,double k)
|
||||||
|
{
|
||||||
|
int y;
|
||||||
|
for(y=0;y<n;y++)
|
||||||
|
{
|
||||||
|
mat[a][y]= mat[a][y]+ mat[b][y]*k;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Mat::ColExchange(int a, int b)
|
||||||
|
{
|
||||||
|
double s;
|
||||||
|
int x;
|
||||||
|
for(x=0;x<m;x++)
|
||||||
|
{
|
||||||
|
s=mat[x][a];
|
||||||
|
mat[x][a]=mat[x][b];
|
||||||
|
mat[x][b]=s;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
void Mat::ColMul(int a,double k)
|
||||||
|
{
|
||||||
|
int x;
|
||||||
|
for(x=0;x<m;x++)
|
||||||
|
{
|
||||||
|
mat[x][a]=mat[x][a]*k;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
void Mat::ColAdd(int a,int b,double k)
|
||||||
|
{
|
||||||
|
int x;
|
||||||
|
for(x=0;x<m;x++)
|
||||||
|
{
|
||||||
|
mat[x][a]=mat[x][a]+mat[x][b]*k;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
double Mat::Sqrt()
|
||||||
|
{
|
||||||
|
int x;
|
||||||
|
double numx;
|
||||||
|
double s=0;
|
||||||
|
for(x=0;x<m;x++)
|
||||||
|
{
|
||||||
|
numx=mat[x][0];
|
||||||
|
s+=(numx*numx);
|
||||||
|
}
|
||||||
|
return s;
|
||||||
|
}
|
||||||
|
double Mat::absvec()
|
||||||
|
{
|
||||||
|
return sqrt(Sqrt());
|
||||||
|
}
|
||||||
|
|
||||||
|
Mat operator ^(Mat a, Mat b)
|
||||||
|
{
|
||||||
|
double ax=a.mat[0][0];
|
||||||
|
double ay=a.mat[1][0];
|
||||||
|
double az=a.mat[2][0];
|
||||||
|
double bx=b.mat[0][0];
|
||||||
|
double by=b.mat[1][0];
|
||||||
|
double bz=b.mat[2][0];
|
||||||
|
|
||||||
|
Mat c(3,1,-1);
|
||||||
|
c.mat[0][0]=ay*bz-az*by;
|
||||||
|
c.mat[1][0]=(-(ax*bz-az*bx));
|
||||||
|
c.mat[2][0]=ax*by-ay*bx;
|
||||||
|
|
||||||
|
return c;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -0,0 +1,94 @@
|
||||||
|
#include "align.h"
|
||||||
|
|
||||||
|
namespace uwb_slam{
|
||||||
|
void Align::Run()
|
||||||
|
{
|
||||||
|
imuDataRxTime = ros::Time::now();
|
||||||
|
uwbDataRxTime = ros::Time::now();
|
||||||
|
|
||||||
|
wheel_odom_sub_= nh_.subscribe("wheel_odom",10,&Align::wheel_odomCB,this);
|
||||||
|
imu_sub_= nh_.subscribe("raw_imu",10,&Align::imuCB,this);
|
||||||
|
odom_sub_= nh_.subscribe("odom",10,&Align::odomCB,this);
|
||||||
|
|
||||||
|
std::ofstream outfile("data.txt",std::ofstream::out);
|
||||||
|
if(outfile.is_open()) {
|
||||||
|
std::cout << "start saving data" << std::endl;
|
||||||
|
outfile << "imuDataRxTime,odomDataRxTime,uwbDataRxTime,"\
|
||||||
|
<< "aX,aY,aZ,"\
|
||||||
|
<< "gX,gY,gZ"\
|
||||||
|
<< "mX,mY,mZ,"\
|
||||||
|
<< "qW,qX,qY,qZ,"\
|
||||||
|
<< "vXY,angleV,"\
|
||||||
|
<< "imuPosX,imuPosY,"\
|
||||||
|
<< "lightHousePosX,lightHousePosY,lightHousePosZ,"\
|
||||||
|
<< "lightHouseQW,lightHouseQX,lightHouseQY,lightHouseQZ"\
|
||||||
|
<< "d1,d2,d3\n";
|
||||||
|
} else {
|
||||||
|
std::cout<<"file can not open"<<std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
while(1){
|
||||||
|
if(imuDataRxTime!=imu_odom_.imu_data_.imu_t_){
|
||||||
|
//std::cout << "imu received" << std::endl;
|
||||||
|
imuDataRxTime = imu_odom_.imu_data_.imu_t_;
|
||||||
|
odomDataRxTime = odom_tmp_;
|
||||||
|
uwbDataRxTime = uwb_->uwb_data_.uwb_t_;
|
||||||
|
|
||||||
|
outfile << imuDataRxTime << "," << odomDataRxTime << "," << uwbDataRxTime <<","\
|
||||||
|
<< imu_odom_.imu_data_.a_[0] << "," << imu_odom_.imu_data_.a_[1] << "," << imu_odom_.imu_data_.a_[2] << ","\
|
||||||
|
<< imu_odom_.imu_data_.g_[0] << "," << imu_odom_.imu_data_.g_[1] << "," << imu_odom_.imu_data_.g_[2] << ","\
|
||||||
|
<< imu_odom_.imu_data_.m_[0] << "," << imu_odom_.imu_data_.m_[1] << "," << imu_odom_.imu_data_.m_[2] << ","\
|
||||||
|
<< imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << ","\
|
||||||
|
<< imu_odom_.vxy_ << "," << imu_odom_.angle_v_ << ","\
|
||||||
|
<< imu_odom_.pose_[0] << "," << imu_odom_.pose_[1] << ","\
|
||||||
|
<< lighthouse_->data.x_ << "," << lighthouse_->data.y_ << "," << lighthouse_->data.z_ << ","\
|
||||||
|
<< lighthouse_->data.qw_ << "," << lighthouse_->data.qx_ << "," << lighthouse_->data.qy_ << "," << lighthouse_->data.qz_ << ","\
|
||||||
|
<< uwb_->d[0] << "," << uwb_->d[1] << "," << uwb_->d[2] << "\n";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// outfile.close();
|
||||||
|
// std::cout<< "Data written to file." << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
void Align::wheel_odomCB(const nav_msgs::Odometry& wheel_odom)
|
||||||
|
{
|
||||||
|
imu_odom_.vxy_= wheel_odom.twist.twist.linear.x;
|
||||||
|
imu_odom_.angle_v_ = wheel_odom.twist.twist.angular.z;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
void Align::imuCB(const pibot_msgs::RawImu& imu)
|
||||||
|
{
|
||||||
|
imu_odom_.imu_data_.imu_t_ = imu.header.stamp;
|
||||||
|
imu_odom_.imu_data_.a_[0] = imu.raw_linear_acceleration.x;
|
||||||
|
imu_odom_.imu_data_.a_[1] = imu.raw_linear_acceleration.y;
|
||||||
|
imu_odom_.imu_data_.a_[2] = imu.raw_linear_acceleration.z;
|
||||||
|
|
||||||
|
imu_odom_.imu_data_.g_[0] = imu.raw_angular_velocity.x;
|
||||||
|
imu_odom_.imu_data_.g_[1] = imu.raw_angular_velocity.y;
|
||||||
|
imu_odom_.imu_data_.g_[2] = imu.raw_angular_velocity.z;
|
||||||
|
|
||||||
|
imu_odom_.imu_data_.m_[0] = imu.raw_magnetic_field.x;
|
||||||
|
imu_odom_.imu_data_.m_[1] = imu.raw_magnetic_field.y;
|
||||||
|
imu_odom_.imu_data_.m_[2] = imu.raw_magnetic_field.z;
|
||||||
|
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Align::odomCB(const nav_msgs::Odometry& odom)
|
||||||
|
{
|
||||||
|
odom_tmp_ = odom.header.stamp;
|
||||||
|
imu_odom_.pose_[0] = odom.pose.pose.position.x;
|
||||||
|
imu_odom_.pose_[1] = odom.pose.pose.position.y;
|
||||||
|
imu_odom_.pose_[2] = odom.pose.pose.position.z;
|
||||||
|
imu_odom_.quat_[0] = odom.pose.pose.orientation.w;
|
||||||
|
imu_odom_.quat_[1] = odom.pose.pose.orientation.x;
|
||||||
|
imu_odom_.quat_[2] = odom.pose.pose.orientation.y;
|
||||||
|
imu_odom_.quat_[3] = odom.pose.pose.orientation.z;
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,79 @@
|
||||||
|
#include "lighthouse.h"
|
||||||
|
#include <cmath>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
namespace uwb_slam{
|
||||||
|
Lighthouse::Lighthouse(){
|
||||||
|
std::cout << "Start Run. " << std::endl;
|
||||||
|
|
||||||
|
// 创建UDP套接字
|
||||||
|
UdpSocket = socket(AF_INET, SOCK_DGRAM, 0);
|
||||||
|
if (UdpSocket == -1) {
|
||||||
|
std::cerr << "Error creating UDP socket." << std::endl;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::cout << "Creating UDP socket sucess. " << std::endl;
|
||||||
|
|
||||||
|
// 设置套接字地址结构
|
||||||
|
sockaddr_in udpAddress;
|
||||||
|
std::memset(&udpAddress, 0, sizeof(udpAddress));
|
||||||
|
udpAddress.sin_family = AF_INET;
|
||||||
|
udpAddress.sin_addr.s_addr = htonl(INADDR_ANY);
|
||||||
|
udpAddress.sin_port = htons(PORT);
|
||||||
|
|
||||||
|
// 将套接字绑定到地址
|
||||||
|
if (bind(UdpSocket, (struct sockaddr*)&udpAddress, sizeof(udpAddress)) == -1) {
|
||||||
|
std::cerr << "Error binding UDP socket." << std::endl;
|
||||||
|
close(UdpSocket);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
Lighthouse::~Lighthouse(){
|
||||||
|
close(UdpSocket);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Lighthouse::Run() {
|
||||||
|
while(1){
|
||||||
|
// 这是一个阻塞线程
|
||||||
|
this->UDPRead();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Lighthouse::UDPRead(){
|
||||||
|
char buffer[1024];
|
||||||
|
ssize_t bytesRead = recvfrom(UdpSocket, buffer, sizeof(buffer), 0, nullptr, nullptr);
|
||||||
|
if (bytesRead == -1) {
|
||||||
|
std::cerr << "Error receiving data." << std::endl;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string data(buffer);
|
||||||
|
|
||||||
|
float x,y,z,qw,qx,qy,qz;
|
||||||
|
|
||||||
|
qw = 1.0;
|
||||||
|
|
||||||
|
sscanf(data.c_str(),"%f %f %f %f %f %f",&x,&y,&z,&qx,&qy,&qz);
|
||||||
|
|
||||||
|
mMutexLighthouse.lock();
|
||||||
|
|
||||||
|
this->data.x_ = x;
|
||||||
|
this->data.y_ = y;
|
||||||
|
this->data.z_ = z;
|
||||||
|
|
||||||
|
this->data.qw_ = qw;
|
||||||
|
this->data.qx_ = qx;
|
||||||
|
this->data.qy_ = qy;
|
||||||
|
this->data.qz_ = qz;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
mMutexLighthouse.unlock();
|
||||||
|
|
||||||
|
// 打印接收到的消息
|
||||||
|
buffer[bytesRead] = '\0';
|
||||||
|
//std::cout << "Received: " << buffer << std::endl;
|
||||||
|
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
@ -0,0 +1,60 @@
|
||||||
|
#include "Mat.h"
|
||||||
|
|
||||||
|
// #include "align.h"
|
||||||
|
#include "mapping.h"
|
||||||
|
|
||||||
|
#include <iostream>
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <thread>
|
||||||
|
#include "senddata.h"
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
int main(int argc, char** argv)
|
||||||
|
{
|
||||||
|
ros::init(argc, argv, "locate_info_pub_node"); // Initialize the ROS node
|
||||||
|
|
||||||
|
|
||||||
|
std::shared_ptr<uwb_slam::Mapping> mp = std::make_shared<uwb_slam::Mapping>();
|
||||||
|
std::shared_ptr<uwb_slam::Uwb> uwb = std::make_shared<uwb_slam::Uwb>();
|
||||||
|
std::shared_ptr<uwb_slam::Senddata> sender = std::make_shared<uwb_slam::Senddata>();
|
||||||
|
std::shared_ptr<uwb_slam::Align> align = std::make_shared<uwb_slam::Align>();
|
||||||
|
std::shared_ptr<uwb_slam::Lighthouse> lighthouse = std::make_shared<uwb_slam::Lighthouse>();
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
mp->uwb_ = uwb;
|
||||||
|
mp->align_ = align;
|
||||||
|
align->uwb_ =uwb;
|
||||||
|
align->lighthouse_ = lighthouse;
|
||||||
|
sender->uwb_ = uwb;
|
||||||
|
// // control data fllow in system
|
||||||
|
// std::thread rose_trd ([&system]() {
|
||||||
|
// system->Run();
|
||||||
|
// });
|
||||||
|
|
||||||
|
// uwb serried read
|
||||||
|
std::thread uwb_trd([&uwb]() {
|
||||||
|
uwb->Run();
|
||||||
|
});
|
||||||
|
|
||||||
|
// build map
|
||||||
|
// std::thread map_trd ([&mp]() {
|
||||||
|
// mp->Run();
|
||||||
|
// });
|
||||||
|
|
||||||
|
std::thread sender_trd ([&sender, uwb]() {
|
||||||
|
sender->Run(uwb);
|
||||||
|
});
|
||||||
|
|
||||||
|
// std::thread align_trd ([&align]() {
|
||||||
|
// align->Run();
|
||||||
|
// });
|
||||||
|
|
||||||
|
// std::thread lighthouse_trd ([&lighthouse]() {
|
||||||
|
// lighthouse->Run();
|
||||||
|
// });
|
||||||
|
|
||||||
|
ros::spin();
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,149 @@
|
||||||
|
#include "mapping.h"
|
||||||
|
#include <mutex>
|
||||||
|
#include <unistd.h>// 包含 usleep() 函数所在的头文件
|
||||||
|
#include <opencv2/core.hpp>
|
||||||
|
#include <opencv2/highgui.hpp>
|
||||||
|
|
||||||
|
namespace uwb_slam
|
||||||
|
{
|
||||||
|
bool Mapping::checkQueue()
|
||||||
|
{
|
||||||
|
//std::unique_lock<std::mutex> lock(mMutexMap);
|
||||||
|
return !posDataQueue.empty();
|
||||||
|
}
|
||||||
|
|
||||||
|
void Mapping::feedPos(const cv::Point2d & imuPosData, const cv::Point2d & uwbPosData, const cv::Point2d & syncPosData)
|
||||||
|
{
|
||||||
|
//std::unique_lock<std::mutex> lock(mMutexMap);
|
||||||
|
posDataQueue.push({imuPosData, uwbPosData, syncPosData});
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void Mapping::process()
|
||||||
|
{
|
||||||
|
{
|
||||||
|
|
||||||
|
//std::unique_lock<std::mutex> lock(mMutexMap);
|
||||||
|
//std::cout << "SIZE: " <<posDataQueue.size() << std::endl;
|
||||||
|
posData = posDataQueue.front();
|
||||||
|
//std::cout << "x: " <<cur_point.x << " y:" << cur_point.y << std::endl;
|
||||||
|
posDataQueue.pop();
|
||||||
|
}
|
||||||
|
/*生成图*/
|
||||||
|
|
||||||
|
int imuPosPointX = posData[0].x / PIXEL_SCALE + ( fmod(posData[0].x ,PIXEL_SCALE) != 0) + realWidth / 2;
|
||||||
|
int imuPosPointY = posData[0].y / PIXEL_SCALE + ( fmod(posData[0].y ,PIXEL_SCALE) != 0) + realHeight / 2;
|
||||||
|
int uwbPosPointX = posData[1].x / PIXEL_SCALE + ( fmod(posData[1].x ,PIXEL_SCALE) != 0) + realWidth / 2;
|
||||||
|
int uwbPosPointY = posData[1].y / PIXEL_SCALE + ( fmod(posData[1].y ,PIXEL_SCALE) != 0) + realHeight / 2;
|
||||||
|
int syncPosPointX = posData[2].x / PIXEL_SCALE + ( fmod(posData[2].x ,PIXEL_SCALE) != 0) + realWidth / 2;
|
||||||
|
int syncPosPointY = posData[2].y / PIXEL_SCALE + ( fmod(posData[2].y ,PIXEL_SCALE) != 0) + realHeight / 2;
|
||||||
|
// std::cout << "syncPos:" <<posData[2].x << " " << posData[2].y;
|
||||||
|
// std::cout << " uwbPos:" <<posData[1].x << " " << posData[1].x;
|
||||||
|
// std::cout << " imuPos:" <<posData[0].x << " " << posData[0].y << std::endl;
|
||||||
|
|
||||||
|
// std::cout << "syncPos:" <<syncPosPointX << " " << syncPosPointY;
|
||||||
|
// std::cout << " uwbPos:" <<uwbPosPointX << " " << uwbPosPointY;
|
||||||
|
// std::cout << " imuPos:" <<imuPosPointX << " " << imuPosPointY << std::endl;
|
||||||
|
// img.at<unsigned char>(imuPosPointY, imuPosPointX) = 0;
|
||||||
|
img.at<cv::Vec3b>(imuPosPointY, imuPosPointX) = cv::Vec3b(0,0,255); //imu trace is red
|
||||||
|
img.at<cv::Vec3b>(uwbPosPointY, uwbPosPointX) = cv::Vec3b(0,255,0); //uwb trace is green
|
||||||
|
img.at<cv::Vec3b>(syncPosPointY, syncPosPointX) = cv::Vec3b(255,0,0); //sync trace is blue
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void Mapping::Run()
|
||||||
|
{
|
||||||
|
|
||||||
|
//int key = cv::waitKey(0);//等待用户按下按键
|
||||||
|
//std::cout << key << std::endl;
|
||||||
|
realWidth = AREA_SIZE / PIXEL_SCALE;
|
||||||
|
realHeight = AREA_SIZE / PIXEL_SCALE;
|
||||||
|
|
||||||
|
img = cv::Mat(realHeight, realWidth, CV_8UC3, cv::Scalar(255,255,255));
|
||||||
|
//cankao
|
||||||
|
for (int j=0;j<AREA_SIZE / PIXEL_SCALE;j+=10)
|
||||||
|
for (int i=0;i<AREA_SIZE / PIXEL_SCALE;i+=10)
|
||||||
|
img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0);
|
||||||
|
|
||||||
|
for (int j=realHeight/2-1; j<=realHeight/2+1; j+=1)
|
||||||
|
for (int i=realWidth/2-1; i<=realWidth/2+1; i+=1)
|
||||||
|
img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0);
|
||||||
|
int i = 0, j = 0;
|
||||||
|
img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
cv::imshow("Image",img);
|
||||||
|
|
||||||
|
// bool printFlag = 0;
|
||||||
|
// std::ofstream outfile("data.txt",std::ofstream::out);
|
||||||
|
// if(outfile.is_open()) {
|
||||||
|
// std::cout << "start saving data" << key << std::endl;
|
||||||
|
// printFlag = 1;
|
||||||
|
// } else {
|
||||||
|
// std::cout<<"file can not open"<<std::endl;
|
||||||
|
// }
|
||||||
|
|
||||||
|
/*
|
||||||
|
std::cout << "waiting" <<std::endl;
|
||||||
|
int key = cv::waitKey(0);
|
||||||
|
if (key == 'q' || key == 27) {
|
||||||
|
this->feed_uwb_data(cv::Point2d(uwb_->x,uwb_->y));
|
||||||
|
readPos = true;
|
||||||
|
std::cout << "non" << key << std::endl;
|
||||||
|
cv::destroyAllWindows();
|
||||||
|
}
|
||||||
|
*/
|
||||||
|
while(1){
|
||||||
|
int key = cv::waitKey(0);
|
||||||
|
if (key == 'q' ) {
|
||||||
|
readPos = true;
|
||||||
|
std::cout << "start mapping" << key << std::endl;
|
||||||
|
|
||||||
|
//cv::destroyAllWindows();
|
||||||
|
}
|
||||||
|
while( readPos )//按下空格键
|
||||||
|
{
|
||||||
|
int key2 = cv::waitKey(1);
|
||||||
|
if (key2 == 'q') {
|
||||||
|
//TODO: save
|
||||||
|
|
||||||
|
std::string pngimage="/home/firefly/Project_Ros11/Project_Ros1/src/pibot_msgs/Map/output_image.png";//保存的图片文件路径
|
||||||
|
cv::imwrite(pngimage,img);
|
||||||
|
readPos = false;
|
||||||
|
|
||||||
|
// outfile.close();
|
||||||
|
// printFlag = 0;
|
||||||
|
// std::cout<< "Data written to file." << std::endl;
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
//this->feedPos(cv::Point2d(align_->imuPos.mat[0][0], align_->imuPos.mat[1][0]), cv::Point2d(align_->uwbPos.mat[0][0], align_->uwbPos.mat[1][0]), cv::Point2d(align_->syncPos.mat[0][0], align_->syncPos.mat[1][0]));
|
||||||
|
//this->feedPos(cv::Point2d(uwb_->x, uwb_->y));
|
||||||
|
|
||||||
|
//uwb xieru
|
||||||
|
//std::cout << "cur_SEQ: " <<uwb_->cur_seq << std::endl;
|
||||||
|
|
||||||
|
if(checkQueue())
|
||||||
|
{
|
||||||
|
//std::cout << " start process" << std::endl;
|
||||||
|
process();
|
||||||
|
//std::cout << " end process" << std::endl;
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// std::cout << "out" << key << std::endl;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
//std::string pngimage="../Map/pngimage.png";//保存的图片文件路径
|
||||||
|
//cv::imwrite(pngimage,img);
|
||||||
|
|
||||||
|
/*ros 发送图片给导航 */
|
||||||
|
}
|
||||||
|
} // namespace uwb_slam
|
||||||
|
|
||||||
|
|
@ -0,0 +1,147 @@
|
||||||
|
#include "senddata.h"
|
||||||
|
|
||||||
|
#define angleLimit 20
|
||||||
|
#define disLimit 200
|
||||||
|
|
||||||
|
namespace uwb_slam
|
||||||
|
{
|
||||||
|
void Senddata::Run(std::shared_ptr<uwb_slam::Uwb>uwb){
|
||||||
|
|
||||||
|
ros::Rate loop_rate(10);
|
||||||
|
// position_pub_=nh_.advertise<nav_msgs::Odometry>("uwb_odom",50);
|
||||||
|
cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel",50);
|
||||||
|
obstacles_sub_ = nh_.subscribe<pibot_msgs::dis_info_array>("ceju_info",1,boost::bind(&Senddata::stereoCB,this,_1));
|
||||||
|
// odom_sub_=nh_.subscribe("odom",10,&Senddata::odomCB,this);
|
||||||
|
while(ros::ok()){
|
||||||
|
// publishOdometry(uwb);
|
||||||
|
if (flag_)
|
||||||
|
{
|
||||||
|
cmd_vel_.linear.x = 0;
|
||||||
|
cmd_vel_.angular.z = -1;
|
||||||
|
for (int i = 0; i < 17; ++i) {
|
||||||
|
cmd_vel_pub_.publish(cmd_vel_);
|
||||||
|
loop_rate.sleep();
|
||||||
|
}
|
||||||
|
|
||||||
|
cmd_vel_.linear.x = 0.2;
|
||||||
|
cmd_vel_.angular.z = 0;
|
||||||
|
for (int i = 0; i < 20; ++i) {
|
||||||
|
cmd_vel_pub_.publish(cmd_vel_);
|
||||||
|
loop_rate.sleep();
|
||||||
|
}
|
||||||
|
|
||||||
|
cmd_vel_.linear.x = 0;
|
||||||
|
cmd_vel_.angular.z = 1;
|
||||||
|
for (int i = 0; i < 16; ++i) {
|
||||||
|
cmd_vel_pub_.publish(cmd_vel_);
|
||||||
|
loop_rate.sleep();
|
||||||
|
}
|
||||||
|
|
||||||
|
cmd_vel_.linear.x = 0;
|
||||||
|
cmd_vel_.angular.z = 0;
|
||||||
|
flag_ = 0;
|
||||||
|
}
|
||||||
|
else if(abs(uwb->aoa[0]) > 180 || abs(uwb->d[0]) > 2000)
|
||||||
|
{
|
||||||
|
cmd_vel_.angular.z = 0;
|
||||||
|
cmd_vel_.linear.x = 0;
|
||||||
|
}
|
||||||
|
else if(uwb->aoa[0] > angleLimit && uwb->aoa[0] <= 180)
|
||||||
|
{
|
||||||
|
float angularSpeed = (float)uwb->aoa[0] / 100 + 1;
|
||||||
|
cmd_vel_.angular.z = angularSpeed;
|
||||||
|
cmd_vel_.linear.x = 0;
|
||||||
|
}
|
||||||
|
else if(uwb->aoa[0] < -angleLimit)
|
||||||
|
{
|
||||||
|
float angularSpeed = (float)uwb->aoa[0] / 100 - 1;
|
||||||
|
cmd_vel_.angular.z = angularSpeed;
|
||||||
|
cmd_vel_.linear.x = 0;
|
||||||
|
}
|
||||||
|
else if(uwb->d[0] > disLimit)
|
||||||
|
{
|
||||||
|
float lineSpeed = (float)uwb->d[0] / 1000 + 0.1;
|
||||||
|
cmd_vel_.angular.z = 0;
|
||||||
|
cmd_vel_.linear.x = lineSpeed;
|
||||||
|
}
|
||||||
|
else if(uwb->d[0] < disLimit - 30)
|
||||||
|
{
|
||||||
|
cmd_vel_.angular.z = 0;
|
||||||
|
cmd_vel_.linear.x = -0.2;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
cmd_vel_.angular.z = 0;
|
||||||
|
cmd_vel_.linear.x = 0;
|
||||||
|
}
|
||||||
|
cmd_vel_pub_.publish(cmd_vel_);
|
||||||
|
ros::spinOnce();
|
||||||
|
loop_rate.sleep();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
// void Senddata::odomCB(const nav_msgs::Odometry& odom){
|
||||||
|
// sub_odom_ = odom;
|
||||||
|
// return;
|
||||||
|
// }
|
||||||
|
void Senddata::stereoCB(const pibot_msgs::dis_info_array::ConstPtr& stereo)
|
||||||
|
{
|
||||||
|
for (const auto& obstacle_info :stereo->dis)
|
||||||
|
{
|
||||||
|
float distance = obstacle_info.distance;
|
||||||
|
// float width = obstacle_info.width;
|
||||||
|
// float height = obstacle_info.height;
|
||||||
|
// float angle = obstacle_info.angle;
|
||||||
|
|
||||||
|
// if(distance>5&&distance<45&&cmd_vel_.linear.x!=0)
|
||||||
|
if(distance>5&&distance<45)
|
||||||
|
{
|
||||||
|
flag_ = 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// void Senddata::publishOdometry(std::shared_ptr<uwb_slam::Uwb>uwb)
|
||||||
|
// {
|
||||||
|
|
||||||
|
// std::mutex mMutexSend;
|
||||||
|
// ros::Time current_time = ros::Time::now();
|
||||||
|
|
||||||
|
// // 设置 Odometry 消息的头部信息
|
||||||
|
// odom_.header.stamp = current_time;
|
||||||
|
// odom_.header.frame_id = "odom"; // 设置坐标系为 "map"
|
||||||
|
// odom_.child_frame_id = "base_link"; // 设置坐标系为 "base_link"
|
||||||
|
|
||||||
|
// // 填充 Odometry 消息的位置信息
|
||||||
|
// odom_.pose.pose.position.x = uwb->uwb_data_.x_;
|
||||||
|
// odom_.pose.pose.position.y = uwb->uwb_data_.y_;
|
||||||
|
// odom_.pose.pose.position.z = 0.0;
|
||||||
|
|
||||||
|
|
||||||
|
// // 填充 Odometry 消息的姿态信息（使用四元数来表示姿态）
|
||||||
|
// // tf2::Quaternion quat;
|
||||||
|
// // quat.setRPY(0, 0, uwb->theta); // 设置了 yaw 角度，其他 roll 和 pitch 设置为 0
|
||||||
|
// // odom.pose.pose.orientation.x = quat.x();
|
||||||
|
// // odom.pose.pose.orientation.y = quat.y();
|
||||||
|
// // odom.pose.pose.orientation.z = quat.z();
|
||||||
|
// // odom.pose.pose.orientation.w = quat.w();
|
||||||
|
|
||||||
|
// odom_.pose.pose.orientation.x = sub_odom_.pose.pose.orientation.x;
|
||||||
|
// odom_.pose.pose.orientation.y = sub_odom_.pose.pose.orientation.y;
|
||||||
|
// odom_.pose.pose.orientation.z = sub_odom_.pose.pose.orientation.z;
|
||||||
|
// odom_.pose.pose.orientation.w = sub_odom_.pose.pose.orientation.w;
|
||||||
|
|
||||||
|
// // 发布 Odometry 消息
|
||||||
|
// position_pub_.publish(odom_);
|
||||||
|
|
||||||
|
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
} // namespace uwb_slam
|
||||||
|
|
@ -0,0 +1,121 @@
|
||||||
|
#include "uwb.h"
|
||||||
|
#include <cmath>
|
||||||
|
#include "Mat.h"
|
||||||
|
|
||||||
|
|
||||||
|
#define CARHEIGHT 20
|
||||||
|
#define DISINDEX 3
|
||||||
|
#define AOAINDEX 15
|
||||||
|
#define DATALENGTH 27
|
||||||
|
|
||||||
|
namespace uwb_slam{
|
||||||
|
Uwb::Uwb(){
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void Uwb::Run() {
|
||||||
|
|
||||||
|
while(1){
|
||||||
|
this->Serread();
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void Uwb::Serread(){
|
||||||
|
try {
|
||||||
|
boost::asio::io_service io;
|
||||||
|
boost::asio::serial_port serial(io, "/dev/ttyUSB1"); // 替换成你的串口设备路径
|
||||||
|
|
||||||
|
serial.set_option(boost::asio::serial_port_base::baud_rate(115200)); // 设置波特率
|
||||||
|
serial.set_option(boost::asio::serial_port_base::character_size(8)); // 设置数据位
|
||||||
|
serial.set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none)); // 设置校验位
|
||||||
|
serial.set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one)); // 设置停止位
|
||||||
|
|
||||||
|
uint8_t tmpdata[DATALENGTH];
|
||||||
|
// std::cerr << "befor read" << std::endl;
|
||||||
|
size_t bytesRead = boost::asio::read(serial, boost::asio::buffer(tmpdata, sizeof(tmpdata))); // 读取串口数据
|
||||||
|
// std::cerr << "after read" << std::endl;
|
||||||
|
this->uwb_data_.uwb_t_ = ros::Time::now();
|
||||||
|
|
||||||
|
for (int i=0;i< bytesRead;i++)
|
||||||
|
{
|
||||||
|
std::cout << std::hex<<static_cast<int>(tmpdata[i]) << " ";
|
||||||
|
}
|
||||||
|
std::cout << std::endl;
|
||||||
|
|
||||||
|
memcpy(&this->d[0], &tmpdata[DISINDEX], sizeof(d[0]));
|
||||||
|
memcpy(&this->d[1], &tmpdata[DISINDEX + sizeof(d[0])], sizeof(d[0]));
|
||||||
|
memcpy(&this->d[2], &tmpdata[DISINDEX + sizeof(d[0]) * 2], sizeof(d[0]));
|
||||||
|
memcpy(&this->aoa[0], &tmpdata[AOAINDEX], sizeof(aoa[0]));
|
||||||
|
memcpy(&this->aoa[1], &tmpdata[AOAINDEX + sizeof(aoa[0])], sizeof(aoa[0]));
|
||||||
|
memcpy(&this->aoa[2], &tmpdata[AOAINDEX + sizeof(aoa[0]) * 2], sizeof(aoa[0]));
|
||||||
|
|
||||||
|
//std::cout << "d:" << d[0] << " " << d[1] << " " << d[2] << std::endl;
|
||||||
|
|
||||||
|
// if(abs(d[0]) > 2000 || abs(d[1]) > 2000 || abs(d[2]) > 2000) {
|
||||||
|
// return;
|
||||||
|
// }
|
||||||
|
// for(int i=0; i<3; i++)
|
||||||
|
// {
|
||||||
|
// this->d[i] = sqrt(this->d[i] * this->d[i] - (AnchorPos[i][2] - CARHEIGHT) * (AnchorPos[i][2] - CARHEIGHT));
|
||||||
|
// }
|
||||||
|
|
||||||
|
std::cout<<"d: "<<this->d[0]<<" aoa: "<<this->aoa[0]<<std::endl;
|
||||||
|
d[0] = ((((-1.4229e-07 * d[0]) + 1.8784e-04) * d[0]) + 0.9112) * d[0] + 2.4464;
|
||||||
|
// d[1] = ((((-2.3004e-07 * d[1]) + 3.2942e-04) * d[1]) + 0.8385) * d[1] + 6.2770;
|
||||||
|
// d[2] = ((((-2.0616e-07 * d[2]) + 3.3886e-04) * d[2]) + 0.8231) * d[2] + 8.1566;
|
||||||
|
} catch (const std::exception& ex) {
|
||||||
|
std::cerr << "Exception: " << ex.what() << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void fusion()
|
||||||
|
{
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
// bool Uwb::checknewdata()
|
||||||
|
// {
|
||||||
|
// std::unique_lock<std::mutex> lock(mMutexUwb);
|
||||||
|
// return !v_buffer_imu_odom_pose_data_.empty();
|
||||||
|
// }
|
||||||
|
|
||||||
|
// void Uwb::Run() {
|
||||||
|
// while(1)
|
||||||
|
// {
|
||||||
|
// if(checknewdata())
|
||||||
|
// {
|
||||||
|
// {
|
||||||
|
// std::unique_lock<std::mutex> lock(mMutexUwb);
|
||||||
|
// Imu_odom_pose_data imu_odom_pose_data = v_buffer_imu_odom_pose_data_.front();
|
||||||
|
// v_buffer_imu_odom_pose_data_.pop();
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// }
|
||||||
|
|
||||||
|
// void Uwb::feed_imu_odom_pose_data(const Imu_odom_pose_data& imu_odom_pose_data){
|
||||||
|
// std::unique_lock<std::mutex> lock(mMutexUwb);
|
||||||
|
// v_buffer_imu_odom_pose_data_.push(imu_odom_pose_data);
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
@ -40,7 +40,12 @@ add_library(${PROJECT_NAME} SHARED
|
||||||
include/senddata.h src/senddata.cpp)
|
include/senddata.h src/senddata.cpp)
|
||||||
|
|
||||||
|
|
||||||
|
# add_message_files(
|
||||||
|
# DIRECTORY msg
|
||||||
|
# FILES
|
||||||
|
# RawImu.msg
|
||||||
|
|
||||||
|
# )
|
||||||
generate_messages(DEPENDENCIES std_msgs geometry_msgs)
|
generate_messages(DEPENDENCIES std_msgs geometry_msgs)
|
||||||
|
|
||||||
catkin_package(CATKIN_DEPENDS std_msgs geometry_msgs)
|
catkin_package(CATKIN_DEPENDS std_msgs geometry_msgs)
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,34 @@
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include "nav_msgs/Odometry.h"
|
||||||
|
#include "geometry_msgs/Twist.h"
|
||||||
|
#include "sensor_msgs/Imu.h"
|
||||||
|
#include "geometry_msgs/PoseStamped.h"
|
||||||
|
#include "geometry_msgs/PoseWithCovarianceStamped.h"
|
||||||
|
#include <boost/thread/mutex.hpp>
|
||||||
|
#include "type.h"
|
||||||
|
#include "uwb.h"
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef READ_SENSOR_DATA_H
|
||||||
|
#define READ_SENSOR_DATA_H
|
||||||
|
|
||||||
|
namespace uwb_slam{
|
||||||
|
typedef boost::shared_ptr<nav_msgs::Odometry const> OdomConstPtr;
|
||||||
|
typedef boost::shared_ptr<sensor_msgs::Imu const> ImuConstPtr;
|
||||||
|
class Read_sensor_data
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
Read_sensor_data();
|
||||||
|
|
||||||
|
void Run(int argc, char* argv[]);
|
||||||
|
//void set_uwb(Uwb * uwb);
|
||||||
|
void imu_call_back(const ImuConstPtr& imu);
|
||||||
|
void odom_call_back(const OdomConstPtr& odom);
|
||||||
|
|
||||||
|
private:
|
||||||
|
ros::Subscriber imu_sub_;
|
||||||
|
ros::Subscriber odom_sub_;
|
||||||
|
|
||||||
|
};
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
@ -1,9 +1,6 @@
|
||||||
#include "../include/system.h"
|
|
||||||
#include "../include/uwb.h"
|
|
||||||
#include "../include/lighthouse.h"
|
#include "../include/lighthouse.h"
|
||||||
#include "Mat.h"
|
#include "Mat.h"
|
||||||
|
#include "mapping.h"
|
||||||
|
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
#include <thread>
|
#include <thread>
|
||||||
|
|
@ -14,11 +11,14 @@
|
||||||
int main(int argc, char** argv)
|
int main(int argc, char** argv)
|
||||||
{
|
{
|
||||||
ros::init(argc, argv, "locate_info_pub_node"); // Initialize the ROS node
|
ros::init(argc, argv, "locate_info_pub_node"); // Initialize the ROS node
|
||||||
|
|
||||||
std::shared_ptr<uwb_slam::Mapping> mp = std::make_shared<uwb_slam::Mapping>();
|
std::shared_ptr<uwb_slam::Mapping> mp = std::make_shared<uwb_slam::Mapping>();
|
||||||
std::shared_ptr<uwb_slam::Uwb> uwb = std::make_shared<uwb_slam::Uwb>();
|
std::shared_ptr<uwb_slam::Uwb> uwb = std::make_shared<uwb_slam::Uwb>();
|
||||||
std::shared_ptr<uwb_slam::Senddata> sender = std::make_shared<uwb_slam::Senddata>();
|
std::shared_ptr<uwb_slam::Senddata> sender = std::make_shared<uwb_slam::Senddata>();
|
||||||
std::shared_ptr<uwb_slam::Align> align = std::make_shared<uwb_slam::Align>();
|
std::shared_ptr<uwb_slam::Align> align = std::make_shared<uwb_slam::Align>();
|
||||||
|
std::shared_ptr<uwb_slam::Lighthouse> lighthouse = std::make_shared<uwb_slam::Lighthouse>();
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
mp->uwb_ = uwb;
|
mp->uwb_ = uwb;
|
||||||
mp->align_ = align;
|
mp->align_ = align;
|
||||||
|
|
|
||||||
|
|
@ -76,7 +76,25 @@ namespace uwb_slam
|
||||||
|
|
||||||
cv::imshow("Image",img);
|
cv::imshow("Image",img);
|
||||||
|
|
||||||
|
// bool printFlag = 0;
|
||||||
|
// std::ofstream outfile("data.txt",std::ofstream::out);
|
||||||
|
// if(outfile.is_open()) {
|
||||||
|
// std::cout << "start saving data" << key << std::endl;
|
||||||
|
// printFlag = 1;
|
||||||
|
// } else {
|
||||||
|
// std::cout<<"file can not open"<<std::endl;
|
||||||
|
// }
|
||||||
|
|
||||||
|
/*
|
||||||
|
std::cout << "waiting" <<std::endl;
|
||||||
|
int key = cv::waitKey(0);
|
||||||
|
if (key == 'q' || key == 27) {
|
||||||
|
this->feed_uwb_data(cv::Point2d(uwb_->x,uwb_->y));
|
||||||
|
readPos = true;
|
||||||
|
std::cout << "non" << key << std::endl;
|
||||||
|
cv::destroyAllWindows();
|
||||||
|
}
|
||||||
|
*/
|
||||||
while(1){
|
while(1){
|
||||||
int key = cv::waitKey(0);
|
int key = cv::waitKey(0);
|
||||||
if (key == 'q' ) {
|
if (key == 'q' ) {
|
||||||
|
|
@ -91,9 +109,14 @@ namespace uwb_slam
|
||||||
if (key2 == 'q') {
|
if (key2 == 'q') {
|
||||||
//TODO: save
|
//TODO: save
|
||||||
|
|
||||||
std::string pngimage = "/home/firefly/Project_Ros/src/ros_merge_test/Map/output_image.png";//保存的图片文件路径
|
std::string pngimage="/home/firefly/Project_Ros11/Project_Ros1/src/upbot_location/Map/output_image.png";//保存的图片文件路径
|
||||||
cv::imwrite(pngimage, img);
|
cv::imwrite(pngimage,img);
|
||||||
read_uwb_ = false;
|
readPos = false;
|
||||||
|
|
||||||
|
// outfile.close();
|
||||||
|
// printFlag = 0;
|
||||||
|
// std::cout<< "Data written to file." << std::endl;
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,30 @@
|
||||||
|
#include <read_sensor_data.h>
|
||||||
|
|
||||||
|
namespace uwb_slam {
|
||||||
|
|
||||||
|
|
||||||
|
//void Read_sensor_data::set_uwb(){}
|
||||||
|
|
||||||
|
|
||||||
|
void Read_sensor_data::imu_call_back(const ImuConstPtr& imu){
|
||||||
|
Imu_data d1= Imu_data(imu->linear_acceleration.x,imu->linear_acceleration.y,imu->linear_acceleration.z,
|
||||||
|
imu->angular_velocity.x,imu->angular_velocity.y,imu->angular_velocity.z);
|
||||||
|
|
||||||
|
}
|
||||||
|
void Read_sensor_data::odom_call_back(const OdomConstPtr& odom){
|
||||||
|
Odom_data d1 = Odom_data(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z,
|
||||||
|
odom->pose.pose.orientation.w,odom->pose.pose.orientation.x, odom->pose.pose.orientation.y, odom->pose.pose.orientation.z,
|
||||||
|
odom->header.stamp,odom->twist.twist.linear.x,odom->twist.twist.linear.y,odom->twist.twist.angular.z);
|
||||||
|
|
||||||
|
}
|
||||||
|
void Read_sensor_data::Run(int argc, char* argv[]){
|
||||||
|
|
||||||
|
ros::init(argc, argv, "imu_odom");
|
||||||
|
// 创建一个节点句柄
|
||||||
|
ros::NodeHandle nh;
|
||||||
|
//imu_sub_ = nh.subscribe<std_msgs::String>("imu/data_raw", 1000, this->imu_call_back);
|
||||||
|
//odom_sub_ =nh.subscribe("odom", 1000,odom_call_back);
|
||||||
|
ros::spin();
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
@ -4,38 +4,25 @@
|
||||||
namespace uwb_slam
|
namespace uwb_slam
|
||||||
{
|
{
|
||||||
void Senddata::Run(std::shared_ptr<uwb_slam::Uwb>uwb){
|
void Senddata::Run(std::shared_ptr<uwb_slam::Uwb>uwb){
|
||||||
// 初始化了一个名为loop_rate的ros::Rate对象,频率设置为10赫兹
|
|
||||||
ros::Rate loop_rate(10);
|
ros::Rate loop_rate(10);
|
||||||
// 初始化一个ROS发布者，用于发布nav_msgs::Odometry类型的消息
|
|
||||||
// 主题被设置为"uwb_odom"，队列大小为50
|
|
||||||
position_pub_=nh_.advertise<nav_msgs::Odometry>("uwb_odom",50);
|
position_pub_=nh_.advertise<nav_msgs::Odometry>("uwb_odom",50);
|
||||||
// 初始化了一个ROS订阅者，用于订阅"odom"主题。它指定了当在该主题上接收到
|
|
||||||
// 消息时，将调用Senddata类的odomCB回调函数。队列大小被设置为10
|
|
||||||
odom_sub_=nh_.subscribe("odom",10,&Senddata::odomCB,this);
|
odom_sub_=nh_.subscribe("odom",10,&Senddata::odomCB,this);
|
||||||
while(ros::ok()){
|
while(ros::ok()){
|
||||||
// 按照10Hz频率发布uwb信息
|
|
||||||
publishOdometry(uwb);
|
publishOdometry(uwb);
|
||||||
ros::spinOnce();
|
ros::spinOnce();
|
||||||
// 用于控制循环速率
|
|
||||||
loop_rate.sleep();
|
loop_rate.sleep();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
void Senddata::odomCB(const nav_msgs::Odometry& odom){
|
void Senddata::odomCB(const nav_msgs::Odometry& odom){
|
||||||
// 这个地方接收的是轮速里程计的信息
|
|
||||||
// 包含位置和姿态
|
|
||||||
sub_odom_ = odom;
|
sub_odom_ = odom;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
void Senddata::publishOdometry(std::shared_ptr<uwb_slam::Uwb>uwb)
|
||||||
* Function : Senddata::publishOdometry
|
{
|
||||||
* Description : 发布UWB里程计数据，这里读取的数据到底是什么，依旧存在疑问
|
|
||||||
* Date : 2023/12/13 zhanli@review
|
|
||||||
*---------------------------------------------------------------------**/
|
|
||||||
void Senddata::publishOdometry(std::shared_ptr<uwb_slam::Uwb> uwb)
|
|
||||||
{
|
|
||||||
|
|
||||||
std::mutex mMutexSend;
|
std::mutex mMutexSend;
|
||||||
ros::Time current_time = ros::Time::now();
|
ros::Time current_time = ros::Time::now();
|
||||||
|
|
|
||||||
|
|
@ -2,7 +2,8 @@
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include "Mat.h"
|
#include "Mat.h"
|
||||||
|
|
||||||
#define PI acos(-1)
|
|
||||||
|
#define CARHEIGHT 20
|
||||||
|
|
||||||
namespace uwb_slam{
|
namespace uwb_slam{
|
||||||
Mat A;
|
Mat A;
|
||||||
|
|
@ -125,7 +126,10 @@ namespace uwb_slam{
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void fusion()
|
||||||
|
{
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -134,4 +138,34 @@ namespace uwb_slam{
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// bool Uwb::checknewdata()
|
||||||
|
// {
|
||||||
|
// std::unique_lock<std::mutex> lock(mMutexUwb);
|
||||||
|
// return !v_buffer_imu_odom_pose_data_.empty();
|
||||||
|
// }
|
||||||
|
|
||||||
|
// void Uwb::Run() {
|
||||||
|
// while(1)
|
||||||
|
// {
|
||||||
|
// if(checknewdata())
|
||||||
|
// {
|
||||||
|
// {
|
||||||
|
// std::unique_lock<std::mutex> lock(mMutexUwb);
|
||||||
|
// Imu_odom_pose_data imu_odom_pose_data = v_buffer_imu_odom_pose_data_.front();
|
||||||
|
// v_buffer_imu_odom_pose_data_.pop();
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// }
|
||||||
|
|
||||||
|
// void Uwb::feed_imu_odom_pose_data(const Imu_odom_pose_data& imu_odom_pose_data){
|
||||||
|
// std::unique_lock<std::mutex> lock(mMutexUwb);
|
||||||
|
// v_buffer_imu_odom_pose_data_.push(imu_odom_pose_data);
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue