diff --git a/example/casic/slam_wrapper.cpp b/example/casic/slam_wrapper.cpp index ca82c2a..0ff7a0e 100644 --- a/example/casic/slam_wrapper.cpp +++ b/example/casic/slam_wrapper.cpp @@ -3,136 +3,137 @@ // #include "slam_wrapper.hpp" +#include slam_wrapper::slam_wrapper(const char *networkInterface) { ChannelFactory::Instance()->Init(0, networkInterface); // pub - // pubQtCommand->InitChannel(); - // pubQtNode->InitChannel(); - // pubQtEdge->InitChannel(); + pubQtCommand->InitChannel(); + pubQtNode->InitChannel(); + pubQtEdge->InitChannel(); // sub - // subQtNotice->InitChannel(std::bind(&slam_wrapper::qt_notice_handler, this, std::placeholders::_1), 10); - // subOdometer->InitChannel(std::bind(&slam_wrapper::odometer_handler, this, std::placeholders::_1), 1); + subQtNotice->InitChannel(std::bind(&slam_wrapper::qt_notice_handler, this, std::placeholders::_1), 10); + subOdometer->InitChannel(std::bind(&slam_wrapper::odometer_handler, this, std::placeholders::_1), 1); } void slam_wrapper::qt_notice_handler(const void *message) { - // int index_, begin_, end_, feedback_, arrive_; - // const std_msgs::msg::dds_::String_ *seq = (const std_msgs::msg::dds_::String_ *) message; - // string str_, notice_; - // - // begin_ = seq->data().find("index:", 0); // Instruction unique identifier - // end_ = seq->data().find(";", begin_); - // str_ = seq->data().substr(begin_ + 6, end_ - begin_ - 6); - // index_ = atoi(str_.c_str()); - // - // begin_ = seq->data().find("notice:", 0); // Prompt message - // end_ = seq->data().find(";", begin_); - // notice_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); - // - // if (index_ <= 10000) { - // // Command execution feedback - // begin_ = seq->data().find("feedback:", 0); - // end_ = seq->data().find(";", begin_); - // str_ = seq->data().substr(begin_ + 9, end_ - begin_ - 9); - // feedback_ = atoi(str_.c_str()); - // } else if (index_ == 10001) { - // // Navigation feedback - // begin_ = seq->data().find("arrive:", 0); - // end_ = seq->data().find(";", begin_); - // str_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); - // arrive_ = atoi(str_.c_str()); - // } + int index_, begin_, end_, feedback_, arrive_; + const std_msgs::msg::dds_::String_ *seq = (const std_msgs::msg::dds_::String_ *) message; + std::string str_, notice_; + + begin_ = seq->data().find("index:", 0); // Instruction unique identifier + end_ = seq->data().find(";", begin_); + str_ = seq->data().substr(begin_ + 6, end_ - begin_ - 6); + index_ = atoi(str_.c_str()); + + begin_ = seq->data().find("notice:", 0); // Prompt message + end_ = seq->data().find(";", begin_); + notice_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); + + if (index_ <= 10000) { + // Command execution feedback + begin_ = seq->data().find("feedback:", 0); + end_ = seq->data().find(";", begin_); + str_ = seq->data().substr(begin_ + 9, end_ - begin_ - 9); + feedback_ = atoi(str_.c_str()); + } else if (index_ == 10001) { + // Navigation feedback + begin_ = seq->data().find("arrive:", 0); + end_ = seq->data().find(";", begin_); + str_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); + arrive_ = atoi(str_.c_str()); + } } void slam_wrapper::odometer_handler(const void *message) { - // currentOdom = (const nav_msgs::msg::dds_::Odometry_ *) message; + currentOdom = (const nav_msgs::msg::dds_::Odometry_ *) message; } void slam_wrapper::start_mapping() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.command_() = 3; // 3 is to start mapping command - // send_msg.attribute_() = 1; - // //When this value is 1, the XT16 LiDAR node is enabled, and when it is 2, the MID360 LiDAR node is enabled. Please confirm the value based on the actual LiDAR being - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.command_() = 3; // 3 is to start mapping command + send_msg.attribute_() = 1; + //When this value is 1, the XT16 LiDAR node is enabled, and when it is 2, the MID360 LiDAR node is enabled. Please confirm the value based on the actual LiDAR being + pubQtCommand->Write(send_msg); } void slam_wrapper::end_mapping() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.command_() = 4; // 4 is the end mapping command - // send_msg.attribute_() = 0; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.floor_index_().push_back(0); // Floor number，just give a fixed value of 0. - // send_msg.pcdmap_index_().push_back(0); // PCD Map number，just give a fixed value of 0. - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.command_() = 4; // 4 is the end mapping command + send_msg.attribute_() = 0; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.floor_index_().push_back(0); // Floor number，just give a fixed value of 0. + send_msg.pcdmap_index_().push_back(0); // PCD Map number，just give a fixed value of 0. + pubQtCommand->Write(send_msg); } void slam_wrapper::start_relocation() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.command_() = 6; // 6 is to start relocation command - // send_msg.attribute_() = 1; - // //When this value is 1, the XT16 LiDAR node is enabled, and when it is 2, the MID360 LiDAR node is enabled. Please confirm the value based on the actual LiDAR being - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.command_() = 6; // 6 is to start relocation command + send_msg.attribute_() = 1; + //When this value is 1, the XT16 LiDAR node is enabled, and when it is 2, the MID360 LiDAR node is enabled. Please confirm the value based on the actual LiDAR being + pubQtCommand->Write(send_msg); } void slam_wrapper::init_pose() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.command_() = 7; // 7 is the pose initialization instruction - // send_msg.quaternion_x_() = 0; // Quaternion - // send_msg.quaternion_y_() = 0; - // send_msg.quaternion_z_() = 0; - // send_msg.quaternion_w_() = 1; - // send_msg.translation_x_() = 0; // Translation - // send_msg.translation_y_() = 0; - // send_msg.translation_z_() = 0; - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.command_() = 7; // 7 is the pose initialization instruction + send_msg.quaternion_x_() = 0; // Quaternion + send_msg.quaternion_y_() = 0; + send_msg.quaternion_z_() = 0; + send_msg.quaternion_w_() = 1; + send_msg.translation_x_() = 0; // Translation + send_msg.translation_y_() = 0; + send_msg.translation_z_() = 0; + pubQtCommand->Write(send_msg); } void slam_wrapper::start_navigation() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.command_() = 8; // 8 is the command to start navigation - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.command_() = 8; // 8 is the command to start navigation + pubQtCommand->Write(send_msg); } void slam_wrapper::default_navigation() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.command_() = 10; // 10 Multi nodes loop navigation(default) command - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.command_() = 10; // 10 Multi nodes loop navigation(default) command + pubQtCommand->Write(send_msg); } void slam_wrapper::add_node_and_edge() { - // node_attribute nodeTemp{}; - // float siny_cosp, cosy_cosp; - // - // siny_cosp = 2 * (currentOdom->pose().pose().orientation().w() * currentOdom->pose().pose().orientation().z() + - // currentOdom->pose().pose().orientation().x() * currentOdom->pose().pose().orientation().y()); - // cosy_cosp = 1 - 2 * (currentOdom->pose().pose().orientation().y() * currentOdom->pose().pose().orientation().y() + - // currentOdom->pose().pose().orientation().z() * currentOdom->pose().pose().orientation().z()); - // node_name++; - // - // nodeTemp.nodeX = currentOdom->pose().pose().position().x(); - // nodeTemp.nodeY = currentOdom->pose().pose().position().y(); - // nodeTemp.nodeZ = currentOdom->pose().pose().position().z(); - // nodeTemp.nodeYaw = std::atan2(siny_cosp, cosy_cosp); - // nodeTemp.nodeName = node_name; - // - // node_attributes.push_back(nodeTemp); - // - // if (node_name >= 2) { - // add_edge(node_name - 1, node_name - 1, node_name); // Sequential connection nodes - // } + node_attribute nodeTemp{}; + float siny_cosp, cosy_cosp; + + siny_cosp = 2 * (currentOdom->pose().pose().orientation().w() * currentOdom->pose().pose().orientation().z() + + currentOdom->pose().pose().orientation().x() * currentOdom->pose().pose().orientation().y()); + cosy_cosp = 1 - 2 * (currentOdom->pose().pose().orientation().y() * currentOdom->pose().pose().orientation().y() + + currentOdom->pose().pose().orientation().z() * currentOdom->pose().pose().orientation().z()); + node_name++; + + nodeTemp.nodeX = currentOdom->pose().pose().position().x(); + nodeTemp.nodeY = currentOdom->pose().pose().position().y(); + nodeTemp.nodeZ = currentOdom->pose().pose().position().z(); + nodeTemp.nodeYaw = std::atan2(siny_cosp, cosy_cosp); + nodeTemp.nodeName = node_name; + + node_attributes.push_back(nodeTemp); + + if (node_name >= 2) { + add_edge(node_name - 1, node_name - 1, node_name); // Sequential connection nodes + } } void slam_wrapper::add_edge(u_int16_t edge_name, u_int16_t start_node, u_int16_t end_node) { @@ -145,90 +146,90 @@ } void slam_wrapper::save_node_and_edge() { - // unitree_interfaces::msg::dds_::QtNode_ nodeMsg; - // unitree_interfaces::msg::dds_::QtEdge_ edgeMsg; - // if (edge_attributes.empty()) { - // return; - // } - // - // index++; - // nodeMsg.seq_().data() = "index:" + to_string(index) + ";"; - // for (auto &[nodeName, nodeX, nodeY, nodeZ, nodeYaw]: node_attributes) { - // nodeMsg.node_().node_name_().push_back(nodeName); // Node name - // nodeMsg.node_().node_position_x_().push_back(nodeX); // Point X coordinate information - // nodeMsg.node_().node_position_y_().push_back(nodeY); // Point Y coordinate information - // nodeMsg.node_().node_position_z_().push_back(nodeZ); // Point Z coordinate information - // nodeMsg.node_().node_yaw_().push_back(nodeYaw); // Point Yaw Angle Information - // nodeMsg.node_().node_attribute_().push_back(0); - // // Not open attribute, please assign a value of 0, note: cannot be empty!!! Cannot be empty!!! Cannot be empty!!! - // nodeMsg.node_().undefined_().push_back(0); - // nodeMsg.node_().node_state_2_().push_back(0); - // nodeMsg.node_().node_state_3_().push_back(0); - // } - // pubQtNode->Write(nodeMsg); - // - // index++; - // edgeMsg.seq_().data() = "index:" + to_string(index) + ";"; - // for (auto &[edgeName, edgeStart, edgeEnd]: edge_attributes) { - // edgeMsg.edge_().edge_name_().push_back(edgeName); // Edge name - // edgeMsg.edge_().start_node_name_().push_back(edgeStart); // Start node name - // edgeMsg.edge_().end_node_name_().push_back(edgeEnd); // End node name - // edgeMsg.edge_().edge_length_().push_back(0); - // edgeMsg.edge_().dog_speed_().push_back(1); // The speed of the dog walking this edge (0-1) - // edgeMsg.edge_().edge_state_2_().push_back(0); - // // 0：Stop 1：Avoid 3：Replan when encountering obstacles. Suggest assigning a value of 0. - // - // edgeMsg.edge_().dog_stats_().push_back(0); - // // Not open attribute, please assign a value of 0, note: cannot be empty!!! Cannot be empty!!! Cannot be empty!!! - // edgeMsg.edge_().dog_back_stats_().push_back(0); - // edgeMsg.edge_().edge_state_().push_back(0); - // edgeMsg.edge_().edge_state_1_().push_back(0); - // edgeMsg.edge_().edge_state_3_().push_back(0); - // edgeMsg.edge_().edge_state_4_().push_back(0); - // } - // pubQtEdge->Write(edgeMsg); + unitree_interfaces::msg::dds_::QtNode_ nodeMsg; + unitree_interfaces::msg::dds_::QtEdge_ edgeMsg; + if (edge_attributes.empty()) { + return; + } + + index++; + nodeMsg.seq_().data() = "index:" + std::to_string(index) + ";"; + for (auto &[nodeName, nodeX, nodeY, nodeZ, nodeYaw]: node_attributes) { + nodeMsg.node_().node_name_().push_back(nodeName); // Node name + nodeMsg.node_().node_position_x_().push_back(nodeX); // Point X coordinate information + nodeMsg.node_().node_position_y_().push_back(nodeY); // Point Y coordinate information + nodeMsg.node_().node_position_z_().push_back(nodeZ); // Point Z coordinate information + nodeMsg.node_().node_yaw_().push_back(nodeYaw); // Point Yaw Angle Information + nodeMsg.node_().node_attribute_().push_back(0); + // Not open attribute, please assign a value of 0, note: cannot be empty!!! Cannot be empty!!! Cannot be empty!!! + nodeMsg.node_().undefined_().push_back(0); + nodeMsg.node_().node_state_2_().push_back(0); + nodeMsg.node_().node_state_3_().push_back(0); + } + pubQtNode->Write(nodeMsg); + + index++; + edgeMsg.seq_().data() = "index:" + std::to_string(index) + ";"; + for (auto &[edgeName, edgeStart, edgeEnd]: edge_attributes) { + edgeMsg.edge_().edge_name_().push_back(edgeName); // Edge name + edgeMsg.edge_().start_node_name_().push_back(edgeStart); // Start node name + edgeMsg.edge_().end_node_name_().push_back(edgeEnd); // End node name + edgeMsg.edge_().edge_length_().push_back(0); + edgeMsg.edge_().dog_speed_().push_back(1); // The speed of the dog walking this edge (0-1) + edgeMsg.edge_().edge_state_2_().push_back(0); + // 0：Stop 1：Avoid 3：Replan when encountering obstacles. Suggest assigning a value of 0. + + edgeMsg.edge_().dog_stats_().push_back(0); + // Not open attribute, please assign a value of 0, note: cannot be empty!!! Cannot be empty!!! Cannot be empty!!! + edgeMsg.edge_().dog_back_stats_().push_back(0); + edgeMsg.edge_().edge_state_().push_back(0); + edgeMsg.edge_().edge_state_1_().push_back(0); + edgeMsg.edge_().edge_state_3_().push_back(0); + edgeMsg.edge_().edge_state_4_().push_back(0); + } + pubQtEdge->Write(edgeMsg); } void slam_wrapper::close_all_node() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.command_() = 99; // 99 Close all nodes command - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.command_() = 99; // 99 Close all nodes command + pubQtCommand->Write(send_msg); } void slam_wrapper::delete_all_node() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.command_() = 1; // 1 is delete instruction - // send_msg.attribute_() = 1; // 1 is select delete node - // send_msg.node_edge_name_().push_back(999); - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.command_() = 1; // 1 is delete instruction + send_msg.attribute_() = 1; // 1 is select delete node + send_msg.node_edge_name_().push_back(999); + pubQtCommand->Write(send_msg); } void slam_wrapper::delete_all_edge() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.command_() = 1; // 1 is delete instruction - // send_msg.attribute_() = 2; // 2 is select delete edge - // send_msg.node_edge_name_().push_back(999); - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.command_() = 1; // 1 is delete instruction + send_msg.attribute_() = 2; // 2 is select delete edge + send_msg.node_edge_name_().push_back(999); + pubQtCommand->Write(send_msg); } void slam_wrapper::pause_navigation() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.command_() = 13; // 13 is the pause navigation command - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.command_() = 13; // 13 is the pause navigation command + pubQtCommand->Write(send_msg); } void slam_wrapper::recover_navigation() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.command_() = 14; // 14 to recover navigation commands - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.command_() = 14; // 14 to recover navigation commands + pubQtCommand->Write(send_msg); } diff --git a/example/casic/slam_wrapper.cpp b/example/casic/slam_wrapper.cpp index ca82c2a..0ff7a0e 100644 --- a/example/casic/slam_wrapper.cpp +++ b/example/casic/slam_wrapper.cpp @@ -3,136 +3,137 @@ // #include "slam_wrapper.hpp" +#include slam_wrapper::slam_wrapper(const char *networkInterface) { ChannelFactory::Instance()->Init(0, networkInterface); // pub - // pubQtCommand->InitChannel(); - // pubQtNode->InitChannel(); - // pubQtEdge->InitChannel(); + pubQtCommand->InitChannel(); + pubQtNode->InitChannel(); + pubQtEdge->InitChannel(); // sub - // subQtNotice->InitChannel(std::bind(&slam_wrapper::qt_notice_handler, this, std::placeholders::_1), 10); - // subOdometer->InitChannel(std::bind(&slam_wrapper::odometer_handler, this, std::placeholders::_1), 1); + subQtNotice->InitChannel(std::bind(&slam_wrapper::qt_notice_handler, this, std::placeholders::_1), 10); + subOdometer->InitChannel(std::bind(&slam_wrapper::odometer_handler, this, std::placeholders::_1), 1); } void slam_wrapper::qt_notice_handler(const void *message) { - // int index_, begin_, end_, feedback_, arrive_; - // const std_msgs::msg::dds_::String_ *seq = (const std_msgs::msg::dds_::String_ *) message; - // string str_, notice_; - // - // begin_ = seq->data().find("index:", 0); // Instruction unique identifier - // end_ = seq->data().find(";", begin_); - // str_ = seq->data().substr(begin_ + 6, end_ - begin_ - 6); - // index_ = atoi(str_.c_str()); - // - // begin_ = seq->data().find("notice:", 0); // Prompt message - // end_ = seq->data().find(";", begin_); - // notice_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); - // - // if (index_ <= 10000) { - // // Command execution feedback - // begin_ = seq->data().find("feedback:", 0); - // end_ = seq->data().find(";", begin_); - // str_ = seq->data().substr(begin_ + 9, end_ - begin_ - 9); - // feedback_ = atoi(str_.c_str()); - // } else if (index_ == 10001) { - // // Navigation feedback - // begin_ = seq->data().find("arrive:", 0); - // end_ = seq->data().find(";", begin_); - // str_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); - // arrive_ = atoi(str_.c_str()); - // } + int index_, begin_, end_, feedback_, arrive_; + const std_msgs::msg::dds_::String_ *seq = (const std_msgs::msg::dds_::String_ *) message; + std::string str_, notice_; + + begin_ = seq->data().find("index:", 0); // Instruction unique identifier + end_ = seq->data().find(";", begin_); + str_ = seq->data().substr(begin_ + 6, end_ - begin_ - 6); + index_ = atoi(str_.c_str()); + + begin_ = seq->data().find("notice:", 0); // Prompt message + end_ = seq->data().find(";", begin_); + notice_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); + + if (index_ <= 10000) { + // Command execution feedback + begin_ = seq->data().find("feedback:", 0); + end_ = seq->data().find(";", begin_); + str_ = seq->data().substr(begin_ + 9, end_ - begin_ - 9); + feedback_ = atoi(str_.c_str()); + } else if (index_ == 10001) { + // Navigation feedback + begin_ = seq->data().find("arrive:", 0); + end_ = seq->data().find(";", begin_); + str_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); + arrive_ = atoi(str_.c_str()); + } } void slam_wrapper::odometer_handler(const void *message) { - // currentOdom = (const nav_msgs::msg::dds_::Odometry_ *) message; + currentOdom = (const nav_msgs::msg::dds_::Odometry_ *) message; } void slam_wrapper::start_mapping() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.command_() = 3; // 3 is to start mapping command - // send_msg.attribute_() = 1; - // //When this value is 1, the XT16 LiDAR node is enabled, and when it is 2, the MID360 LiDAR node is enabled. Please confirm the value based on the actual LiDAR being - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.command_() = 3; // 3 is to start mapping command + send_msg.attribute_() = 1; + //When this value is 1, the XT16 LiDAR node is enabled, and when it is 2, the MID360 LiDAR node is enabled. Please confirm the value based on the actual LiDAR being + pubQtCommand->Write(send_msg); } void slam_wrapper::end_mapping() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.command_() = 4; // 4 is the end mapping command - // send_msg.attribute_() = 0; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.floor_index_().push_back(0); // Floor number，just give a fixed value of 0. - // send_msg.pcdmap_index_().push_back(0); // PCD Map number，just give a fixed value of 0. - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.command_() = 4; // 4 is the end mapping command + send_msg.attribute_() = 0; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.floor_index_().push_back(0); // Floor number，just give a fixed value of 0. + send_msg.pcdmap_index_().push_back(0); // PCD Map number，just give a fixed value of 0. + pubQtCommand->Write(send_msg); } void slam_wrapper::start_relocation() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.command_() = 6; // 6 is to start relocation command - // send_msg.attribute_() = 1; - // //When this value is 1, the XT16 LiDAR node is enabled, and when it is 2, the MID360 LiDAR node is enabled. Please confirm the value based on the actual LiDAR being - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.command_() = 6; // 6 is to start relocation command + send_msg.attribute_() = 1; + //When this value is 1, the XT16 LiDAR node is enabled, and when it is 2, the MID360 LiDAR node is enabled. Please confirm the value based on the actual LiDAR being + pubQtCommand->Write(send_msg); } void slam_wrapper::init_pose() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.command_() = 7; // 7 is the pose initialization instruction - // send_msg.quaternion_x_() = 0; // Quaternion - // send_msg.quaternion_y_() = 0; - // send_msg.quaternion_z_() = 0; - // send_msg.quaternion_w_() = 1; - // send_msg.translation_x_() = 0; // Translation - // send_msg.translation_y_() = 0; - // send_msg.translation_z_() = 0; - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.command_() = 7; // 7 is the pose initialization instruction + send_msg.quaternion_x_() = 0; // Quaternion + send_msg.quaternion_y_() = 0; + send_msg.quaternion_z_() = 0; + send_msg.quaternion_w_() = 1; + send_msg.translation_x_() = 0; // Translation + send_msg.translation_y_() = 0; + send_msg.translation_z_() = 0; + pubQtCommand->Write(send_msg); } void slam_wrapper::start_navigation() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.command_() = 8; // 8 is the command to start navigation - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.command_() = 8; // 8 is the command to start navigation + pubQtCommand->Write(send_msg); } void slam_wrapper::default_navigation() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.command_() = 10; // 10 Multi nodes loop navigation(default) command - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.command_() = 10; // 10 Multi nodes loop navigation(default) command + pubQtCommand->Write(send_msg); } void slam_wrapper::add_node_and_edge() { - // node_attribute nodeTemp{}; - // float siny_cosp, cosy_cosp; - // - // siny_cosp = 2 * (currentOdom->pose().pose().orientation().w() * currentOdom->pose().pose().orientation().z() + - // currentOdom->pose().pose().orientation().x() * currentOdom->pose().pose().orientation().y()); - // cosy_cosp = 1 - 2 * (currentOdom->pose().pose().orientation().y() * currentOdom->pose().pose().orientation().y() + - // currentOdom->pose().pose().orientation().z() * currentOdom->pose().pose().orientation().z()); - // node_name++; - // - // nodeTemp.nodeX = currentOdom->pose().pose().position().x(); - // nodeTemp.nodeY = currentOdom->pose().pose().position().y(); - // nodeTemp.nodeZ = currentOdom->pose().pose().position().z(); - // nodeTemp.nodeYaw = std::atan2(siny_cosp, cosy_cosp); - // nodeTemp.nodeName = node_name; - // - // node_attributes.push_back(nodeTemp); - // - // if (node_name >= 2) { - // add_edge(node_name - 1, node_name - 1, node_name); // Sequential connection nodes - // } + node_attribute nodeTemp{}; + float siny_cosp, cosy_cosp; + + siny_cosp = 2 * (currentOdom->pose().pose().orientation().w() * currentOdom->pose().pose().orientation().z() + + currentOdom->pose().pose().orientation().x() * currentOdom->pose().pose().orientation().y()); + cosy_cosp = 1 - 2 * (currentOdom->pose().pose().orientation().y() * currentOdom->pose().pose().orientation().y() + + currentOdom->pose().pose().orientation().z() * currentOdom->pose().pose().orientation().z()); + node_name++; + + nodeTemp.nodeX = currentOdom->pose().pose().position().x(); + nodeTemp.nodeY = currentOdom->pose().pose().position().y(); + nodeTemp.nodeZ = currentOdom->pose().pose().position().z(); + nodeTemp.nodeYaw = std::atan2(siny_cosp, cosy_cosp); + nodeTemp.nodeName = node_name; + + node_attributes.push_back(nodeTemp); + + if (node_name >= 2) { + add_edge(node_name - 1, node_name - 1, node_name); // Sequential connection nodes + } } void slam_wrapper::add_edge(u_int16_t edge_name, u_int16_t start_node, u_int16_t end_node) { @@ -145,90 +146,90 @@ } void slam_wrapper::save_node_and_edge() { - // unitree_interfaces::msg::dds_::QtNode_ nodeMsg; - // unitree_interfaces::msg::dds_::QtEdge_ edgeMsg; - // if (edge_attributes.empty()) { - // return; - // } - // - // index++; - // nodeMsg.seq_().data() = "index:" + to_string(index) + ";"; - // for (auto &[nodeName, nodeX, nodeY, nodeZ, nodeYaw]: node_attributes) { - // nodeMsg.node_().node_name_().push_back(nodeName); // Node name - // nodeMsg.node_().node_position_x_().push_back(nodeX); // Point X coordinate information - // nodeMsg.node_().node_position_y_().push_back(nodeY); // Point Y coordinate information - // nodeMsg.node_().node_position_z_().push_back(nodeZ); // Point Z coordinate information - // nodeMsg.node_().node_yaw_().push_back(nodeYaw); // Point Yaw Angle Information - // nodeMsg.node_().node_attribute_().push_back(0); - // // Not open attribute, please assign a value of 0, note: cannot be empty!!! Cannot be empty!!! Cannot be empty!!! - // nodeMsg.node_().undefined_().push_back(0); - // nodeMsg.node_().node_state_2_().push_back(0); - // nodeMsg.node_().node_state_3_().push_back(0); - // } - // pubQtNode->Write(nodeMsg); - // - // index++; - // edgeMsg.seq_().data() = "index:" + to_string(index) + ";"; - // for (auto &[edgeName, edgeStart, edgeEnd]: edge_attributes) { - // edgeMsg.edge_().edge_name_().push_back(edgeName); // Edge name - // edgeMsg.edge_().start_node_name_().push_back(edgeStart); // Start node name - // edgeMsg.edge_().end_node_name_().push_back(edgeEnd); // End node name - // edgeMsg.edge_().edge_length_().push_back(0); - // edgeMsg.edge_().dog_speed_().push_back(1); // The speed of the dog walking this edge (0-1) - // edgeMsg.edge_().edge_state_2_().push_back(0); - // // 0：Stop 1：Avoid 3：Replan when encountering obstacles. Suggest assigning a value of 0. - // - // edgeMsg.edge_().dog_stats_().push_back(0); - // // Not open attribute, please assign a value of 0, note: cannot be empty!!! Cannot be empty!!! Cannot be empty!!! - // edgeMsg.edge_().dog_back_stats_().push_back(0); - // edgeMsg.edge_().edge_state_().push_back(0); - // edgeMsg.edge_().edge_state_1_().push_back(0); - // edgeMsg.edge_().edge_state_3_().push_back(0); - // edgeMsg.edge_().edge_state_4_().push_back(0); - // } - // pubQtEdge->Write(edgeMsg); + unitree_interfaces::msg::dds_::QtNode_ nodeMsg; + unitree_interfaces::msg::dds_::QtEdge_ edgeMsg; + if (edge_attributes.empty()) { + return; + } + + index++; + nodeMsg.seq_().data() = "index:" + std::to_string(index) + ";"; + for (auto &[nodeName, nodeX, nodeY, nodeZ, nodeYaw]: node_attributes) { + nodeMsg.node_().node_name_().push_back(nodeName); // Node name + nodeMsg.node_().node_position_x_().push_back(nodeX); // Point X coordinate information + nodeMsg.node_().node_position_y_().push_back(nodeY); // Point Y coordinate information + nodeMsg.node_().node_position_z_().push_back(nodeZ); // Point Z coordinate information + nodeMsg.node_().node_yaw_().push_back(nodeYaw); // Point Yaw Angle Information + nodeMsg.node_().node_attribute_().push_back(0); + // Not open attribute, please assign a value of 0, note: cannot be empty!!! Cannot be empty!!! Cannot be empty!!! + nodeMsg.node_().undefined_().push_back(0); + nodeMsg.node_().node_state_2_().push_back(0); + nodeMsg.node_().node_state_3_().push_back(0); + } + pubQtNode->Write(nodeMsg); + + index++; + edgeMsg.seq_().data() = "index:" + std::to_string(index) + ";"; + for (auto &[edgeName, edgeStart, edgeEnd]: edge_attributes) { + edgeMsg.edge_().edge_name_().push_back(edgeName); // Edge name + edgeMsg.edge_().start_node_name_().push_back(edgeStart); // Start node name + edgeMsg.edge_().end_node_name_().push_back(edgeEnd); // End node name + edgeMsg.edge_().edge_length_().push_back(0); + edgeMsg.edge_().dog_speed_().push_back(1); // The speed of the dog walking this edge (0-1) + edgeMsg.edge_().edge_state_2_().push_back(0); + // 0：Stop 1：Avoid 3：Replan when encountering obstacles. Suggest assigning a value of 0. + + edgeMsg.edge_().dog_stats_().push_back(0); + // Not open attribute, please assign a value of 0, note: cannot be empty!!! Cannot be empty!!! Cannot be empty!!! + edgeMsg.edge_().dog_back_stats_().push_back(0); + edgeMsg.edge_().edge_state_().push_back(0); + edgeMsg.edge_().edge_state_1_().push_back(0); + edgeMsg.edge_().edge_state_3_().push_back(0); + edgeMsg.edge_().edge_state_4_().push_back(0); + } + pubQtEdge->Write(edgeMsg); } void slam_wrapper::close_all_node() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.command_() = 99; // 99 Close all nodes command - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.command_() = 99; // 99 Close all nodes command + pubQtCommand->Write(send_msg); } void slam_wrapper::delete_all_node() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.command_() = 1; // 1 is delete instruction - // send_msg.attribute_() = 1; // 1 is select delete node - // send_msg.node_edge_name_().push_back(999); - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.command_() = 1; // 1 is delete instruction + send_msg.attribute_() = 1; // 1 is select delete node + send_msg.node_edge_name_().push_back(999); + pubQtCommand->Write(send_msg); } void slam_wrapper::delete_all_edge() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.command_() = 1; // 1 is delete instruction - // send_msg.attribute_() = 2; // 2 is select delete edge - // send_msg.node_edge_name_().push_back(999); - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.command_() = 1; // 1 is delete instruction + send_msg.attribute_() = 2; // 2 is select delete edge + send_msg.node_edge_name_().push_back(999); + pubQtCommand->Write(send_msg); } void slam_wrapper::pause_navigation() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.command_() = 13; // 13 is the pause navigation command - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.command_() = 13; // 13 is the pause navigation command + pubQtCommand->Write(send_msg); } void slam_wrapper::recover_navigation() { - // index++; - // unitree_interfaces::msg::dds_::QtCommand_ send_msg; - // send_msg.seq_().data() = "index:" + to_string(index) + ";"; - // send_msg.command_() = 14; // 14 to recover navigation commands - // pubQtCommand->Write(send_msg); + index++; + unitree_interfaces::msg::dds_::QtCommand_ send_msg; + send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; + send_msg.command_() = 14; // 14 to recover navigation commands + pubQtCommand->Write(send_msg); } diff --git a/example/casic/slam_wrapper.hpp b/example/casic/slam_wrapper.hpp index bf02984..ae7d23e 100644 --- a/example/casic/slam_wrapper.hpp +++ b/example/casic/slam_wrapper.hpp @@ -11,6 +11,12 @@ #include #include +#include +#include +#include +#include +#include + #define COMMANDTOPIC "rt/qt_command" #define NOTICETOPIC "rt/qt_notice" #define ODOMTOPIC "rt/lio_sam_ros2/mapping/re_location_odometry" @@ -36,31 +42,31 @@ class slam_wrapper { private: int index = 0; - // const nav_msgs::msg::dds_::Odometry_ *currentOdom{}; + const nav_msgs::msg::dds_::Odometry_ *currentOdom{}; // pub - // ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr< - // unitree_interfaces::msg::dds_::QtCommand_>( - // new ChannelPublisher(COMMANDTOPIC) - // ); - // ChannelPublisherPtr pubQtNode = ChannelPublisherPtr< - // unitree_interfaces::msg::dds_::QtNode_>( - // new ChannelPublisher(ADDNODETOPIC) - // ); - // ChannelPublisherPtr pubQtEdge = ChannelPublisherPtr< - // unitree_interfaces::msg::dds_::QtEdge_>( - // new ChannelPublisher(ADDEDGETOPIC) - // ); + ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr< + unitree_interfaces::msg::dds_::QtCommand_>( + new ChannelPublisher(COMMANDTOPIC) + ); + ChannelPublisherPtr pubQtNode = ChannelPublisherPtr< + unitree_interfaces::msg::dds_::QtNode_>( + new ChannelPublisher(ADDNODETOPIC) + ); + ChannelPublisherPtr pubQtEdge = ChannelPublisherPtr< + unitree_interfaces::msg::dds_::QtEdge_>( + new ChannelPublisher(ADDEDGETOPIC) + ); // sub - // ChannelSubscriberPtr subQtNotice = ChannelSubscriberPtr< - // std_msgs::msg::dds_::String_>( - // new ChannelSubscriber(NOTICETOPIC) - // ); - // ChannelSubscriberPtr subOdometer = ChannelSubscriberPtr< - // nav_msgs::msg::dds_::Odometry_>( - // new ChannelSubscriber(ODOMTOPIC) - // ); + ChannelSubscriberPtr subQtNotice = ChannelSubscriberPtr< + std_msgs::msg::dds_::String_>( + new ChannelSubscriber(NOTICETOPIC) + ); + ChannelSubscriberPtr subOdometer = ChannelSubscriberPtr< + nav_msgs::msg::dds_::Odometry_>( + new ChannelSubscriber(ODOMTOPIC) + ); public: std::vector node_attributes;