diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/test/room_exploration_client.launch b/Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/test/room_exploration_client.launch index 3dd5d50..95a45f2 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/test/room_exploration_client.launch +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/test/room_exploration_client.launch @@ -13,7 +13,7 @@ - + @@ -27,6 +27,6 @@ - + diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/160_160.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/160_160.png new file mode 100644 index 0000000..4c1dfbe Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/map/160_160.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/250_250.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/250_250.png new file mode 100644 index 0000000..5dbb712 Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/map/250_250.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/300_300.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/300_300.png new file mode 100644 index 0000000..9116421 Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/map/300_300.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/400_400.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/400_400.png new file mode 100644 index 0000000..dd6b454 Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/map/400_400.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/500_500.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/500_500.png new file mode 100644 index 0000000..7dff090 Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/map/500_500.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/launch/bringup_with_imu.launch b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/launch/bringup_with_imu.launch index 43eb274..3b4bb48 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/launch/bringup_with_imu.launch +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/launch/bringup_with_imu.launch @@ -37,8 +37,8 @@ - - + + @@ -66,8 +66,8 @@ - - + + @@ -82,7 +82,8 @@ - + + diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/scripts/odom_ekf.py b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/scripts/odom_ekf.py index 9db038d..87b9862 100755 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/scripts/odom_ekf.py +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/scripts/odom_ekf.py @@ -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) diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/launch/nav.launch b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/launch/nav.launch index e9faaac..1f4188c 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/launch/nav.launch +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/launch/nav.launch @@ -12,7 +12,7 @@ - + diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/params/dwa_local_planner_params_apollo.yaml b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/params/dwa_local_planner_params_apollo.yaml index f10afd8..184977a 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/params/dwa_local_planner_params_apollo.yaml +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/params/dwa_local_planner_params_apollo.yaml @@ -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 diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/CMakeLists.txt b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/CMakeLists.txt index 26392cc..35820e3 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/CMakeLists.txt +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/CMakeLists.txt @@ -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 # ) diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h index b11132c..601f30b 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h @@ -1,3 +1,5 @@ +#ifndef ALIGN_H +#define AlIGN_H #include #include #include @@ -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; diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/mapping.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/mapping.h index ac39d27..64b5e60 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/mapping.h +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/mapping.h @@ -2,7 +2,8 @@ #include #include #include "uwb.h" -#include "align.h" +// #include "align.h" +#include "senddata.h" #ifndef MAPPING_H #define MAPPING_H diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/senddata.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/senddata.h index efef283..f582139 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/senddata.h +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/senddata.h @@ -1,35 +1,39 @@ + +#ifndef SENDDATA_H +#define SENDDATA_H #include #include #include #include #include +#include +#include #include #include "uwb.h" -#ifndef SENDDATA_H -#define SENDDATA_H +#include "align.h" + namespace uwb_slam{ - class Senddata { public: Senddata(){}; - void publishOdometry( std::shared_ptruwb); - void Run(std::shared_ptruwb); - void odomCB(const nav_msgs::Odometry& odom); - + void publishOdometry(); + void publishtf(); + void Run(); + public: + std::shared_ptr 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;//坐标变换消息 + }; } diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/launch/upbot_location.launch b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/launch/upbot_location.launch new file mode 100644 index 0000000..f726f40 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/launch/upbot_location.launch @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/map/1.png b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/map/1.png new file mode 100644 index 0000000..67d7af0 Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/map/1.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp index 8cb47b9..dbac7a0 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp @@ -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:"<uwb_data_.uwb_t_) { - 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); + // std::cout << "uwb received" << std::endl; + //这一步是为了把上述提到的UWB设备与imu设备不在同一坐标轴上设计的坐标轴对齐操作 + // 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; - //后期如UWB摆放在imu正上方即删除 + // 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_ << "," <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_ << "," <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; } diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp index a8e5068..5d7d868 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp @@ -4,7 +4,6 @@ #include #include #include -#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]() { diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp index a6240b7..d81a85d 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp @@ -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 diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp index 89c2ee8..e295b0a 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp @@ -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_ptruwb){ + void Senddata::Run(){ // 初始化了一个名为loop_rate的ros::Rate对象,频率设置为10赫兹 ros::Rate loop_rate(10); // 初始化一个ROS发布者，用于发布nav_msgs::Odometry类型的消息 // 主题被设置为"uwb_odom"，队列大小为50 - position_pub_=nh_.advertise("uwb_odom",50); + position_pub_=nh_.advertise("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_ptruwb) + 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); + } +