ROS_Robot/Ros_Qt5_Gui_App
1
0
Fork 0
mirror of https://github.com/chengyangkj/Ros_Qt5_Gui_App.git synced 2025-09-15 12:58:58 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

fix ros2 carto mapping bug

Browse Source
This commit is contained in:
chengyangkj 2024-09-28 16:47:40 +08:00
parent ff69a69c6a
commit d514046b7d
6 changed files with 56 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
channel/ros1/rosnode.cpp
View File

@ -254,7 +254,7 @@ void RosNode::LocalCostMapCallback(nav_msgs::OccupancyGrid::ConstPtr msg) {
origin_pose.y = pose_map_frame.point.y + cost_map.heightMap();
origin_pose.theta = 0;
} catch (tf2::TransformException &ex) {
// LOG_ERROR("getTrasnsform localCostMapCallback error:" << ex.what());
// LOG_ERROR("getTransform localCostMapCallback error:" << ex.what());
}
double map_o_x, map_o_y;
@ -316,7 +316,7 @@ void RosNode::LaserScanCallback(sensor_msgs::LaserScanConstPtr msg) {
laser_points.push_back(p);
}
laser_points.id = 0;
// basic::RobotPose pose = getTrasnsform(msg->header.frame_id,
// basic::RobotPose pose = getTransform(msg->header.frame_id,
// "base_link"); std::cout << "get transform" << pose.x << " " << pose.y
// << " " << pose.theta
// << std::endl;
@ -408,7 +408,7 @@ void RosNode::PubRobotSpeed(const RobotSpeed &speed) {
speed_publisher_.publish(twist);
}
void RosNode::GetRobotPose() {
OnDataCallback(MsgId::kRobotPose, GetTrasnsform("base_link", "map"));
OnDataCallback(MsgId::kRobotPose, getTransform("base_link", "map"));
}
/**
* @description: 获取坐标变化
@ -416,7 +416,7 @@ void RosNode::GetRobotPose() {
* @param {string} to 基坐标系
* @return {basic::RobotPose}from变换到to坐标系下，需要变换的坐标
*/
basic::RobotPose RosNode::GetTrasnsform(std::string from, std::string to) {
basic::RobotPose RosNode::getTransform(std::string from, std::string to) {
basic::RobotPose ret;
try {
tf::StampedTransform transform;
@ -434,7 +434,7 @@ basic::RobotPose RosNode::GetTrasnsform(std::string from, std::string to) {
ret.theta = yaw;
} catch (tf2::TransformException &ex) {
LOG_ERROR("getTrasnsform error from:" << from << " to:" << to
LOG_ERROR("getTransform error from:" << from << " to:" << to
<< " error:" << ex.what());
}
return ret;

2
channel/ros1/rosnode.h
View File

@ -42,7 +42,7 @@ class RosNode : public VirtualChannelNode {
void PubNavGoal(const RobotPose &pose);
void PubRelocPose(const RobotPose &pose);
void GetRobotPose();
basic::RobotPose GetTrasnsform(std::string from, std::string to);
basic::RobotPose getTransform(std::string from, std::string to);
private:
ros::Publisher nav_goal_publisher_;

11
channel/ros2/rclcomm.cpp
View File

@ -193,7 +193,7 @@ void rclcomm::BatteryCallback(
OnDataCallback(MsgId::kBatteryState, map);
}
void rclcomm::getRobotPose() {
OnDataCallback(MsgId::kRobotPose, getTrasnsform("base_link", "map"));
OnDataCallback(MsgId::kRobotPose, getTransform("base_link", "map"));
}
/**
* @description: 获取坐标变化
@ -201,7 +201,7 @@ void rclcomm::getRobotPose() {
* @param {string} to 基坐标系
* @return {basic::RobotPose}from变换到to坐标系下，需要变换的坐标
*/
basic::RobotPose rclcomm::getTrasnsform(std::string from, std::string to) {
basic::RobotPose rclcomm::getTransform(std::string from, std::string to) {
basic::RobotPose ret;
try {
geometry_msgs::msg::TransformStamped transform =
@ -222,8 +222,8 @@ basic::RobotPose rclcomm::getTrasnsform(std::string from, std::string to) {
ret.theta = yaw;
} catch (tf2::TransformException &ex) {
LOG_ERROR("getTrasnsform error from:" << from << " to:" << to
<< " error:" << ex.what());
LOG_ERROR("getTransform error from:" << from << " to:" << to
<< " error:" << ex.what());
}
return ret;
}
@ -402,7 +402,7 @@ void rclcomm::localCostMapCallback(
origin_pose.y = pose_map_frame.pose.position.y + cost_map.heightMap();
origin_pose.theta = yaw;
} catch (tf2::TransformException &ex) {
LOG_ERROR("getTrasnsform localCostMapCallback error:" << ex.what());
LOG_ERROR("getTransform localCostMapCallback error:" << ex.what());
}
double map_o_x, map_o_y;
@ -434,6 +434,7 @@ void rclcomm::map_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
occ_map_(x, y) = msg->data[i];
}
occ_map_.SetFlip();
// std::cout << "recv map:" << occ_map_.rows << "," << occ_map_.cols << std::endl;
OnDataCallback(MsgId::kOccupancyMap, occ_map_);
}

2
channel/ros2/rclcomm.h
View File

@ -54,7 +54,7 @@ class rclcomm : public VirtualChannelNode {
void PubRelocPose(const RobotPose &pose);
void PubNavGoal(const RobotPose &pose);
void PubRobotSpeed(const RobotSpeed &speed);
basic::RobotPose getTrasnsform(std::string from, std::string to);
basic::RobotPose getTransform(std::string from, std::string to);
void SendMessage(const MsgId &msg_id, const std::any &msg) override;
private:

61
mainwindow/src/display/display_occ_map.cpp
View File

@ -54,31 +54,37 @@ void DisplayOccMap::paint(QPainter *painter,
void DisplayOccMap::ParseOccupyMap() {
QtConcurrent::run([this]() {
// Eigen::matrix 坐标系与QImage坐标系不同,这里行列反着遍历
map_image_ = QImage(map_data_.Cols(), map_data_.Rows(), QImage::Format_RGB32);
QVector<QPointF> points;
//QImage坐标系
// **************x
// *
// *
// *
// y
for (int i = 0; i < map_data_.Cols(); i++)
map_image_ = QImage(map_data_.Cols(), map_data_.Rows(), QImage::Format_ARGB32);
// 遍历地图数据，设置每个像素的颜色
for (int i = 0; i < map_data_.Cols(); i++) {
for (int j = 0; j < map_data_.Rows(); j++) {
double map_value = map_data_(j, i);
QColor color;
if (map_value == 100) {
color = Qt::black; // black
} else if (map_value == 0) {
color = Qt::white; // gray
} else if (map_value == 255) {
color = Qt::gray; // white
} else {
if (map_value > 0) {
// 将 map_value 从 0-100 映射到透明度 0-255 范围
int alpha = static_cast<int>(std::clamp(map_value * 2.55, 0.0, 255.0));
color = QColor(0, 0, 0, alpha); // 黑色, 透明度根据占据值动态调整
} else if (map_value == 0 || map_value == -1) {
// 自由区域和未知区域都设为白色
color = Qt::white;
} else {
color = Qt::white; // 默认白色
}
map_image_.setPixel(i, j, color.rgb());
// 使用 RGBA 颜色值绘制像素
map_image_.setPixel(i, j, color.rgba());
}
}
// 更新边界矩形
SetBoundingRect(QRectF(0, 0, map_image_.width(), map_image_.height()));
update();
//以0 0点为中心
@ -111,22 +117,41 @@ void DisplayOccMap::EraseMapRange(const QPointF &pose, double range) {
}
update();
}
OccupancyMap DisplayOccMap::GetOccupancyMap() {
OccupancyMap map = map_data_;
for (int i = 0; i < map_image_.width(); i++)
for (int i = 0; i < map_image_.width(); i++) {
for (int j = 0; j < map_image_.height(); j++) {
QRgb pixelValue = map_image_.pixel(i, j); // (x, y) 是指定位置的坐标
if (pixelValue == QColor(Qt::black).rgb()) {
map(j, i) = 100;
} else if (pixelValue == QColor(Qt::gray).rgb()) {
map(j, i) = -1;
QRgb pixelValue = map_image_.pixel(i, j); // 获取指定位置的像素值
QColor color(pixelValue); // 从像素值创建 QColor 对象
// 提取Alpha通道值，范围是 0-255
int alpha = color.alpha();
// 如果颜色是黑色且 alpha > 0，表示占据栅格
if (color == QColor(Qt::black) && alpha > 0) {
// 将 alpha 映射回 0-100 的栅格值 (之前是将 0-100 映射到 0-255 的透明度)
int map_value = static_cast<int>(alpha / 2.55); // 反向还原栅格值
map(j, i) = map_value; // 还原栅格数据
}
// 如果颜色是白色，表示自由区域或未知区域
else if (color == QColor(Qt::white)) {
// 原始数据可能是自由区域或未知区域
if (alpha == 255) {
map(j, i) = 0; // 自由区域
} else {
map(j, i) = -1; // 未知区域
}
} else {
map(j, i) = 0;
map(j, i) = -1; // 未知区域，或其他情况
}
}
}
return map;
}
void DisplayOccMap::StartDrawLine(const QPointF &pose) {
line_start_pose_ = pose;
}

15
mainwindow/src/display/manager/view_manager.cpp
View File

@ -34,9 +34,6 @@ ViewManager::ViewManager(QWidget *parent) : QGraphicsView(parent) {
"QToolButton {"
" border: none;"
" background-color: transparent;"
"}"
"QToolButton:hover {"
" cursor: pointer;"
"}");
bottom_bar_layout->addWidget(set_big_btn_);
QToolButton *set_small_btn_ = new QToolButton();
@ -48,9 +45,6 @@ ViewManager::ViewManager(QWidget *parent) : QGraphicsView(parent) {
"QToolButton {"
" border: none;"
" background-color: transparent;"
"}"
"QToolButton:hover {"
" cursor: pointer;"
"}");
bottom_bar_layout->addWidget(set_small_btn_);
focus_robot_btn_ = new QToolButton();
@ -61,9 +55,6 @@ ViewManager::ViewManager(QWidget *parent) : QGraphicsView(parent) {
"QToolButton {"
" border: none;"
" background-color: transparent;"
"}"
"QToolButton:hover {"
" cursor: pointer;"
"}");
focus_robot_btn_->setIconSize(QSize(25, 25));
bottom_bar_layout->addWidget(focus_robot_btn_);
@ -80,9 +71,6 @@ ViewManager::ViewManager(QWidget *parent) : QGraphicsView(parent) {
"QToolButton {"
" border: none;"
" background-color: transparent;"
"}"
"QToolButton:hover {"
" cursor: pointer;"
"}");
display_config_layout->addWidget(display_list_show_btn);
@ -96,9 +84,6 @@ ViewManager::ViewManager(QWidget *parent) : QGraphicsView(parent) {
"QToolButton {"
" border: none;"
" background-color: transparent;"
"}"
"QToolButton:hover {"
" cursor: pointer;"
"}");
display_btn_list_layout->addWidget(display_laser_btn_);