<commit_before>#include <iostream>
#include <memory>
#include <thread>
#include <chrono>
#include <unordered_map>

#include <Eigen/Geometry>

#include <lcm/lcm-cpp.hpp>
#include <lcmtypes/drc/data_request_list_t.hpp>
#include <lcmtypes/drc/map_request_t.hpp>
#include <lcmtypes/drc/shaper_data_request_t.hpp>

#include <ConciseArgs>
#include <drc_utils/Clock.hpp>
#include <drc_utils/BotWrapper.hpp>

#include "RobotState.hpp"

using namespace std;

struct Worker {
  typedef std::shared_ptr<Worker> Ptr;

  bool mActive;
  drc::data_request_t mRequest;
  std::shared_ptr<lcm::LCM> mLcm;
  std::shared_ptr<drc::BotWrapper> mBotWrapper;
  std::shared_ptr<maps::RobotState> mRobotState;
  std::thread mThread;

  ~Worker() {
    stop();
  }

  void start() {
    if (mActive) return;
    if (mThread.joinable()) mThread.join();
    mActive = true;
    mThread = std::thread(std::ref(*this));
  }

  void stop() {
    mActive = false;
    if (mThread.joinable()) mThread.join();
  }

  void operator()() {
    mActive = true;
    int numCycles = 0;
    while (mActive) {

      // see if this is a cancel request
      if ((mRequest.period<0) || ((mRequest.period == 0) && (numCycles>0))) {
        mActive = false;
        break;
      }

      // dispatch
      switch(mRequest.type) {
      case drc::data_request_t::CAMERA_IMAGE_HEAD_LEFT:
      case drc::data_request_t::CAMERA_IMAGE_HEAD_RIGHT:
      case drc::data_request_t::CAMERA_IMAGE_LHAND:
      case drc::data_request_t::CAMERA_IMAGE_RHAND:
      case drc::data_request_t::CAMERA_IMAGE_LCHEST:
      case drc::data_request_t::CAMERA_IMAGE_RCHEST:
        sendCameraImageRequest(); break;
      case drc::data_request_t::MINIMAL_ROBOT_STATE:
        sendMinimalRobotStateRequest(); break;
//      case drc::data_request_t::AFFORDANCE_LIST:
  //      sendAffordanceListRequest(); break;
      case drc::data_request_t::MAP_CATALOG:
        sendMapViewCatalogRequest(); break;
      case drc::data_request_t::OCTREE_SCENE:
        sendOctreeSceneRequest(); break;
      case drc::data_request_t::OCTREE_WORKSPACE:
        sendOctreeWorkspaceRequest(); break;
      case drc::data_request_t::CLOUD_SCENE:
        sendCloudSceneRequest(); break;
      case drc::data_request_t::CLOUD_WORKSPACE:
        sendCloudWorkspaceRequest(); break;
      case drc::data_request_t::HEIGHT_MAP_SCENE:
        sendHeightMapSceneRequest(); break;
      case drc::data_request_t::HEIGHT_MAP_CORRIDOR:
        sendHeightMapCorridorRequest(); break;
      case drc::data_request_t::HEIGHT_MAP_COARSE:
        sendHeightMapCoarseRequest(); break;
      case drc::data_request_t::HEIGHT_MAP_DENSE:
        sendHeightMapDenseRequest(); break;
      case drc::data_request_t::DEPTH_MAP_SCENE:
        sendDepthMapSceneRequest(); break;
      case drc::data_request_t::DEPTH_MAP_WORKSPACE_C:
        sendDepthMapWorkspaceRequestCenter(); break;
      case drc::data_request_t::DEPTH_MAP_WORKSPACE_L:
        sendDepthMapWorkspaceRequestLeft(); break;
      case drc::data_request_t::DEPTH_MAP_WORKSPACE_R:
        sendDepthMapWorkspaceRequestRight(); break;
      case drc::data_request_t::DENSE_CLOUD_LHAND:
        sendDenseCloudLeftHandRequest(); break;
      case drc::data_request_t::DENSE_CLOUD_RHAND:
        sendDenseCloudRightHandRequest(); break;
      case drc::data_request_t::SCANS_HALF_SWEEP:
        sendScansHalfSweep(); break;
      case drc::data_request_t::TERRAIN_COST:
        sendTerrainCostRequest(); break;
      case drc::data_request_t::FUSED_DEPTH:
        sendFusedDepthRequest(); break;
      case drc::data_request_t::FUSED_HEIGHT:
        sendFusedHeightRequest(); break;
      case drc::data_request_t::STEREO_HEIGHT:
        sendStereoHeightRequest(); break;
      default:
        cout << "Unknown request type" << endl; break;
      }

      // wait for timer expiry
      // period is in tenths of seconds, so conversion to milli = x100
      ++numCycles;
      int millis = mRequest.period*100;
      std::this_thread::sleep_for(std::chrono::milliseconds(millis));
    }
  }

