@xiaoqiang @weijiz
我已经检查了摄像头，二维码识别，odom，速度，全都工作良好。运行xiangqiang_local.launch文件,试图打印dock_position,没有任何输出，运行保存dock_position也没有任何数据。我想问一下，应该检查如下哪个文件？
谢谢
我现在觉得getDockPosition.cpp文件根本就没有subscribe任何topic。
您能提供一下测试好的正确代码吗？或者告诉我应该如何能修改现有代码，能保存充电桩位置。或者是您告诉我应该如何测验？来回来去还久了，能麻烦您提供一个有效的解决方法吗？谢谢
@xiaoqiang @weijiz
能识别ar_maker,id 为1.
有odomtopic，目前都为0.
cmd_vel，都为0.
rqt_tree如图
@xiaoqiang
能识别ar_maker,id 为1.
有odomtopic，目前都为0.
cmd_vel，都为0.
rqt_tree如图,
@xiaoqiang @小助理 @weijiz
但为什么存不到dock_position呢？解决方法是什么呢？我的tf是这样的
@weijiz 我的odom到base_link,是机器人发布的。
我的机器人的tf结构没办法改，我现在想在充电部分把base_link和base_footprint之间的tf反过来。如果按照我给您发的tf树，我应该如何修改代码？
@小助理 我上面提供的代码是对二维码的吗？还是针对ir自动充电的啊？我在怀疑这个不是针对二维码保存位置信息的文件。你能提供给我一个对二维码自动充电的bw_quto_dock包的下载链接吗？我的ROS是melodic版本
@小助理 我检查了getDockPosition.cpp文件，如下
#include “bw_auto_dock/getDockPosition.h”
namespace bw_auto_dock
{
CaculateDockPosition::CaculateDockPosition(double grid_length, std::string frame_id, std::string dock_station_filename,
DockController* dock_controler, StatusPublisher* bw_status)
{
mdock_controler_ = dock_controler;
mbw_status_ = bw_status;
mdock_station_filename_ = dock_station_filename;
mlocal_grid_ = new LocalGrid(0.1, grid_length / 2.0 / 0.1, frame_id);
station_distance_ = 0.8;
}
bool CaculateDockPosition::getDockPosition(float (&station_pose1)[2], float (&station_pose2)[2])
{
//获取充电桩参考点
boost::mutex::scoped_lock lock(mMutex_);
std::string dbfile_path = mdock_station_filename_;//std::string(std::getenv(“HOME”)) + std::string(“/slamdb”) + std::string(“/”) + mdock_station_filename_;
if (!boost::filesystem::exists(dbfile_path.c_str()))
{
return false;
}
else
{
std::ifstream dock_station_file(dbfile_path);
if (dock_station_file.is_open())
{
dock_station_file >> station_pose1[0];
dock_station_file >> station_pose1[1];
dock_station_file >> station_pose2[0];
dock_station_file >> station_pose2[1];
}
else
{
return false;
}
}
return true;
}
void CaculateDockPosition::run()
{
ros::NodeHandle nodeHandler;
ros::Subscriber sub1 =
nodeHandler.subscribe(“/bw_auto_dock/dockposition_save”, 1, &CaculateDockPosition::updateMapsaveFlag, this);
ros::Rate r(10);
int num = 0;
bool dock_postion_update_ = false;
while (ros::ok())
{
num++;
if (num % 100 == 0)
{
mlocal_grid_->runPub();
}
if (num % 100 == 0 && dock_postion_update_)
{
// 10秒自动保存一次
dock_postion_update_ = false;
this->saveDockPositon();
float station_pose1[2], station_pose2[2];
}
DOCK_POSITION sensor_value = mbw_status_->get_dock_position();
float ir_pose[3];
if (mdock_controler_->getIRPose(ir_pose) && sensor_value != DOCK_POSITION::not_found)
{
if (!mlocal_grid_->get_ready_flag())
{
float center[3];
center[0] = ir_pose[0];
center[1] = ir_pose[1];
center[2] = ir_pose[2];
mlocal_grid_->setOrigin(center);
}
mlocal_grid_->update_clear(ir_pose);
mlocal_grid_->update_sensor(sensor_value, ir_pose);
if (mbw_status_->sensor_status.power > 9.0)
{
if(mbw_status_->get_charge_status() == CHARGE_STATUS::freed)
{
//触发充电桩
if (mlocal_grid_->set_dock_position(ir_pose))
dock_postion_update_ = true;
// ROS_INFO("dock_realposition_set %d %f %f %f",num,ir_pose[0],ir_pose[1],ir_pose[2]);
}
}
}
ros::spinOnce();
r.sleep();
}
}
void CaculateDockPosition::saveDockPositon()
{
boost::mutex::scoped_lock lock(mMutex_);
boost::filesystem::path p(mdock_station_filename_);
boost::filesystem::path dir = p.parent_path();
std::string dbfile_rootpath = p.parent_path().string();
float dock_pose[3], station_pose1[2], station_pose2[2];
if (mlocal_grid_->get_dock_position(dock_pose))
{
ROS_INFO(“dock_position_saved %f %f %f”, dock_pose[0], dock_pose[1], dock_pose[2]);
//(station_distance_,station_distance_) (station_distance_,-station_distance_)
station_pose1[0] =
dock_pose[0] + station_distance_ * cos(dock_pose[2]) - 0.5 * station_distance_ * sin(dock_pose[2]);
station_pose1[1] =
dock_pose[1] + station_distance_ * sin(dock_pose[2]) + 0.5 * station_distance_ * cos(dock_pose[2]);
station_pose2[0] =
dock_pose[0] + station_distance_ * cos(dock_pose[2]) + 0.5 * station_distance_ * sin(dock_pose[2]);
station_pose2[1] =
dock_pose[1] + station_distance_ * sin(dock_pose[2]) - 0.5 * station_distance_ * cos(dock_pose[2]);
if (!boost::filesystem::exists(dbfile_rootpath.c_str()))
{
boost::filesystem::create_directories(dbfile_rootpath.c_str());
}
std::ofstream dock_station_file;
dock_station_file.open(mdock_station_filename_);
dock_station_file << station_pose1[0] << " " << station_pose1[1] << std::endl
<< station_pose2[0] << " " << station_pose2[1] << std::endl;
dock_station_file << dock_pose[0] << " " << dock_pose[1] << " " << dock_pose[2] << std::endl;
dock_station_file.close();
}
}
void CaculateDockPosition::updateMapsaveFlag(const std_msgs::Bool& currentFlag)
{
if (currentFlag.data)
{
float ir_pose[3];
if (mdock_controler_->getIRPose(ir_pose))
{
mlocal_grid_->set_dock_position(ir_pose);
}
this->saveDockPositon();
}
}
} // namespace bw_auto_dock
@小助理
我是在git上下载的bw_auto_dock包，关于node没有作任何处理，只是修改了launch文件的dock保存地址和usb的port名称。我已经确认了保存文件具有可读写权限，tf从odom–>usb_cam–>ar_marker_1是连接的。但是仍然不能保存位置信息。
@小助理
3. 检查保存代码的逻辑：确保你的保存代码逻辑正确，包括文件的打开、写入和关闭操作。可以使用Python的文件操作函数（如open、write和close）来实现保存功能。
代码是哪个文件？
