重要提示
提问前必看的发帖注意事项—— 提问前必看!不符合要求的问题拒绝回答!!
社区使用指南—如何添加标签修改密码
yudonghou123 发布的最新帖子
-
ros2 message filter
系统UBUNTU22.04,ROS2 HUMBLE
各位大佬,我使用Message filter想做时间软同步,代码如下:#include <memory> #include <stdio.h> #include <stdlib.h> #include <string> #include <cstring> #include "rclcpp/rclcpp.hpp" #include <message_filters/subscriber.h> #include <message_filters/time_synchronizer.h> #include <message_filters/synchronizer.h> #include <message_filters/sync_policies/approximate_time.h> #include "dvl_msgs/msg/odom.hpp" #include "ahrs_msgs/msg/fn70_auxa.hpp" #include "plc_msgs/msg/sensor.hpp" using namespace message_filters; using std::placeholders::_1; using std::placeholders::_2; using std::placeholders::_3; class AUVSENSORS_MESSAGES : public rclcpp::Node { public: AUVSENSORS_MESSAGES() : Node("auvsensors_message") { odom_sub_.subscribe(this,"/dvl/odom"); imu_sub_.subscribe(this,"/ahrs/raw"); depth_sub_.subscribe(this,"/plc"); sync_ = std::make_shared<message_filters::TimeSynchronizer<dvl_msgs::msg::Odom, ahrs_msgs::msg::Fn70Auxa, plc_msgs::msg::Sensor>>(odom_sub_, imu_sub_, depth_sub_,10); sync_->registerCallback(std::bind(&AUVSENSORS_MESSAGES::disparityCb, this, _1, _2, _3)); } private: void disparityCb(const dvl_msgs::msg::Odom::ConstSharedPtr& dvl_msg, const ahrs_msgs::msg::Fn70Auxa::ConstSharedPtr& imu_msg,const plc_msgs::msg::Sensor::ConstSharedPtr& ph_msg) { const char *t_1 = std::to_string(dvl_msg->linear.x).c_str(); const char *t_2 = std::to_string(dvl_msg->linear.x).c_str(); const char *t_3 = std::to_string(dvl_msg->linear.x).c_str(); const char *t_4 = std::to_string(imu_msg->altitude).c_str(); const char *t_5 = std::to_string(ph_msg->depth).c_str(); RCLCPP_INFO(this->get_logger(), "消息:%s,%s,%s,%s,%s", t_1,t_2,t_3,t_4,t_5); } message_filters::Subscriber<dvl_msgs::msg::Odom> odom_sub_ ; message_filters::Subscriber<ahrs_msgs::msg::Fn70Auxa> imu_sub_; message_filters::Subscriber<plc_msgs::msg::Sensor> depth_sub_; std::shared_ptr<message_filters::TimeSynchronizer<dvl_msgs::msg::Odom, ahrs_msgs::msg::Fn70Auxa, plc_msgs::msg::Sensor>> sync_; }; int main(int argc, char * argv[]) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared<AUVSENSORS_MESSAGES>()); rclcpp::shutdown(); return 0; }
我想输出几个实例数据,因此我运行记录数据的bag包,其能够正常play,
os2 bag play rosbag2_2023_12_08-15_36_13 [INFO] [1702044100.446161521] [rosbag2_storage]: Opened database 'rosbag2_2023_12_08-15_36_13/rosbag2_2023_12_08-15_36_13_0.db3' for READ_ONLY. [INFO] [1702044100.446240068] [rosbag2_player]: Set rate to 1 [INFO] [1702044100.456354394] [rosbag2_player]: Adding keyboard callbacks. [INFO] [1702044100.456406646] [rosbag2_player]: Press SPACE for Pause/Resume [INFO] [1702044100.456417656] [rosbag2_player]: Press CURSOR_RIGHT for Play Next Message [INFO] [1702044100.456426625] [rosbag2_player]: Press CURSOR_UP for Increase Rate 10% [INFO] [1702044100.456434797] [rosbag2_player]: Press CURSOR_DOWN for Decrease Rate 10% [INFO] [1702044100.456868918] [rosbag2_storage]: Opened database 'rosbag2_2023_12_08-15_36_13/rosbag2_2023_12_08-15_36_13_0.db3' for READ_ONLY.
但是我订阅其话题并实现时间同步输出实例参数的终端报以下错误,这是怎么回事,怎样解决呢?
terminate called after throwing an instance of 'std::runtime_error' what(): can't compare times with different time sources [ros2run]: Aborted
-
鱼总编写的ros2bag_convert问题
系统UBUNTU22.04,系统ros2 humble
大佬们,我想请问一下各位在使用鱼总开发的将bag包转为csv文件的那个工具ros2bag_convert,我在转自定义信息的时候都可以,但是我在转sensor_msgs/msg/NavSatfIX这种ros2自带的消息类型的时候容易报错或者转换不成功,大家有遇到过这种问题吗?(github上有人提出这个问题过,但是好像没有解决(https://github.com/fishros/ros2bag_convert/issues/3))
问题:
-
ROS2的lifecycle的相关问题
系统ubuntu22.04,ROS2humble
各位大佬们,我在项目中遇到这么一个问题,就是我的设备目前情况是这样的:
在短途内通过wifi发送上电指令,上电后,通过PLC的上电状况来启动和暂停节点,举个问题的例子因为我的IMU和DVL是通过串口转socket向工控机发送消息,如果首先开启IMU的发布节点,而IMU没有上电,IMU节点会因为socket通信迟迟没有连接从而报错退出节点,因此我考虑使用lifecycle来管理节点,通过上电后返回的上电状态来启动和暂停节点的运行,但是lifecycle的客户端总是在响应过后,能够正常启动服务端节点,但是每次启动后,客户端总是会退出,必须重新运行节点,我想能让他持续等待下一次的下电状态,请问我应该怎么修改比较好.
大佬们,我采用官方给的客户端程序如下(学的比较基础)#include <chrono> #include <memory> #include <string> #include <thread> #include <iostream> #include <stdio.h> #include <stdlib.h> #include <auv_msgs/msg/wifidata.hpp> #include "lifecycle_msgs/msg/state.hpp" #include "lifecycle_msgs/msg/transition.hpp" #include "lifecycle_msgs/srv/change_state.hpp" #include "lifecycle_msgs/srv/get_state.hpp" #include "auv_msgs/msg/wifidata.hpp" #include "rclcpp/rclcpp.hpp" #include "auv_lifecycle/lifecycle_var.h" #include "rcutils/logging_macros.h" using namespace std; using std::placeholders::_1; using namespace std::chrono_literals; static constexpr char const *lifecycle_node = "ctd_sensor_publisher"; static constexpr char const *node_get_state_topic = "ctd_sensor_publisher/get_state"; static constexpr char const *node_change_state_topic = "ctd_sensor_publisher/change_state"; template <typename FutureT, typename WaitTimeT> std::future_status wait_for_result( FutureT &future, WaitTimeT time_to_wait) { auto end = std::chrono::steady_clock::now() + time_to_wait; std::chrono::milliseconds wait_period(100); std::future_status status = std::future_status::timeout; do { auto now = std::chrono::steady_clock::now(); auto time_left = end - now; if (time_left <= std::chrono::seconds(0)) { break; } status = future.wait_for((time_left < wait_period) ? time_left : wait_period); } while (rclcpp::ok() && status != std::future_status::ready); return status; } class LifecycleServiceClient : public rclcpp::Node { public: explicit LifecycleServiceClient(const std::string &node_name) : Node(node_name) { } void init() { Power_on_sub_ = this->create_subscription<auv_msgs::msg::WIFIDATA>("wifi_data_state", 10, std::bind(&LifecycleServiceClient::power_on_callback, this, _1)); client_get_state_ = this->create_client<lifecycle_msgs::srv::GetState>(node_get_state_topic); client_change_state_ = this->create_client<lifecycle_msgs::srv::ChangeState>(node_change_state_topic); } unsigned int get_state(std::chrono::seconds time_out = 1000000s) { auto request = std::make_shared<lifecycle_msgs::srv::GetState::Request>(); if (!client_get_state_->wait_for_service(time_out)) { RCLCPP_ERROR( get_logger(), "Service %s is not available.", client_get_state_->get_service_name()); return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; } // We send the service request for asking the current // state of the lc_talker node. auto future_result = client_get_state_->async_send_request(request).future.share(); // Let's wait until we have the answer from the node. // If the request times out, we return an unknown state. auto future_status = wait_for_result(future_result, time_out); if (future_status != std::future_status::ready) { RCLCPP_ERROR( get_logger(), "Server time out while getting current state for node %s", lifecycle_node); return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; } // We have an succesful answer. So let's print the current state. if (future_result.get()) { RCLCPP_INFO( get_logger(), "Node %s has current state %s.", lifecycle_node, future_result.get()->current_state.label.c_str()); return future_result.get()->current_state.id; } else { RCLCPP_ERROR( get_logger(), "Failed to get current state for node %s", lifecycle_node); return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; } } bool change_state(std::uint8_t transition, std::chrono::seconds time_out = 1000000s) { auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>(); request->transition.id = transition; if (!client_change_state_->wait_for_service(time_out)) { RCLCPP_ERROR( get_logger(), "Service %s is not available.", client_change_state_->get_service_name()); return false; } // We send the request with the transition we want to invoke. auto future_result = client_change_state_->async_send_request(request).future.share(); // Let's wait until we have the answer from the node. // If the request times out, we return an unknown state. auto future_status = wait_for_result(future_result, time_out); if (future_status != std::future_status::ready) { RCLCPP_ERROR( get_logger(), "Server time out while getting current state for node %s", lifecycle_node); return false; } // We have an answer, let's print our success. if (future_result.get()->success) { RCLCPP_INFO( get_logger(), "Transition %d successfully triggered.", static_cast<int>(transition)); return true; } else { RCLCPP_WARN( get_logger(), "Failed to trigger transition %u", static_cast<unsigned int>(transition)); return false; } } private: std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::GetState>> client_get_state_; std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::ChangeState>> client_change_state_; rclcpp::Subscription<auv_msgs::msg::WIFIDATA>::SharedPtr Power_on_sub_; std::mutex mtxlf; void power_on_callback(const auv_msgs::msg::WIFIDATA &power_on_msg) { mtxlf.lock(); ctd_powerflag = power_on_msg.bcontrolon_cabina; // cout<<ctd_powerflag<<endl; mtxlf.unlock(); } }; void callee_script(std::shared_ptr<LifecycleServiceClient> lc_client) { rclcpp::WallRate time_between_state_changes(0.1); // 10s sleep(3); cout << ctd_powerflag << endl; if (ctd_powerflag == true) { if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE)) { return; } if (!lc_client->get_state()) { return; } } if (!rclcpp::ok()) { return; } if (ctd_powerflag == false) { if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP)) { return; } if (!lc_client->get_state()) { return; } } // activate // { // time_between_state_changes.sleep(); // if (!rclcpp::ok()) // { // return; // } // if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE)) // { // return; // } // if (!lc_client->get_state()) // { // return; // } // } // deactivate // { // time_between_state_changes.sleep(); // if (!rclcpp::ok()) // { // return; // } // if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE)) // { // return; // } // if (!lc_client->get_state()) // { // return; // } // } // // activate it again // { // time_between_state_changes.sleep(); // if (!rclcpp::ok()) { // return; // } // if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE)) { // return; // } // if (!lc_client->get_state()) { // return; // } // } // // and deactivate it again // { // time_between_state_changes.sleep(); // if (!rclcpp::ok()) { // return; // } // if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE)) { // return; // } // if (!lc_client->get_state()) { // return; // } // } // we cleanup // time_between_state_changes.sleep(); // if (!rclcpp::ok()) { // return; // } // and finally shutdown // Note: We have to be precise here on which shutdown transition id to call // We are currently in the unconfigured state and thus have to call // TRANSITION_UNCONFIGURED_SHUTDOWN // { // time_between_state_changes.sleep(); // if (!rclcpp::ok()) { // return; // } // if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)) // { // return; // } // if (!lc_client->get_state()) { // return; // } // } } // void wake_executor(std::shared_future<void> future, rclcpp::executors::SingleThreadedExecutor &exec) void wake_executor(std::shared_future<void> future, rclcpp::executors::MultiThreadedExecutor &exec) { future.wait(); exec.cancel(); } int main(int argc, char **argv) { // force flush of the stdout buffer. setvbuf(stdout, NULL, _IONBF, BUFSIZ); rclcpp::init(argc, argv); auto lc_client = std::make_shared<LifecycleServiceClient>("ctd_sensor_publisher"); lc_client->init(); // rclcpp::executors::SingleThreadedExecutor exe; rclcpp::executors::MultiThreadedExecutor exe; exe.add_node(lc_client); std::shared_future<void> script = std::async( std::launch::async, std::bind(callee_script, lc_client)); auto wake_exec = std::async( std::launch::async, std::bind(wake_executor, script, std::ref(exe))); exe.spin_until_future_complete(script); rclcpp::shutdown(); return 0; }
-
RE: Segmentation fault (Address not mapped to object [(nil)])
@小鱼 您好,我的终端是这样报错的
Thread 1 "usbl_parse_pub" received signal SIGSEGV, Segmentation fault. 0x0000555555567ab6 in usbl_parse_pub::USBL_DATA_PARSE (this=0x555555640430, data=0x7fffffff2f6e "\fZ\003") at /home/user7/auv533_ws/src/auv_socket/src/usbl_parse_pub.cpp:175 175 if (tmpt.PACKET_DATA[0] == 10 && tmpt.PACKET_LEN == 15) (gdb)
-
RE: Segmentation fault (Address not mapped to object [(nil)])
@小鱼 您好,我测试出是哪个地方的问题了,我目前想在一个cpp文件中发布多个话题,但是在发布另一个话题时总是出现上面的问题,您能帮我看看程序的问题吗?
#include <memory> #include <iostream> #include <fstream> #include <iomanip> #include "rclcpp/rclcpp.hpp" #include <stdio.h> #include <stdlib.h> #include <string.h> #include <sys/socket.h> #include <sys/types.h> #include <unistd.h> #include <arpa/inet.h> #include <netinet/in.h> #include <fcntl.h> #include <list> #include <ctime> #include <time.h> #include <sys/shm.h> #include "auv_msgs/msg/usbl.hpp" #include "auv_msgs/msg/shorebased.hpp" #include <vector> #include "auv_socket/USBL_STRUCT.h" #include "auv_socket/USBL_SHOREBASED.h" #include "std_msgs/msg/bool.hpp" // 通过socket通信接收数据,并实现USBL数据传输发布,发布话题名称为usbl_data // 信息格式:auv_msgs/msg/USBL ofstream usblfile; string usblfilename; // 设置端口和数据长度 #define PORT 29 #define DATASIZE 1024 #define BUFSIZE 40960 // 23 DVL 26 IMU 29 USBL 32 BDS // 设置IP地址 char *SERVER_IP = "192.168.0.7"; using namespace std; using namespace std::chrono_literals; class usbl_data_pub : public rclcpp::Node { public: usbl_data_pub() : Node("usbl_data_pub") { // 创建输出文件时间前缀 time_t now; struct tm *usbl_time_info; char usbl_time[20]; time(&now); usbl_time_info = localtime(&now); strftime(usbl_time, sizeof(usbl_time), "%Y%m%d%H%M%S", usbl_time_info); string usbltime(usbl_time); usblfilename = "txt/usbldata/usbldata_" + usbltime + ".txt"; ofstream usblfile(usblfilename); if (!usblfile.is_open()) { RCLCPP_ERROR(this->get_logger(), "USBL FILE OPEN ERROR"); } // 创建回调组 callback_group_1 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); auto pub_opt_1 = rclcpp::PublisherOptions(); pub_opt_1.callback_group = callback_group_1; callback_group_2 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); auto pub_opt_2 = rclcpp::PublisherOptions(); pub_opt_1.callback_group = callback_group_2; // 创建发布对象 usbl_publisher_ = this->create_publisher<auv_msgs::msg::USBL>("usbl_data", 100,pub_opt_1); usbl_parse_pub = this->create_publisher<auv_msgs::msg::SHOREBASED>("usbl_parsedata", 100,pub_opt_2); usbl_flag_pub = this->create_publisher<std_msgs::msg::Bool>("usbl_response", 100,pub_opt_1); usbl_timer_ = this->create_wall_timer(1s, std::bind(&usbl_data_pub::usbl_time_callback, this),callback_group_1); } private: void usbl_time_callback() { // socket通讯,创建socket对象和连接 char recvbuf[BUFSIZE]; int socket_cli = socket(AF_INET, SOCK_STREAM, 0); if (socket_cli < 0) { RCLCPP_ERROR(this->get_logger(), "USBL SOCKET ERROR"); } struct sockaddr_in sev_addr; memset(&sev_addr, 0, sizeof(sev_addr)); sev_addr.sin_family = AF_INET; sev_addr.sin_port = htons(PORT); sev_addr.sin_addr.s_addr = inet_addr(SERVER_IP); // 判断是否连接成功 if (connect(socket_cli, (struct sockaddr *)&sev_addr, sizeof(sev_addr)) < 0) { RCLCPP_ERROR(this->get_logger(), "USBL CONNECT ERROR"); } else { RCLCPP_INFO(this->get_logger(), "USBL CONNECTED SUCCESS"); } recvFlag = false; while (rclcpp::ok()) { // 接收来自socket缓冲区的信息 recv(socket_cli, recvbuf, sizeof(recvbuf), 0); RCLCPP_INFO(this->get_logger(), "%s", recvbuf); // 创建USBL的消息腹部格式并赋值发布 auto usbl_data = auv_msgs::msg::USBL(); auto usbl_response = std_msgs::msg::Bool(); auto usbl_parse_msg = auv_msgs::msg::SHOREBASED(); usbl_response.data = recvFlag; // usbl_flag_pub->publish(usbl_response); // usbl_data.header.frame_id = "usbl_link"; rclcpp::Time usbl_right_time = this->get_clock()->now(); usbl_data.usbl_sys_time = usbl_right_time.seconds(); // usbl_data.header.stamp = usbl_right_time; unsigned char data[BUFSIZE] = {0}; const char *split = "$"; // 使用,割符对接收到的数据进行分割 char *buf; char *p = strtok(recvbuf, split); while (p != NULL) { buf = p; // 将获得的数据赋给buf,并将其从16进制转为ASCII,赋值给data数组 for (int i = 0; i < strlen(buf); i += 2) { unsigned char c = 0; unsigned char a = buf[i]; // 第一位 unsigned char b = buf[i + 1]; // 后一位 if (a <= '9' && a >= '0') c = a - '0'; // zhuanma else if (a <= 'F' && a >= 'A') c = a - 'A' + 10; else if (a <= 'f' && a >= 'a') c = a - 'a' + 10; else break; c = c << 4; if (b <= '9' && b >= '0') c += b - '0'; else if (b <= 'F' && b >= 'A') c += b - 'A' + 10; else if (b <= 'f' && b >= 'a') c += b - 'a' + 10; else break; data[i >> 1] = c; } // 确定数据类型 // 150向110发送数据,150发送成功,110接收失败,150反馈收到32和63的数据,110显示32 // 单$32 110发送成功,150接收不到 $32 $63 150发送失败 // 110PING150或者向150发送数据,或者150向110发送数据,其反馈的以$39开头的数据其均未包含任何信息, // 只有150PING110时,以$39开头的数据才包含角度,距离信息 // 考虑反馈返回的数据程序是否冲突 switch (data[0]) { case 0x32: { // 如果出现32数据,说明110向150发送数据,150接收不成功 RCLCPP_ERROR(this->get_logger(), "Message sent when the transceiver receiver encounters an error"); break; } case 0x35: { // 如果出现35数据说明150向110发送数据,110已经接收代码未处理 RCLCPP_INFO_ONCE(this->get_logger(), "Message sent when the transceiver receives a response (to a transmitted request)."); break; } case 0x38: { // USBL自身设备信息 CID_XCVR_USBL tmpt; const unsigned char *q = data; tmpt = CID_XCVR_USBL(q); break; } case 0x39: { // 读取USBL相对信息 CID_XCVR_FIX tmpt; const unsigned char *q = data; tmpt = CID_XCVR_FIX(q); usbl_data.beacon_id = tmpt.ACO_FIX.DEST_ID; usbl_data.local_depth = tmpt.ACO_FIX.DEPTH_LOCAL; usbl_data.usbl_vos = tmpt.ACO_FIX.VOS / 10.0; usbl_data.usbl_roll = tmpt.ACO_FIX.ATTITUDE_ROLL / 10.0; // 通过此标志符号判断39数据是由PING产生的还是发送数据60产生的,不是PING产生的订阅时产生的数据为0 // 发送60数据产生的39数值错误 if (tmpt.ACO_FIX.FLAGS & 0x01) { usbl_data.range_count = tmpt.ACO_FIX.RANGE_FIELD.RANGE_COUNT; usbl_data.range_time = tmpt.ACO_FIX.RANGE_FIELD.RANGE_TIME; usbl_data.range_dist = tmpt.ACO_FIX.RANGE_FIELD.RANGE_DIST / 10.0 / 2.0; } if (tmpt.ACO_FIX.FLAGS & 0x02) { usbl_data.usbl_pitch = tmpt.ACO_FIX.USBL_FIELD.USBL_ELEVATION / 10.0; usbl_data.usbl_yaw = tmpt.ACO_FIX.USBL_FIELD.USBL_AZIMUTH / 10.0; } if (tmpt.ACO_FIX.FLAGS & 0x04) { usbl_data.position_east = tmpt.ACO_FIX.POSITION_FIELD.POSITION_EASTING / 10.0 / 2.0; usbl_data.position_north = tmpt.ACO_FIX.POSITION_FIELD.POSITION_NORTHING / 10.0 / 2.0; usbl_data.position_depth = tmpt.ACO_FIX.POSITION_FIELD.POSITION_DEPTH / 10.0 / 2.0; } usbl_publisher_->publish(usbl_data); // 将数据保存进txt文件中 usblfile.open(usblfilename, ios::out | ios::app); usblfile << usbl_data.usbl_sys_time << " "; usblfile << fixed << setprecision(4) << usbl_data.usbl_pitch << " " << usbl_data.usbl_yaw << " " << usbl_data.position_east << " " << usbl_data.position_north << " " << usbl_data.position_depth << endl; usblfile.close(); break; } case 0x43: { // 如果出现43数据,说明150PING110不成功代码未处理 RCLCPP_ERROR(this->get_logger(), "Message sent when a PING response error/timeout occurs"); break; } case 0x60: { // 150 向110发送成功反馈,不管110是否已经接收成功 // usbl_response.data = false; // usbl_flag_pub->publish(usbl_response); recvFlag = false; RCLCPP_INFO(this->get_logger(), "Message sent to transmit a datagram to another beacon)."); break; } case 0x61: { // 解析110发送给150的数据 // 注意格式61数据产生的39指令不包含位姿角度和东北天向距离 // CID_DAT_RECEIVE tmpt; const unsigned char *q = data; USBL_DATA_PARSE(data); break; } case 0x63: { // 标志150向110发送信息失败,代码未处理 RCLCPP_ERROR(this->get_logger(), "Message generated when a beacon response error/timeout occurs for ACKs."); break; } default: break; } p = strtok(NULL, split); } } close(socket_cli); } void USBL_DATA_PARSE(const unsigned char *data) { auto usbl_parse_msg = auv_msgs::msg::SHOREBASED(); CID_DAT_RECEIVE tmpt; tmpt = CID_DAT_RECEIVE(data); if (tmpt.PACKET_DATA[0] == 10 && tmpt.PACKET_LEN == 15) { // recvFlag = true; usbl_parse_msg.nextpos[0] = tmpt.PACKET_DATA[1]; // 当前路径点总个数 usbl_parse_msg.nextpos[1] = tmpt.PACKET_DATA[2]; // 当前路径点位数 usbl_parse_msg.nextpos[2] = ((tmpt.PACKET_DATA[3] + (tmpt.PACKET_DATA[4] << 8) + (tmpt.PACKET_DATA[5] << 16) + (tmpt.PACKET_DATA[6] << 24))); // 纬度 usbl_parse_msg.nextpos[3] = ((tmpt.PACKET_DATA[7] + (tmpt.PACKET_DATA[8] << 8) + (tmpt.PACKET_DATA[9] << 16) + (tmpt.PACKET_DATA[10] << 24))); // 经度 usbl_parse_msg.nextpos[4] = ((tmpt.PACKET_DATA[11] + (tmpt.PACKET_DATA[12] << 8) + (tmpt.PACKET_DATA[13] << 16) + (tmpt.PACKET_DATA[14] << 24))); // 高度 usbl_parse_pub->publish(usbl_parse_msg); // usbl_parse_pub->publish(usbl_parse_msg); } } rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr usbl_flag_pub; rclcpp::TimerBase::SharedPtr usbl_timer_; rclcpp::Publisher<auv_msgs::msg::USBL>::SharedPtr usbl_publisher_; rclcpp::Publisher<auv_msgs::msg::SHOREBASED>::SharedPtr usbl_parse_pub; rclcpp::CallbackGroup::SharedPtr callback_group_1; rclcpp::CallbackGroup::SharedPtr callback_group_2; }; int main(int argc, char *argv[]) { rclcpp::init(argc, argv); // rclcpp::spin(std::make_shared<usbl_data_pub>()); rclcpp::executors::MultiThreadedExecutor executor; auto usbl_data_pub_node = std::make_shared<usbl_data_pub>(); executor.add_node(usbl_data_pub_node); executor.spin(); rclcpp::shutdown(); return 0; }
程序在发布话题时出现问题,我把话题发布注释掉就可行了,但是目前我想发布这个话题
void USBL_DATA_PARSE(const unsigned char *data) { auto usbl_parse_msg = auv_msgs::msg::SHOREBASED(); CID_DAT_RECEIVE tmpt; tmpt = CID_DAT_RECEIVE(data); if (tmpt.PACKET_DATA[0] == 10 && tmpt.PACKET_LEN == 15) { // recvFlag = true; usbl_parse_msg.nextpos[0] = tmpt.PACKET_DATA[1]; // 当前路径点总个数 usbl_parse_msg.nextpos[1] = tmpt.PACKET_DATA[2]; // 当前路径点位数 usbl_parse_msg.nextpos[2] = ((tmpt.PACKET_DATA[3] + (tmpt.PACKET_DATA[4] << 8) + (tmpt.PACKET_DATA[5] << 16) + (tmpt.PACKET_DATA[6] << 24))); // 纬度 usbl_parse_msg.nextpos[3] = ((tmpt.PACKET_DATA[7] + (tmpt.PACKET_DATA[8] << 8) + (tmpt.PACKET_DATA[9] << 16) + (tmpt.PACKET_DATA[10] << 24))); // 经度 usbl_parse_msg.nextpos[4] = ((tmpt.PACKET_DATA[11] + (tmpt.PACKET_DATA[12] << 8) + (tmpt.PACKET_DATA[13] << 16) + (tmpt.PACKET_DATA[14] << 24))); // 高度 usbl_parse_pub->publish(usbl_parse_msg); // usbl_parse_pub->publish(usbl_parse_msg); } }
-
Segmentation fault (Address not mapped to object [(nil)])
大佬们,救救孩子吧,一天了,请问这是怎么回事,我用GDB调试,程序输出以下问题
Stack trace (most recent call last): #10 Object "[0xffffffffffffffff]", at 0xffffffffffffffff, in #9 Object "/home/user7/auv533_ws/install/auv_socket/lib/auv_socket/usbl_data_pub", at 0x55e9f1cc8254, in #8 Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7f2d54e29e3f, in __libc_start_main #7 Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7f2d54e29d8f, in #6 Object "/home/user7/auv533_ws/install/auv_socket/lib/auv_socket/usbl_data_pub", at 0x55e9f1cc812f, in #5 Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f2d5562524e, in rclcpp::spin(std::shared_ptr<rclcpp::Node>) #4 Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f2d55625154, in rclcpp::spin(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>) #3 Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f2d55624f3f, in rclcpp::executors::SingleThreadedExecutor::spin() #2 Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f2d5561d650, in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) #1 Object "/home/user7/auv533_ws/install/auv_socket/lib/auv_socket/usbl_data_pub", at 0x55e9f1ccca44, in #0 Object "/home/user7/auv533_ws/install/auv_socket/lib/auv_socket/usbl_data_pub", at 0x55e9f1cdbbad, in Segmentation fault (Address not mapped to object [(nil)]) [ros2run]: Segmentation fault
救救孩子吧
-
message_filters的问题
系统UBUNTU22.04,ros2 humble
各位大佬,我在使用message_filters时,订阅多个传感器的信息,我的程序如下#include <memory> #include <iostream> #include <fstream> #include <iomanip> #include "rclcpp/rclcpp.hpp" #include <stdio.h> #include <stdlib.h> #include <string.h> #include <sys/socket.h> #include <sys/types.h> #include <unistd.h> #include <arpa/inet.h> #include <netinet/in.h> #include <fcntl.h> #include <algorithm> #include <sys/shm.h> #include <message_filters/subscriber.h> #include <message_filters/time_synchronizer.h> #include <message_filters/signal9.h> #include "auv_msgs/msg/imu.hpp" #include "auv_msgs/msg/ctd.hpp" #include "auv_msgs/msg/obstacl_esonar.hpp" #include "auv_msgs/msg/usbl.hpp" #include "auv_socket/USBL_STRUCT.h" // 设置端口和数据长度 #define PORT 29 #define BUFSIZE 1024 extern bool error_flag; // 23 DVL 26 IMU 29 USBL 32 BDS char *SERVER_IP = "192.168.0.7"; using namespace std; using namespace std::chrono_literals; class imu_sensor_sub : public rclcpp::Node { public: imu_sensor_sub() : Node("imu_sensor_sub") { // 创建创建传感器订阅对象 imu_sub_.subscribe(this,"imu_data"); ctd_sub_.subscribe(this,"ctd_data"); hsonar_sub_.subscribe(this,"sf_data"); vsonar_sub_.subscribe(this,"sf_data_v"); //设置message_filters同步订阅对象 message_filters::TimeSynchronizer<auv_msgs::msg::IMU,auv_msgs::msg::CTD,auv_msgs::msg::OBSTACLEsonar,auv_msgs::msg::OBSTACLEsonar> sensor_info_(imu_sub_,ctd_sub_,hsonar_sub_,vsonar_sub_,5); sensor_info_.registerCallback(std::bind(&imu_sensor_sub::sensor_info_callback,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4)); //imu_sub_ = this->create_subscription<auv_msgs::msg::IMU>("imu_data", 10, std::bind(&imu_sensor_sub::imu_callback, this, _1)); } private: void sensor_info_callback(const auv_msgs::msg::IMU &imu_msg, const auv_msgs::msg::CTD &ctd_msg, const auv_msgs::msg::OBSTACLEsonar &hsonar_msg,const auv_msgs::msg::OBSTACLEsonar &vsonar_msg) { // socket通讯,创建socket对象和进行连接 int socket_cli = socket(AF_INET, SOCK_STREAM, 0); if (socket_cli < 0) { RCLCPP_ERROR(this->get_logger(), "SENSOR SUB AND USBL TRANSMIT SOCKET ERROR"); } struct sockaddr_in sev_addr; memset(&sev_addr, 0, sizeof(sev_addr)); sev_addr.sin_family = AF_INET; sev_addr.sin_port = htons(PORT); sev_addr.sin_addr.s_addr = inet_addr(SERVER_IP); RCLCPP_INFO(this->get_logger(), "SENSOR SUB AND USBL TRANSMIT CONNECT RUNNING"); // 判断连接是否正常 if (connect(socket_cli, (struct sockaddr *)&sev_addr, sizeof(sev_addr)) < 0) { RCLCPP_ERROR(this->get_logger(), "SENSOR SUB AND USBL TRANSMIT CONNECT ERROR"); } else { RCLCPP_INFO(this->get_logger(), "SENSOR SUB AND USBL TRANSMIT CONNECTED SUCCESS"); } while (rclcpp::ok()) { } message_filters::Subscriber<auv_msgs::msg::IMU> imu_sub_; message_filters::Subscriber<auv_msgs::msg::CTD> ctd_sub_; message_filters::Subscriber<auv_msgs::msg::OBSTACLEsonar> hsonar_sub_; message_filters::Subscriber<auv_msgs::msg::OBSTACLEsonar> vsonar_sub_; /* rclcpp::Subscription<auv_msgs::msg::IMU>::SharedPtr imu_sub_; rclcpp::Subscription<auv_msgs::msg::CTD>::SharedPtr ctd_sub_; */ }; int main(int argc, char *argv[]) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared<imu_sensor_sub>()); rclcpp::shutdown(); return 0; }
终端始终报以下错误
/opt/ros/humble/include/message_filters/message_filters/signal9.h:234:14: note: candidate: ‘template<class T, class P0, class P1, class P2> message_filters::Connection message_filters::Signal9<M0, M1, M2, M3, M4, M5, M6, M7, M8>::addCallback(void (T::*)(P0, P1, P2), T*) [with T = T; P0 = P0; P1 = P1; P2 = P2; M0 = auv_msgs::msg::IMU_<std::allocator<void> >; M1 = auv_msgs::msg::CTD_<std::allocator<void> >; M2 = auv_msgs::msg::OBSTACLEsonar_<std::allocator<void> >; M3 = auv_msgs::msg::OBSTACLEsonar_<std::allocator<void> >; M4 = message_filters::NullType; M5 = message_filters::NullType; M6 = message_filters::NullType; M7 = message_filters::NullType; M8 = message_filters::NullType]’ 234 | Connection addCallback(void(T::*callback)(P0, P1, P2), T* t) | ^~~~~~~~~~~ /opt/ros/humble/include/message_filters/message_filters/signal9.h:234:14: note: template argument deduction/substitution failed: /opt/ros/humble/include/message_filters/message_filters/signal9.h:280:40: error: wrong number of template arguments (9, should be 4) 272 | return addCallback<const M0ConstPtr&, | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 273 | const M1ConstPtr&, | ~~~~~~~~~~~~~~~~~~ 274 | const M2ConstPtr&, | ~~~~~~~~~~~~~~~~~~ 275 | const M3ConstPtr&, | ~~~~~~~~~~~~~~~~~~ 276 | const M4ConstPtr&, | ~~~~~~~~~~~~~~~~~~ 277 | const M5ConstPtr&, | ~~~~~~~~~~~~~~~~~~ 278 | const M6ConstPtr&, | ~~~~~~~~~~~~~~~~~~ 279 | const M7ConstPtr&, | ~~~~~~~~~~~~~~~~~~ 280 | const M8ConstPtr&>(std::bind(callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, std::placeholders::_8, std::placeholders::_9)); | ~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ /opt/ros/humble/include/message_filters/message_filters/signal9.h:234:14: note: provided for ‘template<class T, class P0, class P1, class P2> message_filters::Connection message_filters::Signal9<M0, M1, M2, M3, M4, M5, M6, M7, M8>::addCallback(void (T::*)(P0, P1, P2), T*) [with T = T; P0 = P0; P1 = P1; P2 = P2; M0 = auv_msgs::msg::IMU_<std::allocator<void> >; M1 = auv_msgs::msg::CTD_<std::allocator<void> >; M2 = auv_msgs::msg::OBSTACLEsonar_<std::allocator<void> >; M3 = auv_msgs::msg::OBSTACLEsonar_<std::allocator<void> >; M4 = message_filters::NullType; M5 = message_filters::NullType; M6 = message_filters::NullType; M7 = message_filters::NullType; M8 = message_filters::NullType]’ 234 | Connection addCallback(void(T::*callback)(P0, P1, P2), T* t) | ^~~~~~~~~~~ /opt/ros/humble/include/message_filters/message_filters/signal9.h:240:14: note: candidate: ‘template<class T, class P0, class P1, class P2, class P3> message_filters::Connection message_filters::Signal9<M0, M1, M2, M3, M4, M5, M6, M7, M8>::addCallback(void (T::*)(P0, P1, P2, P3), T*) [with T = T; P0 = P0; P1 = P1; P2 = P2; P3 = P3; M0 = auv_msgs::msg::IMU_<std::allocator<void> >; M1 = auv_msgs::msg::CTD_<std::allocator<void> >; M2 = auv_msgs::msg::OBSTACLEsonar_<std::allocator<void> >; M3 = auv_msgs::msg::OBSTACLEsonar_<std::allocator<void> >; M4 = message_filters::NullType; M5 = message_filters::NullType; M6 = message_filters::NullType; M7 = message_filters::NullType; M8 = message_filters::NullType]’ 240 | Connection addCallback(void(T::*callback)(P0, P1, P2, P3), T* t) | ^~~~~~~~~~~ /opt/ros/humble/include/message_filters/message_filters/signal9.h:240:14: note: template argument deduction/substitution failed: /opt/ros/humble/include/message_filters/message_filters/signal9.h:280:40: error: wrong number of template arguments (9, should be 5)
问题大概是这种,重复了九次,请问我这个问题出在哪里,模板函数明明有,奇怪
-
[ros2run]: Segmentation fault
系统ubuntu22.04,ros2
各位大佬,我在进行ROS2 RUN时,终端输出以下问题:ros2 run auv_socket imu_data_pub [INFO] [1679059271.449093718] [imu_data_pub]: Connect RUNNING [INFO] [1679059271.449619076] [imu_data_pub]: connected successfullly! server message: $AUXA,1873.105,001873.100, 0.000, 0.000, 0.000,31.88882479,120.56537209, 29.64, 0.000, 0.000,0.0000, 0.000464, 0.001863,-0.003287,-0.002777, 0.999490, 0.017888,23.31,31.88882479,120.56537209, 29.64, 0.000, 0.000, 0.000, 0,00|00|00|000|000|0.00,0x02004080,0x00000010*C9 ��U [ros2run]: Segmentation fault
我的惯导发布节点代码如下:
#include <memory> #include <iostream> #include <fstream> #include <iomanip> #include "rclcpp/rclcpp.hpp" #include <stdio.h> #include <stdlib.h> #include <string.h> #include <sys/socket.h> #include <sys/types.h> #include <unistd.h> #include <arpa/inet.h> #include <netinet/in.h> #include <fcntl.h> #include <sys/shm.h> #include "auv_msgs/msg/imu.hpp" // IMU数据传输发布 #define PORT 26 #define BUFSIZE 1024 // 23 DVL 26 IMU 29 USBL 32 BDS char *SERVER_IP = "192.168.0.7"; int result = 0; using namespace std; using namespace std::chrono_literals; class imu_data_pub : public rclcpp::Node { public: imu_data_pub() : Node("imu_data_pub") { // 添加上电信号的指令 auto now = std::chrono::system_clock::now(); // 通过不同精度获取相差的毫秒数 uint64_t dis_millseconds = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count() - std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch()).count() * 1000; time_t tt = std::chrono::system_clock::to_time_t(now); auto time_tm = localtime(&tt); char strTime[25] = {0}; sprintf(strTime, "%d-%02d-%02d %02d:%02d:%02d %03d", time_tm->tm_year + 1900, time_tm->tm_mon + 1, time_tm->tm_mday, time_tm->tm_hour, time_tm->tm_min, time_tm->tm_sec, (int)dis_millseconds); ofstream imufile; imufile.open("imudata.txt", ios::out | ios::app); imufile << "******" << strTime << "******" << endl; imufile.close(); // 创建发布对象 imu_publisher_ = this->create_publisher<auv_msgs::msg::IMU>("imu_data", 100); imu_timer_ = this->create_wall_timer(0.1s, std::bind(&imu_data_pub::imu_time_callback, this)); } private: void imu_time_callback() { // socket通讯 char recvbuf[BUFSIZE]; int socket_cli = socket(AF_INET, SOCK_STREAM, 0); if (socket_cli < 0) { RCLCPP_INFO(this->get_logger(), "socket() error!"); } struct sockaddr_in sev_addr; memset(&sev_addr, 0, sizeof(sev_addr)); sev_addr.sin_family = AF_INET; sev_addr.sin_port = htons(PORT); sev_addr.sin_addr.s_addr = inet_addr(SERVER_IP); RCLCPP_INFO(this->get_logger(), "Connect RUNNING"); if (connect(socket_cli, (struct sockaddr *)&sev_addr, sizeof(sev_addr)) < 0) { RCLCPP_INFO(this->get_logger(), "connect error"); } else { RCLCPP_INFO(this->get_logger(), "connected successfullly!"); } while (rclcpp::ok()) { // 读取变量信息 recv(socket_cli, recvbuf, sizeof(recvbuf), 0); printf("server message: %s\n", recvbuf); auto imu_data = auv_msgs::msg::IMU(); const char *split = ","; // 使用,割符进行分割 char *p = strtok(recvbuf, split); int num = 0; double imu_value[24]; while (p != NULL) { imu_value[num] = strtod(p, NULL); num++; p = strtok(NULL, split); } imu_data.header.frame_id = "imu_link"; rclcpp::Time imu_right_time = this->get_clock()->now(); imu_data.header.stamp = imu_right_time; imu_data.imu_run_time = imu_value[1]; imu_data.imu_utc_time = imu_value[2]; imu_data.imu_yaw = imu_value[3]; imu_data.imu_pitch = imu_value[4]; imu_data.imu_roll = imu_value[5]; imu_data.imu_lat = imu_value[6]; imu_data.imu_lon = imu_value[7]; imu_data.imu_height = imu_value[8]; imu_data.imu_n_velocity = imu_value[9]; imu_data.imu_e_velocity = imu_value[10]; imu_data.imu_estimate_err = imu_value[11]; imu_data.imu_x_angvel = imu_value[12]; imu_data.imu_y_angvel = imu_value[13]; imu_data.imu_z_angvel = imu_value[14]; imu_data.imu_x_speforce = imu_value[15]; imu_data.imu_y_speforce = imu_value[16]; imu_data.imu_z_speforce = imu_value[17]; imu_data.imu_sys_temp = imu_value[18]; imu_data.imu_ref_lat = imu_value[19]; imu_data.imu_ref_lon = imu_value[20]; imu_data.imu_ref_height = imu_value[21]; imu_data.imu_ref_northvel = imu_value[22]; imu_data.imu_ref_eastvel = imu_value[23]; imu_data.imu_ref_yaw = imu_value[24]; // 判断角度不能超过其规定范围 /*if (imu_data.imu_yaw < 0) { imu_data.imu_yaw += 360; // RCLCPP_INFO(this->get_logger(), "IMU DATA ERROR"); } else if (imu_data.imu_yaw > 360) { imu_data.imu_yaw -= 360; } else { */ // 将数据写入txt文件储存,以便后期调用 ofstream imufile; imufile.open("imudata.txt", ios::out | ios::app); for (int i = 0; i < 25; i++) { imufile << fixed << setprecision(4) << imu_value[i] << " "; } cout << endl; imufile.close(); // 发布数据 imu_publisher_->publish(imu_data); //} } close(socket_cli); } rclcpp::TimerBase::SharedPtr imu_timer_; rclcpp::Publisher<auv_msgs::msg::IMU>::SharedPtr imu_publisher_; }; int main(int argc, char *argv[]) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared<imu_data_pub>()); rclcpp::shutdown(); return 0; }
我利用的socket通信来获取数据信息,大佬们能帮我看看怎么回事吗,不胜感激