  void sendCameraImageRequest() {
    mLcm->publish("TRIGGER_CAMERA", &mRequest);
    cout << "Sent camera image request" << endl;
  }

  void sendMinimalRobotStateRequest() {
    mLcm->publish("TRIGGER_STATE", &mRequest);
    cout << "Sent minimal robot state request" << endl;
  }

/*
  void sendAffordanceListRequest() {
    std::vector<affordance::AffPlusPtr> affordances;
    mAffordanceWrapper->getAllAffordancesPlus(affordances);
    drc::affordance_plus_collection_t msg;
    msg.name = "updateFromDataRequestServer";
    msg.utime = drc::Clock::instance()->getCurrentTime();
    msg.map_id = -1;
    msg.naffs = affordances.size();
    for (int i = 0; i < affordances.size(); ++i) {
      drc::affordance_plus_t affPlus;
      affPlus.npoints = affPlus.ntriangles = 0;
      affordances[i]->aff->toMsg(&affPlus.aff);
      affPlus.aff.aff_store_control = drc::affordance_t::UPDATE;
      msg.affs_plus.push_back(affPlus);
    }
    mLcm->publish(affordance::AffordanceServer::
                  AFFORDANCE_PLUS_BASE_OVERWRITE_CHANNEL, &msg);
    cout << "Sent affordance list" << endl;
  }
*/

  void sendMapViewCatalogRequest() {
    mLcm->publish("TRIGGER_MAP_CATALOG", &mRequest);
    cout << "Sent map catalog" << endl;
  }

  void sendOctreeSceneRequest() {
    drc::map_request_t msg = prepareRequestMessage();
    msg.view_id = drc::data_request_t::OCTREE_SCENE;
    mLcm->publish("MAP_REQUEST", &msg);
  }

  void sendOctreeWorkspaceRequest() {
    drc::map_request_t msg = prepareRequestMessage();
    msg.map_id = 1; // 1 means use SCAN_FREE | 2 or 3 means use SCAN
    msg.view_id = drc::data_request_t::OCTREE_WORKSPACE;
    msg.resolution = 0.01;
    msg.time_min = 0;
    msg.time_max = 180;
    msg.time_mode = drc::map_request_t::ROLL_ANGLE_ABSOLUTE;
    msg.clip_planes[0][3] = 0.25;
    msg.clip_planes[1][3] = 2;
    msg.clip_planes[2][3] = 1;
    msg.clip_planes[3][3] = 1;
    msg.clip_planes[4][3] = 3;
    msg.clip_planes[5][3] = 0.3;
    mLcm->publish("MAP_REQUEST", &msg);
  }

  void sendCloudSceneRequest() {
    drc::map_request_t msg = prepareRequestMessage();
    msg.type = drc::map_request_t::POINT_CLOUD;
    msg.view_id = drc::data_request_t::CLOUD_SCENE;
    mLcm->publish("MAP_REQUEST", &msg);
  }

