forked from logzhan/RobotKernal-UESTC
融合UWB定位，小车验证版
parent
8354e4d9ea
commit
f9631ad63d
|
|
@ -13,7 +13,7 @@
|
|||
<!-- 大车参数 -->
|
||||
<!-- <arg name="robot_radius" default="0.3"/>
|
||||
<arg name="coverage_radius" default="0.3"/> -->
|
||||
<arg name="use_test_maps" default="true"/>
|
||||
<arg name="use_test_maps" default="false"/>
|
||||
|
||||
<!-- -->
|
||||
<node ns="room_exploration" pkg="ipa_room_exploration" type="room_exploration_client" name="room_exploration_client" output="screen">
|
||||
|
|
@ -27,6 +27,6 @@
|
|||
<!-- <node pkg="ipa_room_exploration" type="talkerUltraSound" name="talkerUltraSound" required="true"/> -->
|
||||
|
||||
<!-- <include file="$(find ipa_room_exploration)/ros/launch/clean_work.launch"/> -->
|
||||
<include file="$(find pibot_navigation)/launch/nav.launch"/>
|
||||
<!-- <include file="$(find pibot_navigation)/launch/nav.launch"/> -->
|
||||
|
||||
</launch>
|
||||
|
|
|
|||
Binary file not shown.
|
After Width: | Height: | Size: 994 B |
Binary file not shown.
|
After Width: | Height: | Size: 1.0 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.1 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.1 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.2 KiB |
|
|
@ -37,8 +37,8 @@
|
|||
<node pkg="tf" type="static_transform_publisher" name="base_imu_to_base_link"
|
||||
args="0 0.0 0 0 0.0 0.0 /base_link /imu_link 40" />
|
||||
|
||||
<!-- <node pkg="tf" type="static_transform_publisher" name="fix_map" args="1 -1 0 0 0 0 /map /odom 50" /> -->
|
||||
<node pkg="tf" type="static_transform_publisher" name="fix_map" args="-2 -2 0 0 0 0 /map /odom 50" />
|
||||
<node pkg="tf" type="static_transform_publisher" name="fix_map" args="0 0 0 0 0 0 /map /odom 50" />
|
||||
<!-- <node pkg="tf" type="static_transform_publisher" name="fix_map" args="-2 -2 0 0 0 0 /map /odom 50" /> -->
|
||||
|
||||
<!-- <node pkg="tf" type="static_transform_publisher" name="fix_odom" args="0 0 0 0 0 0 /odom /base_link 50" /> -->
|
||||
<!-- <node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="1.0 1.0 0.0 0 0 0.0 /map /odom 1000"/> -->
|
||||
|
|
@ -66,8 +66,8 @@
|
|||
<group ns="/" if="$(arg use_odom)" >
|
||||
<!-- robot pose ekf -->
|
||||
<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
|
||||
<!-- <param name="output_frame" value="odom" /> -->
|
||||
<param name="output_frame" value="odom" />
|
||||
<param name="output_frame" type="bool" value="false" />
|
||||
<param name="base_footprint_frame" value="base_link"/>
|
||||
<param name="freq" value="2000.0"/>
|
||||
<param name="sensor_timeout" value="1.0"/>
|
||||
|
|
@ -82,7 +82,8 @@
|
|||
<!-- odom type adapter -->
|
||||
<node pkg="pibot_bringup" type="odom_ekf.py" name="odom_ekf" output="screen">
|
||||
<remap from="input" to="robot_pose_ekf/odom_combined"/>
|
||||
<remap from="output" to="odom"/>
|
||||
<remap from="output" to="odom_imu_ekf"/>
|
||||
</node>
|
||||
</group>
|
||||
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -67,8 +67,8 @@ class OdomEKF():
|
|||
def pub_ekf_odom(self, msg):
|
||||
odom = Odometry()
|
||||
odom.header = msg.header
|
||||
odom.header.frame_id = '/odom'
|
||||
odom.child_frame_id = 'base_link'
|
||||
# odom.header.frame_id = '/odom'
|
||||
# odom.child_frame_id = 'base_link'
|
||||
odom.pose = msg.pose
|
||||
odom.twist = self.twist
|
||||
self.ekf_pub.publish(odom)
|
||||
|
|
|
|||
|
|
@ -12,7 +12,7 @@
|
|||
<node name="map_server" pkg="map_server" type="map_server" args="$(find pibot_navigation)/maps/$(arg map_name)"/>
|
||||
|
||||
<include file="$(find pibot_navigation)/launch/include/move_base.launch.xml" />
|
||||
|
||||
<include file="$(find upbot_location)/launch/upbot_location.launch" />
|
||||
<!-- <include file="$(find pibot_navigation)/launch/include/amcl.launch.xml" /> -->
|
||||
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -33,6 +33,7 @@ DWAPlannerROS:
|
|||
# Goal Tolerance Parameters
|
||||
yaw_goal_tolerance: 0.2
|
||||
xy_goal_tolerance: 0.15
|
||||
latch_xy_goal_tolerance: true
|
||||
# latch_xy_goal_tolerance: false
|
||||
|
||||
# Forward Simulation Parameters
|
||||
|
|
|
|||
|
|
@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS
|
|||
message_generation
|
||||
pibot_msgs
|
||||
geometry_msgs
|
||||
tf2_ros
|
||||
)
|
||||
|
||||
# 寻找OpenCV库
|
||||
|
|
@ -18,7 +19,7 @@ find_package(Boost REQUIRED)
|
|||
# catkin_package(
|
||||
# # INCLUDE_DIRS include
|
||||
# # LIBRARIES upbot_location
|
||||
# # CATKIN_DEPENDS roscpp rospy std_msgs
|
||||
# # CATKIN_DEPENDS roscpp rospy std_msgs tf2_ros
|
||||
# # DEPENDS system_lib
|
||||
# CATKIN_DEPENDS message_runtime std_msgs geometry_msgs
|
||||
# )
|
||||
|
|
|
|||
|
|
@ -1,3 +1,5 @@
|
|||
#ifndef ALIGN_H
|
||||
#define AlIGN_H
|
||||
#include <cmath>
|
||||
#include <ros/ros.h>
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
|
@ -11,39 +13,37 @@
|
|||
#include "lighthouse.h"
|
||||
#include "Mat.h"
|
||||
|
||||
#ifndef ALIGN_H
|
||||
#define AlIGN_H
|
||||
|
||||
namespace uwb_slam{
|
||||
class Align
|
||||
{
|
||||
public:
|
||||
Align(){
|
||||
imuPos.Init(2, 1, 0);
|
||||
imu_odomPos.Init(2, 1, 0);
|
||||
uwbPos.Init(2, 1, 0);
|
||||
syncPos.Init(2, 1, 0);
|
||||
imuStartPos.Init(2, 1, 0);
|
||||
imu_odom_StartPos.Init(2, 1, 0);
|
||||
Rotate.Init(2,2,0);
|
||||
uwbStartPos.Init(2,1,0);
|
||||
};
|
||||
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);
|
||||
void odom_imuCB(const nav_msgs::Odometry& odom);
|
||||
|
||||
public:
|
||||
ros::NodeHandle nh_;
|
||||
ros::Subscriber wheel_odom_sub_;
|
||||
ros::Subscriber imu_sub_;
|
||||
ros::Subscriber odom_sub_;
|
||||
ros::Subscriber odom_imu_sub_;
|
||||
Imu_odom_pose_data imu_odom_;
|
||||
Uwb_data uwb_data_;
|
||||
ros::Time imuDataRxTime, uwbDataRxTime, odomDataRxTime;
|
||||
Mat imuStartPos;
|
||||
Mat imuPos;
|
||||
Mat imu_odom_StartPos;
|
||||
Mat imu_odomPos;
|
||||
Mat uwbPos;
|
||||
Mat syncPos;
|
||||
Mat Rotate;
|
||||
Mat uwbStartPos;
|
||||
double qx, qy, qz, qw;
|
||||
ros::Time odom_tmp_ ;
|
||||
bool write_data_ = false;
|
||||
cv::Mat img1;
|
||||
|
|
|
|||
|
|
@ -2,7 +2,8 @@
|
|||
#include <opencv2/opencv.hpp>
|
||||
#include <mutex>
|
||||
#include "uwb.h"
|
||||
#include "align.h"
|
||||
// #include "align.h"
|
||||
#include "senddata.h"
|
||||
|
||||
#ifndef MAPPING_H
|
||||
#define MAPPING_H
|
||||
|
|
|
|||
|
|
@ -1,34 +1,38 @@
|
|||
|
||||
#ifndef SENDDATA_H
|
||||
#define SENDDATA_H
|
||||
#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 <tf2_ros/transform_broadcaster.h>
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
#include <mutex>
|
||||
#include "uwb.h"
|
||||
#ifndef SENDDATA_H
|
||||
#define SENDDATA_H
|
||||
#include "align.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 publishOdometry();
|
||||
void publishtf();
|
||||
void Run();
|
||||
|
||||
public:
|
||||
std::shared_ptr<uwb_slam::Align> align_;
|
||||
std::mutex mMutexSend;
|
||||
|
||||
private:
|
||||
ros::Publisher position_pub_;
|
||||
ros::Subscriber odom_sub_;
|
||||
ros::NodeHandle nh_;
|
||||
nav_msgs::Odometry odom_;//odom的消息类型
|
||||
nav_msgs::Odometry sub_odom_;//odom的消息类型
|
||||
|
||||
|
||||
ros::Publisher position_pub_;
|
||||
tf2_ros::TransformBroadcaster broadcaster;
|
||||
nav_msgs::Odometry odom_imu_uwb_;//消息类型 发布融合uwb imu 里程计信息
|
||||
geometry_msgs::TransformStamped transformStamped;//坐标变换消息
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,5 @@
|
|||
<launch>
|
||||
|
||||
<node pkg="upbot_location" type="upbot_location_node" name="upbot_location" output="screen" />
|
||||
|
||||
</launch>
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 36 KiB |
|
|
@ -6,9 +6,8 @@ namespace uwb_slam{
|
|||
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);
|
||||
odom_imu_sub_= nh_.subscribe("odom_imu_ekf",10,&Align::odom_imuCB,this);
|
||||
|
||||
std::ofstream outfile("data.txt",std::ofstream::out);
|
||||
if(outfile.is_open()) {
|
||||
|
|
@ -18,7 +17,7 @@ namespace uwb_slam{
|
|||
<< "wX,wY,wZ,"\
|
||||
<< "qX,qY,qZ,qW,"\
|
||||
<< "vXY,angleV,"\
|
||||
<< "imuPosX,imuPosY,"\
|
||||
<< "imu_odomPosX,imu_odomPosY,"\
|
||||
<< "uwbPosX,uwbPosY,"\
|
||||
<< "syncPosX,syncPosY,"\
|
||||
<< "lightHousePosX,lightHousePosY,lightHousePosZ,"
|
||||
|
|
@ -28,7 +27,7 @@ namespace uwb_slam{
|
|||
}
|
||||
|
||||
Mat prevPos(2, 1, 0);
|
||||
Mat detPos(2, 1, 0);
|
||||
Mat deltaPos(2, 1, 0);
|
||||
Mat predPos(2, 1, 0);
|
||||
Mat R(2, 2, 1);
|
||||
Mat Q(2, 2 ,1);
|
||||
|
|
@ -44,8 +43,8 @@ namespace uwb_slam{
|
|||
Mat I = F;
|
||||
bool imuStartFlag = 1;
|
||||
bool printFlag = 0;
|
||||
double qx, qy, qz, qw;
|
||||
float roll, imuStartRoll;
|
||||
|
||||
float yaw, imuStartyaw;
|
||||
while(1){
|
||||
// int key3 = cv::waitKey(1);
|
||||
// if(key3 == 's'){
|
||||
|
|
@ -54,42 +53,41 @@ namespace uwb_slam{
|
|||
// printFlag = 0;
|
||||
// }
|
||||
if(imuDataRxTime!=imu_odom_.imu_data_.imu_t_){
|
||||
std::cout << "imu received" << std::endl;
|
||||
imuPos.mat[0][0] = imu_odom_.pose_[0] * 100;
|
||||
imuPos.mat[1][0] = imu_odom_.pose_[1] * 100;
|
||||
// std::cout << "imu received" << std::endl;
|
||||
imu_odomPos.mat[0][0] = imu_odom_.pose_[0] * 100;
|
||||
imu_odomPos.mat[1][0] = imu_odom_.pose_[1] * 100;
|
||||
qx = imu_odom_.quat_[0];
|
||||
qy = imu_odom_.quat_[1];
|
||||
qz = imu_odom_.quat_[2];
|
||||
qw = imu_odom_.quat_[3];
|
||||
roll = -atan2(2*(qw*qz+qx*qy),1-2*(qy*qy+qz*qz));
|
||||
yaw = -atan2(2*(qw*qz+qx*qy),1-2*(qy*qy+qz*qz));
|
||||
|
||||
std::cout<<"roll:"<<roll<<std::endl;
|
||||
if (imuStartFlag == 1 && imuPos.mat[0][0] != 0 && imuPos.mat[1][0] != 0) {
|
||||
imuStartRoll = roll-PI/2;//跟imu摆放的位置有关，减PI / 2是为了让角度变成跟车前进方向的夹角
|
||||
|
||||
imuStartPos = imuPos;
|
||||
std::cout<<"yaw:"<<yaw<<std::endl;
|
||||
if (imuStartFlag == 1 && imu_odomPos.mat[0][0] != 0 && imu_odomPos.mat[1][0] != 0) {
|
||||
imuStartyaw = yaw-PI/2;//跟imu摆放的位置有关，减PI / 2是为了让角度变成跟车前进方向的夹角
|
||||
imu_odom_StartPos = imu_odomPos;
|
||||
imuStartFlag = 0;
|
||||
}
|
||||
//std::cout << imuStartPos.mat[0][0] << " " << imuStartPos.mat[1][0] << std::endl;
|
||||
roll = roll - imuStartRoll;
|
||||
imuPos = imuPos - imuStartPos;
|
||||
detPos = imuPos - prevPos;
|
||||
prevPos = imuPos;
|
||||
//std::cout << imu_odom_StartPos.mat[0][0] << " " << imu_odom_StartPos.mat[1][0] << std::endl;
|
||||
yaw = yaw - imuStartyaw;
|
||||
imu_odomPos = imu_odomPos - imu_odom_StartPos;
|
||||
deltaPos = imu_odomPos - prevPos;
|
||||
prevPos = imu_odomPos;
|
||||
if (uwbDataRxTime != uwb_->uwb_data_.uwb_t_) {
|
||||
std::cout << "uwb received" << std::endl;
|
||||
// std::cout << "uwb received" << std::endl;
|
||||
//这一步是为了把上述提到的UWB设备与imu设备不在同一坐标轴上设计的坐标轴对齐操作
|
||||
Rotate.mat[0][0] = cos(roll);
|
||||
Rotate.mat[0][1] = -sin(roll);
|
||||
Rotate.mat[1][0] = sin(roll);
|
||||
Rotate.mat[1][1] = cos(roll);
|
||||
// Rotate.mat[0][0] = cos(yaw);
|
||||
// Rotate.mat[0][1] = -sin(yaw);
|
||||
// Rotate.mat[1][0] = sin(yaw);
|
||||
// Rotate.mat[1][1] = cos(yaw);
|
||||
|
||||
uwbPos.mat[0][0] = uwb_->uwb_data_.x_;
|
||||
uwbPos.mat[1][0] = uwb_->uwb_data_.y_;
|
||||
|
||||
uwbPos = uwbPos - Rotate * uwbStartPos;
|
||||
// uwbPos = uwbPos - Rotate * uwbStartPos;
|
||||
//后期如UWB摆放在imu正上方即删除
|
||||
//卡尔曼更新过程
|
||||
predPos = F * syncPos + detPos;
|
||||
predPos = F * syncPos + deltaPos;
|
||||
Z = H * uwbPos;
|
||||
P = F * P * (~F) + Q;
|
||||
Kg = P * (~H) / (H * P * (~H) + R);
|
||||
|
|
@ -97,27 +95,27 @@ namespace uwb_slam{
|
|||
P = (I - Kg * H) * P;
|
||||
uwbDataRxTime = uwb_->uwb_data_.uwb_t_;
|
||||
} else {
|
||||
syncPos = syncPos + detPos;//如果UWB没有更新信息，则使用imu对齐位置进行更新
|
||||
syncPos = syncPos + deltaPos;//如果UWB没有更新信息，则使用imu对齐位置进行更新
|
||||
}
|
||||
imuDataRxTime = imu_odom_.imu_data_.imu_t_;
|
||||
odomDataRxTime = odom_tmp_;
|
||||
|
||||
std::cout << "syncPos:" << syncPos.mat[0][0] << " " << syncPos.mat[1][0];
|
||||
std::cout << " uwbPos:" << uwbPos.mat[0][0] << " " << uwbPos.mat[1][0];
|
||||
std::cout << " imuPos:" << imuPos.mat[0][0] << " " << imuPos.mat[1][0];
|
||||
std::cout << " lightHousePos:" << lighthouse_->data.x_ << " " << lighthouse_->data.y_ << std::endl;
|
||||
std::cout << " imu_odomPos:" << imu_odomPos.mat[0][0] << " " << imu_odomPos.mat[1][0]<< std::endl;
|
||||
// std::cout << " lightHousePos:" << lighthouse_->data.x_ << " " << lighthouse_->data.y_ << std::endl;
|
||||
|
||||
|
||||
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_.w_[0] << "," << imu_odom_.imu_data_.w_[1] << "," << imu_odom_.imu_data_.w_[2] << ","\
|
||||
<< qx << "," << qy << "," << qz << "," << qw << ","\
|
||||
<< imu_odom_.vxy_ << "," <<imu_odom_.angle_v_ << ","\
|
||||
<< imuPos.mat[0][0] << "," << imuPos.mat[1][0] << ","\
|
||||
<< uwbPos.mat[0][0] << "," << uwbPos.mat[1][0] << ","\
|
||||
<< syncPos.mat[0][0] << "," << syncPos.mat[1][0] << ","\
|
||||
<< lighthouse_->data.x_ << "," << lighthouse_->data.y_ << "," << lighthouse_->data.z_ << ","\
|
||||
<< uwb_->d[0] << "," << uwb_->d[1] << "," << uwb_->d[2] << "\n";
|
||||
// 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_.w_[0] << "," << imu_odom_.imu_data_.w_[1] << "," << imu_odom_.imu_data_.w_[2] << ","\
|
||||
// << qx << "," << qy << "," << qz << "," << qw << ","\
|
||||
// << imu_odom_.vxy_ << "," <<imu_odom_.angle_v_ << ","\
|
||||
// << imu_odomPos.mat[0][0] << "," << imu_odomPos.mat[1][0] << ","\
|
||||
// << uwbPos.mat[0][0] << "," << uwbPos.mat[1][0] << ","\
|
||||
// << syncPos.mat[0][0] << "," << syncPos.mat[1][0] << ","\
|
||||
// << lighthouse_->data.x_ << "," << lighthouse_->data.y_ << "," << lighthouse_->data.z_ << ","\
|
||||
// << uwb_->d[0] << "," << uwb_->d[1] << "," << uwb_->d[2] << "\n";
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -126,23 +124,7 @@ namespace uwb_slam{
|
|||
// 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;
|
||||
// imu_odom_.pose_[0] = wheel_odom.pose.pose.position.x;
|
||||
// imu_odom_.pose_[1] = wheel_odom.pose.pose.position.y;
|
||||
// imu_odom_.pose_[2] = wheel_odom.pose.pose.position.z;
|
||||
// imu_odom_.quat_[0] = wheel_odom.pose.pose.orientation.x;
|
||||
// imu_odom_.quat_[1] = wheel_odom.pose.pose.orientation.y;
|
||||
// imu_odom_.quat_[2] = wheel_odom.pose.pose.orientation.z;
|
||||
// imu_odom_.quat_[3] = wheel_odom.pose.pose.orientation.w;
|
||||
|
||||
|
||||
return;
|
||||
}
|
||||
//imu原始数据，不知道有没有纠正零偏的
|
||||
void Align::imuCB(const pibot_msgs::RawImu& imu)
|
||||
{
|
||||
imu_odom_.imu_data_.imu_t_ = imu.header.stamp;
|
||||
|
|
@ -157,7 +139,8 @@ namespace uwb_slam{
|
|||
return;
|
||||
}
|
||||
|
||||
void Align::odomCB(const nav_msgs::Odometry& odom)
|
||||
//imu里程计融合位姿
|
||||
void Align::odom_imuCB(const nav_msgs::Odometry& odom)
|
||||
{
|
||||
odom_tmp_ = odom.header.stamp;
|
||||
imu_odom_.pose_[0] = odom.pose.pose.position.x;
|
||||
|
|
@ -167,6 +150,9 @@ namespace uwb_slam{
|
|||
imu_odom_.quat_[1] = odom.pose.pose.orientation.y;
|
||||
imu_odom_.quat_[2] = odom.pose.pose.orientation.z;
|
||||
imu_odom_.quat_[3] = odom.pose.pose.orientation.w;
|
||||
|
||||
imu_odom_.vxy_= odom.twist.twist.linear.x;
|
||||
imu_odom_.angle_v_ = odom.twist.twist.angular.z;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -4,7 +4,6 @@
|
|||
#include <iostream>
|
||||
#include <ros/ros.h>
|
||||
#include <thread>
|
||||
#include "senddata.h"
|
||||
|
||||
|
||||
|
||||
|
|
@ -24,11 +23,7 @@ int main(int argc, char** argv)
|
|||
mp->align_ = align;
|
||||
align->uwb_ = uwb;
|
||||
align->lighthouse_ = lighthouse;
|
||||
|
||||
// // control data fllow in system
|
||||
// std::thread rose_trd ([&system]() {
|
||||
// system->Run();
|
||||
// });
|
||||
sender->align_ = align;
|
||||
|
||||
// uwb serried read
|
||||
std::thread uwb_trd([&uwb]() {
|
||||
|
|
@ -40,8 +35,8 @@ int main(int argc, char** argv)
|
|||
mp->Run();
|
||||
});
|
||||
|
||||
std::thread sender_trd ([&sender, uwb]() {
|
||||
sender->Run(uwb);
|
||||
std::thread sender_trd ([&sender]() {
|
||||
sender->Run();
|
||||
});
|
||||
|
||||
std::thread align_trd ([&align]() {
|
||||
|
|
|
|||
|
|
@ -109,7 +109,7 @@ namespace uwb_slam
|
|||
if (key2 == 'q') {
|
||||
//TODO: save
|
||||
|
||||
std::string pngimage="/home/firefly/Project_Ros11/Project_Ros1/src/upbot_location/Map/output_image.png";//保存的图片文件路径
|
||||
std::string pngimage="/home/firefly/pibot_ros/ros_ws/src/upbot_location/map/output_image.png";//保存的图片文件路径
|
||||
cv::imwrite(pngimage,img);
|
||||
readPos = false;
|
||||
|
||||
|
|
@ -120,7 +120,7 @@ namespace uwb_slam
|
|||
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(align_->imu_odomPos.mat[0][0], align_->imu_odomPos.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
|
||||
|
|
|
|||
|
|
@ -1,20 +1,27 @@
|
|||
/**---------------------------------------------------------------------
|
||||
* Function : Senddata::publishOdometry
|
||||
* Description : 发布UWB IMU 里程计融合的定位信息 一个发布给导航 一个发布给tftree 坐标变换使用
|
||||
* Date : 2024/3/19 linyuehang
|
||||
*---------------------------------------------------------------------**/
|
||||
|
||||
#include "senddata.h"
|
||||
|
||||
|
||||
namespace uwb_slam
|
||||
{
|
||||
void Senddata::Run(std::shared_ptr<uwb_slam::Uwb>uwb){
|
||||
void Senddata::Run(){
|
||||
// 初始化了一个名为loop_rate的ros::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>("odom",10);
|
||||
// 初始化了一个ROS订阅者，用于订阅"odom"主题。它指定了当在该主题上接收到
|
||||
// 消息时，将调用Senddata类的odomCB回调函数。队列大小被设置为10
|
||||
odom_sub_=nh_.subscribe("odom",10,&Senddata::odomCB,this);
|
||||
|
||||
while(ros::ok()){
|
||||
// 按照10Hz频率发布uwb信息
|
||||
publishOdometry(uwb);
|
||||
publishOdometry();
|
||||
publishtf();
|
||||
ros::spinOnce();
|
||||
// 用于控制循环速率
|
||||
loop_rate.sleep();
|
||||
|
|
@ -22,54 +29,62 @@ namespace uwb_slam
|
|||
|
||||
|
||||
}
|
||||
void Senddata::odomCB(const nav_msgs::Odometry& odom){
|
||||
// 这个地方接收的是轮速里程计的信息
|
||||
// 包含位置和姿态
|
||||
sub_odom_ = odom;
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : Senddata::publishOdometry
|
||||
* Description : 发布UWB里程计数据，这里读取的数据到底是什么，依旧存在疑问
|
||||
* Date : 2023/12/13 zhanli@review
|
||||
*---------------------------------------------------------------------**/
|
||||
void Senddata::publishOdometry(std::shared_ptr<uwb_slam::Uwb>uwb)
|
||||
void Senddata::publishOdometry()
|
||||
{
|
||||
|
||||
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"
|
||||
odom_imu_uwb_.header.stamp = current_time;
|
||||
odom_imu_uwb_.header.frame_id = "odom"; // 设置坐标系为 "map"
|
||||
odom_imu_uwb_.child_frame_id = "base_link"; // 设置坐标系为 "base_link"
|
||||
|
||||
// odom_imu_uwb_.header.frame_id = "odom";
|
||||
// odom_imu_uwb_.child_frame_id = "map";
|
||||
// 填充 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;
|
||||
odom_imu_uwb_.pose.pose.position.x = align_->syncPos.mat[0][0]/100;
|
||||
odom_imu_uwb_.pose.pose.position.y = align_->syncPos.mat[1][0]/100;
|
||||
odom_imu_uwb_.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_imu_uwb_.pose.pose.orientation.x = align_->qx;
|
||||
odom_imu_uwb_.pose.pose.orientation.y = align_->qy;
|
||||
odom_imu_uwb_.pose.pose.orientation.z = align_->qz;
|
||||
odom_imu_uwb_.pose.pose.orientation.w = align_->qw;
|
||||
|
||||
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_);
|
||||
position_pub_.publish(odom_imu_uwb_);
|
||||
|
||||
|
||||
}
|
||||
|
||||
void Senddata::publishtf()
|
||||
{
|
||||
transformStamped.header.stamp = ros::Time::now();
|
||||
|
||||
transformStamped.header.frame_id = "odom"; // 父坐标系为 "odom"
|
||||
transformStamped.child_frame_id = "base_link"; // 子坐标系为 "base_link"
|
||||
transformStamped.transform.translation.x = align_->syncPos.mat[0][0]/100; // 设置坐标变换的平移部分
|
||||
transformStamped.transform.translation.y = align_->syncPos.mat[1][0]/100;
|
||||
transformStamped.transform.translation.z = 0.0;
|
||||
transformStamped.transform.rotation.x = align_->qx; // 设置坐标变换的旋转部分（使用四元数表示）
|
||||
transformStamped.transform.rotation.y = align_->qy;
|
||||
transformStamped.transform.rotation.z = align_->qz;
|
||||
transformStamped.transform.rotation.w = align_->qw;
|
||||
|
||||
broadcaster.sendTransform(transformStamped);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue