创建 std_srvs::Empty 型 Service (参数为空的服务)

  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
解释以下代码bool ret = laser.initialize(); if (ret) { ret = laser.turnOn(); } else { RCLCPP_ERROR(node->get_logger(), "%s\n", laser.DescribeError()); } auto laser_pub = node->create_publisher<sensor_msgs::msg::LaserScan>("scan", rclcpp::SensorDataQoS()); auto stop_scan_service = [&laser](const std::shared_ptr<rmw_request_id_t> request_header, const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> response) -> bool { return laser.turnOff(); }; auto stop_service = node->create_service<std_srvs::srv::Empty>("stop_scan",stop_scan_service); auto start_scan_service = [&laser](const std::shared_ptr<rmw_request_id_t> request_header, const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> response) -> bool { return laser.turnOn(); }; auto start_service = node->create_service<std_srvs::srv::Empty>("start_scan",start_scan_service); rclcpp::WallRate loop_rate(20); while (ret && rclcpp::ok()) { LaserScan scan;// if (laser.doProcessSimple(scan)) { auto scan_msg = std::make_shared<sensor_msgs::msg::LaserScan>(); scan_msg->header.stamp.sec = RCL_NS_TO_S(scan.stamp); scan_msg->header.stamp.nanosec = scan.stamp - RCL_S_TO_NS(scan_msg->header.stamp.sec); scan_msg->header.frame_id = frame_id; scan_msg->angle_min = scan.config.min_angle; scan_msg->angle_max = scan.config.max_angle; scan_msg->angle_increment = scan.config.angle_increment; scan_msg->scan_time = scan.config.scan_time; scan_msg->time_increment = scan.config.time_increment; scan_msg->range_min = scan.config.min_range; scan_msg->range_max = scan.config.max_range; int size = (scan.config.max_angle - scan.config.min_angle)/ scan.config.angle_increment + 1; scan_msg->ranges.resize(size); scan_msg->intensities.resize(size); for(size_t i=0; i < scan.points.size(); i++) { int index = std::ceil((scan.points[i].angle - scan.config.min_angle)/scan.config.angle_increment); if(index >=0 && index < size) { scan_msg->ranges[index] = scan.points[i].range; scan_msg->intensities[index] = scan.points[i].intensity; } } laser_pub->publish(*scan_msg); } else { RCLCPP_ERROR(node->get_logger(), "Failed to get scan"); } if(!rclcpp::ok()) { break; } rclcpp::spin_some(node); loop_rate.sleep(); } RCLCPP_INFO(node->get_logger(), "[YDLIDAR INFO] Now YDLIDAR is stopping ......."); laser.turnOff(); laser.disconnecting(); rclcpp::shutdown(); return 0; }
05-30
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值