  void sendCloudWorkspaceRequest() {
    drc::map_request_t msg = prepareRequestMessage();
    msg.type = drc::map_request_t::POINT_CLOUD;
    msg.map_id = 2;<commit_msg>change octree workspace request to be time based<commit_after>#include <iostream>
#include <memory>
#include <thread>
#include <chrono>
#include <unordered_map>

#include <Eigen/Geometry>

#include <lcm/lcm-cpp.hpp>
#include <lcmtypes/drc/data_request_list_t.hpp>
#include <lcmtypes/drc/map_request_t.hpp>
#include <lcmtypes/drc/shaper_data_request_t.hpp>

#include <ConciseArgs>
#include <drc_utils/Clock.hpp>
#include <drc_utils/BotWrapper.hpp>

#include "RobotState.hpp"

using namespace std;

struct Worker {
  typedef std::shared_ptr<Worker> Ptr;

  bool mActive;
  drc::data_request_t mRequest;
  std::shared_ptr<lcm::LCM> mLcm;
  std::shared_ptr<drc::BotWrapper> mBotWrapper;
  std::shared_ptr<maps::RobotState> mRobotState;
  std::thread mThread;

  ~Worker() {
    stop();
  }

  void start() {
    if (mActive) return;
    if (mThread.joinable()) mThread.join();
    mActive = true;
    mThread = std::thread(std::ref(*this));
  }

  void stop() {
    mActive = false;
    if (mThread.joinable()) mThread.join();
  }

  void operator()() {
    mActive = true;
    int numCycles = 0;
    while (mActive) {

      // see if this is a cancel request
      if ((mRequest.period<0) || ((mRequest.period == 0) && (numCycles>0))) {
        mActive = false;
        break;
      }

      // dispatch
      switch(mRequest.type) {
      case drc::data_request_t::CAMERA_IMAGE_HEAD_LEFT:
      case drc::data_request_t::CAMERA_IMAGE_HEAD_RIGHT:
      case drc::data_request_t::CAMERA_IMAGE_LHAND:
      case drc::data_request_t::CAMERA_IMAGE_RHAND:
      case drc::data_request_t::CAMERA_IMAGE_LCHEST:
      case drc::data_request_t::CAMERA_IMAGE_RCHEST:
        sendCameraImageRequest(); break;
      case drc::data_request_t::MINIMAL_ROBOT_STATE:
        sendMinimalRobotStateRequest(); break;
//      case drc::data_request_t::AFFORDANCE_LIST:
  //      sendAffordanceListRequest(); break;
      case drc::data_request_t::MAP_CATALOG:
        sendMapViewCatalogRequest(); break;
      case drc::data_request_t::OCTREE_SCENE:
        sendOctreeSceneRequest(); break;
      case drc::data_request_t::OCTREE_WORKSPACE:
        sendOctreeWorkspaceRequest(); break;
      case drc::data_request_t::CLOUD_SCENE:
        sendCloudSceneRequest(); break;
      case drc::data_request_t::CLOUD_WORKSPACE:
        sendCloudWorkspaceRequest(); break;
      case drc::data_request_t::HEIGHT_MAP_SCENE:
        sendHeightMapSceneRequest(); break;
      case drc::data_request_t::HEIGHT_MAP_CORRIDOR:
        sendHeightMapCorridorRequest(); break;
      case drc::data_request_t::HEIGHT_MAP_COARSE:
        sendHeightMapCoarseRequest(); break;
      case drc::data_request_t::HEIGHT_MAP_DENSE:
        sendHeightMapDenseRequest(); break;
      case drc::data_request_t::DEPTH_MAP_SCENE:
        sendDepthMapSceneRequest(); break;
      case drc::data_request_t::DEPTH_MAP_WORKSPACE_C:
        sendDepthMapWorkspaceRequestCenter(); break;
      case drc::data_request_t::DEPTH_MAP_WORKSPACE_L:
        sendDepthMapWorkspaceRequestLeft(); break;
      case drc::data_request_t::DEPTH_MAP_WORKSPACE_R:
        sendDepthMapWorkspaceRequestRight(); break;
      case drc::data_request_t::DENSE_CLOUD_LHAND:
        sendDenseCloudLeftHandRequest(); break;
      case drc::data_request_t::DENSE_CLOUD_RHAND:
        sendDenseCloudRightHandRequest(); break;
      case drc::data_request_t::SCANS_HALF_SWEEP:
        sendScansHalfSweep(); break;
      case drc::data_request_t::TERRAIN_COST:
        sendTerrainCostRequest(); break;
      case drc::data_request_t::FUSED_DEPTH:
        sendFusedDepthRequest(); break;
      case drc::data_request_t::FUSED_HEIGHT:
        sendFusedHeightRequest(); break;
      case drc::data_request_t::STEREO_HEIGHT:
        sendStereoHeightRequest(); break;
      default:
        cout << "Unknown request type" << endl; break;
      }

      // wait for timer expiry
      // period is in tenths of seconds, so conversion to milli = x100
      ++numCycles;
      int millis = mRequest.period*100;
      std::this_thread::sleep_for(std::chrono::milliseconds(millis));
    }
  }

  void sendCameraImageRequest() {
    mLcm->publish("TRIGGER_CAMERA", &mRequest);
    cout << "Sent camera image request" << endl;
  }

  void sendMinimalRobotStateRequest() {
    mLcm->publish("TRIGGER_STATE", &mRequest);
    cout << "Sent minimal robot state request" << endl;
  }

/*
  void sendAffordanceListRequest() {
    std::vector<affordance::AffPlusPtr> affordances;
    mAffordanceWrapper->getAllAffordancesPlus(affordances);
    drc::affordance_plus_collection_t msg;
    msg.name = "updateFromDataRequestServer";
    msg.utime = drc::Clock::instance()->getCurrentTime();
    msg.map_id = -1;
    msg.naffs = affordances.size();
    for (int i = 0; i < affordances.size(); ++i) {
      drc::affordance_plus_t affPlus;
      affPlus.npoints = affPlus.ntriangles = 0;
      affordances[i]->aff->toMsg(&affPlus.aff);
      affPlus.aff.aff_store_control = drc::affordance_t::UPDATE;
      msg.affs_plus.push_back(affPlus);
    }
    mLcm->publish(affordance::AffordanceServer::
                  AFFORDANCE_PLUS_BASE_OVERWRITE_CHANNEL, &msg);
    cout << "Sent affordance list" << endl;
  }
*/

  void sendMapViewCatalogRequest() {
    mLcm->publish("TRIGGER_MAP_CATALOG", &mRequest);
    cout << "Sent map catalog" << endl;
  }

  void sendOctreeSceneRequest() {
    drc::map_request_t msg = prepareRequestMessage();
    msg.view_id = drc::data_request_t::OCTREE_SCENE;
    mLcm->publish("MAP_REQUEST", &msg);
  }

  void sendOctreeWorkspaceRequest() {
    drc::map_request_t msg = prepareRequestMessage();
    msg.map_id = 1; // 1 means use SCAN_FREE | 2 or 3 means use SCAN
    msg.view_id = drc::data_request_t::OCTREE_WORKSPACE;
    msg.resolution = 0.01;
    // Mode recently changed to time history
    //msg.time_min = 0;
    //msg.time_max = 180;
    //msg.time_mode = drc::map_request_t::ROLL_ANGLE_ABSOLUTE;
    msg.clip_planes[0][3] = 0.25;
    msg.clip_planes[1][3] = 2;
    msg.clip_planes[2][3] = 1;
    msg.clip_planes[3][3] = 1;
    msg.clip_planes[4][3] = 3;
    msg.clip_planes[5][3] = 0.3;
    mLcm->publish("MAP_REQUEST", &msg);
  }

  void sendCloudSceneRequest() {
    drc::map_request_t msg = prepareRequestMessage();
    msg.type = drc::map_request_t::POINT_CLOUD;
    msg.view_id = drc::data_request_t::CLOUD_SCENE;
    mLcm->publish("MAP_REQUEST", &msg);
  }

  void sendCloudWorkspaceRequest() {
    drc::map_request_t msg = prepareRequestMessage();
    msg.type = drc::map_request_t::POINT_CLOUD;
    msg.map_id = 2;
{'fraction_non_alphanumeric': 0.12576875768757687, 'fraction_numerical': 0.007072570725707257, 'mean_word_length': 3.8559163867114594, 'pattern_counts': {'":': 0, '<': 83, '<?xml version=': 0, '>': 63, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3746240', 'n_tokens_mistral': 4713, 'n_tokens_neox': 4591, 'n_words': 738}