SICKAG / sick_scan_xd

Based on the sick_scan drivers for ROS1, sick_scan_xd merges sick_scan, sick_scan2 and sick_scan_base repositories. The driver supports both Linux (native, ROS1, ROS2) and Windows (native and ROS2).
Apache License 2.0
95 stars 85 forks source link

multiscan136 no pointcloud callbacks after power cycle #253

Closed leanderhendrikx closed 7 months ago

leanderhendrikx commented 7 months ago

I'm trying to get pointcloud messages from my multiscan136. If i execute the snippet below, i don't get pointcloud messages the first time i try after the lidar has started. If i let the example finish and try again, it works.

The logs are exactly the same for both tries (expect "Got PointCloudMSg" obviously), the last message I see is: [Info]: SickScanServices::sendSopasCmdCheckResponse(): request: "sMN Run", response: "sAN Run 1"

I'm using v3.0.0 on Ubuntu 20.04 arm64.

#include "sick_scan_xd/sick_scan_api.h"
#include <iostream>
#include <thread>

void pointcloud_callback(
    SickScanApiHandle handle,
    const SickScanPointCloudMsg* msg) 
{
  std::cout << "Got PointCloudMsg" << std::endl;
}

int main()
{
  SickScanApiLoadLibrary("/usr/local/lib/libsick_scan_xd_shared_lib.so");
  auto* apiHandle = SickScanApiCreate(0, nullptr);
  if (apiHandle == nullptr) {
    std::cerr << "Failed to create API handle" << std::endl;
    return -1;
  }

  const auto ret = SickScanApiInitByLaunchfile(apiHandle, "/opt/recorder/sick_multiscan.launch");
  if (ret != SICK_SCAN_API_SUCCESS) {
    std::cerr << "Failed to init API handle" << std::endl;
    return -1;
  }

  const auto regRet = SickScanApiRegisterCartesianPointCloudMsg(
      apiHandle, 
      &pointcloud_callback);
  if (regRet != SICK_SCAN_API_SUCCESS) {
    std::cerr << "Failed to register callback" << std::endl;
    return -1;
  }

  std::this_thread::sleep_for(std::chrono::seconds(5));

  const auto deregRet = SickScanApiDeregisterCartesianPointCloudMsg(
      apiHandle, 
      &pointcloud_callback);
  if (deregRet != SICK_SCAN_API_SUCCESS) {
    std::cerr << "Failed to deregister callback" << std::endl;
    return -1;
  }

  SickScanApiClose(apiHandle);
  SickScanApiRelease(apiHandle);
  SickScanApiUnloadLibrary();

  return 0;
}
leanderhendrikx commented 7 months ago

Seems to be fixed in v3.1.0. Thanks for fixing!