<commit_before>#include <julia.h>
#include "Values.h"
#include "lvalue.h"

#include <iostream>

using namespace std;

template <typename V,typename E> static shared_ptr<nj::Value> reboxArray(jl_value_t *jlarray)
{
   shared_ptr<nj::Value> value;
   V *p = (V*)jl_array_data(jlarray);
   int ndims = jl_array_ndims(jlarray);
   vector<int> dims;

   for(int dim = 0;dim < ndims;dim++) dims.push_back(jl_array_dim(jlarray,dim));

   nj::Array<V,E> *array = new nj::Array<V,E>(dims);

printf("In rebox array: dim = %lu\n",dims.size());

   value.reset(array);
   memcpy(array->ptr(),p,array->size()*sizeof(V));
   return value;
}

static shared_ptr<nj::Value> getArrayValue(jl_value_t *jlarray)
{
   shared_ptr<nj::Value> value;
   jl_value_t *elementType = jl_tparam0(jl_typeof(jlarray));

printf("In getArray Value\n");

   if(jl_is_int64(elementType)) value = reboxArray<int64_t,nj::Int64_t>(jlarray);
   else if(jl_is_int32(elementType)) value = reboxArray<int,nj::Int32_t>(jlarray);
   else if(jl_is_float64(elementType)) value = reboxArray<double,nj::Float64_t>(jlarray);
   else if(jl_is_float32(elementType)) value = reboxArray<float,nj::Float32_t>(jlarray);
   else if(jl_is_uint64(elementType)) value = reboxArray<uint64_t,nj::UInt64_t>(jlarray);
   else if(jl_is_uint32(elementType)) value = reboxArray<unsigned int,nj::UInt32_t>(jlarray);
   else if(jl_is_int8(elementType)) value = reboxArray<char,nj::Char_t>(jlarray);
   else if(jl_is_int16(elementType)) value = reboxArray<short,nj::Int16_t>(jlarray);
   else if(jl_is_uint8(elementType)) value = reboxArray<unsigned char,nj::UChar_t>(jlarray);
   else if(jl_is_uint16(elementType)) value = reboxArray<unsigned short,nj::UInt16_t>(jlarray);

   return value;
}

vector<shared_ptr<nj::Value>> nj::lvalue(jl_value_t *jlvalue)
{
   vector<shared_ptr<nj::Value>> res;

   if(jl_is_null(jlvalue))
   {
      shared_ptr<nj::Value>  value(new nj::Null);
      res.push_back(value);
   }
   else if(jl_is_array(jlvalue))
   {
      res.push_back(getArrayValue(jlvalue));
   }
   else
   {
      shared_ptr<nj::Value> value;

      if(jl_is_int64(jlvalue)) value.reset(new nj::Int64(jl_unbox_int64(jlvalue)));
      else if(jl_is_int32(jlvalue)) value.reset(new nj::Int32(jl_unbox_int32(jlvalue)));
      else if(jl_is_float64(jlvalue)) value.reset(new nj::Float64(jl_unbox_float64(jlvalue)));
      else if(jl_is_float32(jlvalue)) value.reset(new nj::Float32(jl_unbox_float32(jlvalue)));
      else if(jl_is_uint64(jlvalue)) value.reset(new nj::UInt64(jl_unbox_uint64(jlvalue)));
      else if(jl_is_uint32(jlvalue)) value.reset(new nj::UInt32(jl_unbox_uint32(jlvalue)));
      else if(jl_is_int8(jlvalue)) value.reset(new nj::Char(jl_unbox_int8(jlvalue)));
      else if(jl_is_int16(jlvalue)) value.reset(new nj::Int16(jl_unbox_int16(jlvalue)));
      else if(jl_is_uint8(jlvalue)) value.reset(new nj::UChar(jl_unbox_uint8(jlvalue)));
      else if(jl_is_uint16(jlvalue)) value.reset(new nj::UInt16(jl_unbox_uint16(jlvalue)));
      else if(jl_is_ascii_string(jlvalue)) value.reset(new nj::String((char*)jl_unbox_voidpointer(jlvalue)));

      if(value.get()) res.push_back(value);
   }
   return res;
}
<commit_msg>More debugging Changed check for array element type<commit_after>#include <julia.h>
#include "Values.h"
#include "lvalue.h"

#include <iostream>

using namespace std;

template <typename V,typename E> static shared_ptr<nj::Value> reboxArray(jl_value_t *jlarray)
{
   shared_ptr<nj::Value> value;
   V *p = (V*)jl_array_data(jlarray);
   int ndims = jl_array_ndims(jlarray);
   vector<int> dims;

   for(int dim = 0;dim < ndims;dim++) dims.push_back(jl_array_dim(jlarray,dim));

   nj::Array<V,E> *array = new nj::Array<V,E>(dims);

printf("In rebox array: dim = %lu\n",dims.size());

   value.reset(array);
   memcpy(array->ptr(),p,array->size()*sizeof(V));
   return value;
}

static shared_ptr<nj::Value> getArrayValue(jl_value_t *jlarray)
{
   shared_ptr<nj::Value> value;
   jl_value_t *elementType = jl_tparam0(jl_typeof(jlarray));

printf("In getArray Value\n");

   if(jl_is_int64(elementType)) value = reboxArray<int64_t,nj::Int64_t>(jlarray);
   else if(jl_is_int32(elementType)) value = reboxArray<int,nj::Int32_t>(jlarray);
   else if(elementType == (jl_value_t*)jl_float64_type) value = reboxArray<double,nj::Float64_t>(jlarray);
   else if(jl_is_float32(elementType)) value = reboxArray<float,nj::Float32_t>(jlarray);
   else if(jl_is_uint64(elementType)) value = reboxArray<uint64_t,nj::UInt64_t>(jlarray);
   else if(jl_is_uint32(elementType)) value = reboxArray<unsigned int,nj::UInt32_t>(jlarray);
   else if(jl_is_int8(elementType)) value = reboxArray<char,nj::Char_t>(jlarray);
   else if(jl_is_int16(elementType)) value = reboxArray<short,nj::Int16_t>(jlarray);
   else if(jl_is_uint8(elementType)) value = reboxArray<unsigned char,nj::UChar_t>(jlarray);
   else if(jl_is_uint16(elementType)) value = reboxArray<unsigned short,nj::UInt16_t>(jlarray);

   return value;
}

vector<shared_ptr<nj::Value>> nj::lvalue(jl_value_t *jlvalue)
{
   vector<shared_ptr<nj::Value>> res;

   if(jl_is_null(jlvalue))
   {
      shared_ptr<nj::Value>  value(new nj::Null);
      res.push_back(value);
   }
   else if(jl_is_array(jlvalue))
   {
      res.push_back(getArrayValue(jlvalue));
   }
   else
   {
      shared_ptr<nj::Value> value;

      if(jl_is_int64(jlvalue)) value.reset(new nj::Int64(jl_unbox_int64(jlvalue)));
      else if(jl_is_int32(jlvalue)) value.reset(new nj::Int32(jl_unbox_int32(jlvalue)));
      else if(jl_is_float64(jlvalue)) value.reset(new nj::Float64(jl_unbox_float64(jlvalue)));
      else if(jl_is_float32(jlvalue)) value.reset(new nj::Float32(jl_unbox_float32(jlvalue)));
      else if(jl_is_uint64(jlvalue)) value.reset(new nj::UInt64(jl_unbox_uint64(jlvalue)));
      else if(jl_is_uint32(jlvalue)) value.reset(new nj::UInt32(jl_unbox_uint32(jlvalue)));
      else if(jl_is_int8(jlvalue)) value.reset(new nj::Char(jl_unbox_int8(jlvalue)));
      else if(jl_is_int16(jlvalue)) value.reset(new nj::Int16(jl_unbox_int16(jlvalue)));
      else if(jl_is_uint8(jlvalue)) value.reset(new nj::UChar(jl_unbox_uint8(jlvalue)));
      else if(jl_is_uint16(jlvalue)) value.reset(new nj::UInt16(jl_unbox_uint16(jlvalue)));
      else if(jl_is_ascii_string(jlvalue)) value.reset(new nj::String((char*)jl_unbox_voidpointer(jlvalue)));

      if(value.get()) res.push_back(value);
   }
   return res;
}
<|endoftext|>


<commit_before>/*
    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

        http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
*/

#include "ch.h"
#include "hal.h"

#include "analog.h"
#include "encoder.h"
#include "packet/frame.h"

#include "gitsha1.h"
#include "angle.h"
#include "blink.h"
#include "simplebicycle.h"
#include "usbconfig.h"
#include "saconfig.h"

#include "parameters.h"

#include <array>

namespace {
    /* sensors */
    Analog analog;
    Encoder encoder(sa::RLS_ENC, sa::RLS_ENC_INDEX_CFG);

    struct __attribute__((__packed__)) pose_t {
        float x; /* m */
        float y; /* m */
        float pitch; /* rad */
        float yaw; /* rad */
        float roll; /* rad */
        float steer; /* rad */
        float v; /* m/s      * Wheel angle and radius are not considered here.
                             * Computation must occur during visualization */
        uint8_t timestamp;  /* Converted from system ticks to milliseconds
                             * and contains only the least significant bits */
    }; /* 29 bytes */

    pose_t pose = {};

    using namespace packet::frame;
    std::array<uint8_t, sizeof(pose_t) + PACKET_OVERHEAD> packet_buffer;
} // namespace

/*
 * Application entry point.
 */
int main(void) {

    /*
     * System initializations.
     * - HAL initialization, this also initializes the configured device drivers
     *   and performs the board-specific initializations.
     * - Kernel initialization, the main() function becomes a thread and the
     *   RTOS is active.
     */
    halInit();
    chSysInit();

    /*
     * Initialize a serial-over-USB CDC driver.
     */
    sduObjectInit(&SDU1);
    sduStart(&SDU1, &serusbcfg);

    /*
     * Activate the USB driver and then the USB bus pull-up on D+.
     * Note, a delay is inserted in order to not have to disconnect the cable
     * after a reset.
     */
    board_usb_lld_disconnect_bus();   //usbDisconnectBus(serusbcfg.usbp);
    chThdSleepMilliseconds(1500);
    usbStart(serusbcfg.usbp, &usbcfg);
    board_usb_lld_connect_bus();      //usbConnectBus(serusbcfg.usbp);

    /* create the blink thread and print state monitor */
    chBlinkThreadCreateStatic();

    /*
     * Start sensors.
     * Encoder:
     *   Initialize encoder driver 5 on pins PA0, PA1 (EXT2-4, EXT2-8).
     */
    palSetLineMode(LINE_TIM5_CH1, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    palSetLineMode(LINE_TIM5_CH2, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    encoder.start();
    analog.start(1000); /* trigger ADC conversion at 1 kHz */

    /*
     * Set torque measurement enable line low.
     * The output of the Kistler torque sensor is not valid until after a falling edge
     * on the measurement line and it is held low. The 'LINE_TORQUE_MEAS_EN' line is
     * reversed due to NPN switch Q1.
     */
    palClearLine(LINE_TORQUE_MEAS_EN);
    chThdSleepMilliseconds(1);
    palSetLine(LINE_TORQUE_MEAS_EN);

    /*
     * Start DAC1 driver and set output pin as analog as suggested in Reference Manual.
     * The default line configuration is OUTPUT_OPENDRAIN_PULLUP  for SPI1_ENC1_NSS
     * and must be changed to use as analog output.
     */
    palSetLineMode(LINE_KOLLM_ACTL_TORQUE, PAL_MODE_INPUT_ANALOG);
    dacStart(sa::KOLLM_DAC, sa::KOLLM_DAC_CFG);

    /*
     * Initialize bicycle with default parameters, dt = 0.005 s, v = 5 m/s.
     */
    model::SimpleBicycle bicycle;

    /* transmit git sha information, block until receiver is ready */
    uint8_t bytes_written = packet::frame::stuff(g_GITSHA1, packet_buffer.data(), 7);
    while ((SDU1.config->usbp->state != USB_ACTIVE) || (SDU1.state != SDU_READY)) {
        chThdSleepMilliseconds(10);
    }
    streamWrite(&SDU1, packet_buffer.data(), bytes_written);

    /*
     * Normal main() thread activity, in this demo it simulates the bicycle
     * dynamics in real-time (roughly).
     */
    while (true) {
        systime_t starttime = chVTGetSystemTime();

        constexpr float roll_torque = 0.0f;

        /* get sensor measurements */
        float steer_torque = static_cast<float>(analog.get_adc12()*2.0f*sa::MAX_KISTLER_TORQUE/4096 -
                sa::MAX_KISTLER_TORQUE);
        float motor_torque = static_cast<float>(
                analog.get_adc13()*2.0f*sa::MAX_KOLLMORGEN_TORQUE/4096 -
                sa::MAX_KOLLMORGEN_TORQUE);
        float steer_angle = angle::encoder_count<float>(encoder);

        (void)motor_torque; /* remove build warning for unused variable */

        /* yaw angle, just use previous state value */
        float yaw_angle = angle::wrap(bicycle.pose().yaw);

        /* simulate bicycle */
        bicycle.update(roll_torque, steer_torque, yaw_angle, steer_angle);

        /* generate an example torque output for testing */
        float feedback_torque = bicycle.handlebar_feedback_torque();
        dacsample_t aout = static_cast<dacsample_t>(
                (feedback_torque/21.0f * 2048) + 2048); /* reduce output to half of full range */
        dacPutChannelX(sa::KOLLM_DAC, 0, aout);

        pose = pose_t{}; /* reset pose to zero */
        pose.x = bicycle.pose().x;
        pose.y = bicycle.pose().y;
        pose.pitch = angle::wrap(bicycle.pose().pitch);
        pose.yaw = angle::wrap(bicycle.pose().yaw);
        pose.roll = angle::wrap(bicycle.pose().roll);
        pose.steer = angle::wrap(bicycle.pose().steer);
        pose.v = bicycle.v();
        pose.timestamp = static_cast<decltype(pose.timestamp)>(ST2MS(chVTGetSystemTime()));
        /*
         * Timing information
         * (replaced chVTGetSystemTIme() with chSysGetRealtimeCounterX() and two uint32_t timing related fields added to
         * pose_t)
         * - Debug mode with all ChibiOS debug configuration options enabled: computation ~3.1 ms, tx ~20 us
         * - Release mode: computation ~200 us, tx ~10 us
         *
         * TODO: Pose message should be transmitted asynchronously.
         * After starting transmission, the simulation should start to calculate pose for the next timestep.
         * This is currently not necessary given the observed timings.
         */
        bytes_written = packet::frame::stuff(&pose, packet_buffer.data(), sizeof(pose));
        streamWrite(&SDU1, packet_buffer.data(), bytes_written);

        systime_t dt = MS2ST(static_cast<systime_t>(1000*bicycle.dt()));
        systime_t sleeptime = dt + starttime - chVTGetSystemTime();
        if (sleeptime >= dt) {
            //chDbgAssert(false, "deadline missed");
            continue;
        }
        chThdSleep(sleeptime);
    }
}
<commit_msg>Change bicycle forward velocity to 3.0 m/s<commit_after>/*
    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

        http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
*/

#include "ch.h"
#include "hal.h"

#include "analog.h"
#include "encoder.h"
#include "packet/frame.h"

#include "gitsha1.h"
#include "angle.h"
#include "blink.h"
#include "simplebicycle.h"
#include "usbconfig.h"
#include "saconfig.h"

#include "parameters.h"

#include <array>

namespace {
    constexpr float v = 3.0;
    /* sensors */
    Analog analog;
    Encoder encoder(sa::RLS_ENC, sa::RLS_ENC_INDEX_CFG);

    struct __attribute__((__packed__)) pose_t {
        float x; /* m */
        float y; /* m */
        float pitch; /* rad */
        float yaw; /* rad */
        float roll; /* rad */
        float steer; /* rad */
        float v; /* m/s      * Wheel angle and radius are not considered here.
                             * Computation must occur during visualization */
        uint8_t timestamp;  /* Converted from system ticks to milliseconds
                             * and contains only the least significant bits */
    }; /* 29 bytes */

    pose_t pose = {};

    using namespace packet::frame;
    std::array<uint8_t, sizeof(pose_t) + PACKET_OVERHEAD> packet_buffer;
} // namespace

/*
 * Application entry point.
 */
int main(void) {

    /*
     * System initializations.
     * - HAL initialization, this also initializes the configured device drivers
     *   and performs the board-specific initializations.
     * - Kernel initialization, the main() function becomes a thread and the
     *   RTOS is active.
     */
    halInit();
    chSysInit();

    /*
     * Initialize a serial-over-USB CDC driver.
     */
    sduObjectInit(&SDU1);
    sduStart(&SDU1, &serusbcfg);

    /*
     * Activate the USB driver and then the USB bus pull-up on D+.
     * Note, a delay is inserted in order to not have to disconnect the cable
     * after a reset.
     */
    board_usb_lld_disconnect_bus();   //usbDisconnectBus(serusbcfg.usbp);
    chThdSleepMilliseconds(1500);
    usbStart(serusbcfg.usbp, &usbcfg);
    board_usb_lld_connect_bus();      //usbConnectBus(serusbcfg.usbp);

    /* create the blink thread and print state monitor */
    chBlinkThreadCreateStatic();

    /*
     * Start sensors.
     * Encoder:
     *   Initialize encoder driver 5 on pins PA0, PA1 (EXT2-4, EXT2-8).
     */
    palSetLineMode(LINE_TIM5_CH1, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    palSetLineMode(LINE_TIM5_CH2, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    encoder.start();
    analog.start(1000); /* trigger ADC conversion at 1 kHz */

    /*
     * Set torque measurement enable line low.
     * The output of the Kistler torque sensor is not valid until after a falling edge
     * on the measurement line and it is held low. The 'LINE_TORQUE_MEAS_EN' line is
     * reversed due to NPN switch Q1.
     */
    palClearLine(LINE_TORQUE_MEAS_EN);
    chThdSleepMilliseconds(1);
    palSetLine(LINE_TORQUE_MEAS_EN);

    /*
     * Start DAC1 driver and set output pin as analog as suggested in Reference Manual.
     * The default line configuration is OUTPUT_OPENDRAIN_PULLUP  for SPI1_ENC1_NSS
     * and must be changed to use as analog output.
     */
    palSetLineMode(LINE_KOLLM_ACTL_TORQUE, PAL_MODE_INPUT_ANALOG);
    dacStart(sa::KOLLM_DAC, sa::KOLLM_DAC_CFG);

    /*
     * Initialize bicycle with default parameters, dt = 0.005 s.
     */
    model::SimpleBicycle bicycle(v);

    /* transmit git sha information, block until receiver is ready */
    uint8_t bytes_written = packet::frame::stuff(g_GITSHA1, packet_buffer.data(), 7);
    while ((SDU1.config->usbp->state != USB_ACTIVE) || (SDU1.state != SDU_READY)) {
        chThdSleepMilliseconds(10);
    }
    streamWrite(&SDU1, packet_buffer.data(), bytes_written);

    /*
     * Normal main() thread activity, in this demo it simulates the bicycle
     * dynamics in real-time (roughly).
     */
    while (true) {
        systime_t starttime = chVTGetSystemTime();

        constexpr float roll_torque = 0.0f;

        /* get sensor measurements */
        float steer_torque = static_cast<float>(analog.get_adc12()*2.0f*sa::MAX_KISTLER_TORQUE/4096 -
                sa::MAX_KISTLER_TORQUE);
        float motor_torque = static_cast<float>(
                analog.get_adc13()*2.0f*sa::MAX_KOLLMORGEN_TORQUE/4096 -
                sa::MAX_KOLLMORGEN_TORQUE);
        float steer_angle = angle::encoder_count<float>(encoder);

        (void)motor_torque; /* remove build warning for unused variable */

        /* yaw angle, just use previous state value */
        float yaw_angle = angle::wrap(bicycle.pose().yaw);

        /* simulate bicycle */
        bicycle.update(roll_torque, steer_torque, yaw_angle, steer_angle);

        /* generate an example torque output for testing */
        float feedback_torque = bicycle.handlebar_feedback_torque();
        dacsample_t aout = static_cast<dacsample_t>(
                (feedback_torque/21.0f * 2048) + 2048); /* reduce output to half of full range */
        dacPutChannelX(sa::KOLLM_DAC, 0, aout);

        pose = pose_t{}; /* reset pose to zero */
        pose.x = bicycle.pose().x;
        pose.y = bicycle.pose().y;
        pose.pitch = angle::wrap(bicycle.pose().pitch);
        pose.yaw = angle::wrap(bicycle.pose().yaw);
        pose.roll = angle::wrap(bicycle.pose().roll);
        pose.steer = angle::wrap(bicycle.pose().steer);
        pose.v = bicycle.v();
        pose.timestamp = static_cast<decltype(pose.timestamp)>(ST2MS(chVTGetSystemTime()));
        /*
         * Timing information
         * (replaced chVTGetSystemTIme() with chSysGetRealtimeCounterX() and two uint32_t timing related fields added to
         * pose_t)
         * - Debug mode with all ChibiOS debug configuration options enabled: computation ~3.1 ms, tx ~20 us
         * - Release mode: computation ~200 us, tx ~10 us
         *
         * TODO: Pose message should be transmitted asynchronously.
         * After starting transmission, the simulation should start to calculate pose for the next timestep.
         * This is currently not necessary given the observed timings.
         */
        bytes_written = packet::frame::stuff(&pose, packet_buffer.data(), sizeof(pose));
        streamWrite(&SDU1, packet_buffer.data(), bytes_written);

        systime_t dt = MS2ST(static_cast<systime_t>(1000*bicycle.dt()));
        systime_t sleeptime = dt + starttime - chVTGetSystemTime();
        if (sleeptime >= dt) {
            //chDbgAssert(false, "deadline missed");
            continue;
        }
        chThdSleep(sleeptime);
    }
}
<|endoftext|>


<commit_before>#include <GDE/Core/Log.hpp>

namespace GDE
{

bool Log::initialized = false;
time_t Log::rawtime;
std::string Log::logFileName;

    void Log::init(){
        if(!initialized){
            logFileName = "log.txt";
            initialized = true;
            std::cout << "Sistema de Log inicializado" << std::endl;
            Log::info("init", "Sistema de Log inicializado");
        }
    }

    void Log::info(std::string tag, std::string text)
    {
        if (!initialized)
        {
            std::cout << "El sistema de log no ha sido inicializado, por favor, inicielo mediante GDE::Log::init()" << std::endl;
            return;
        }
        std::ofstream logFile(logFileName.c_str(), std::ofstream::app);
        time (&rawtime);
        char buffer[20];
        struct tm * timeinfo;
        timeinfo = localtime(&rawtime);
        strftime(buffer, 20, "%d/%m/%y %X ", timeinfo);
        logFile << buffer << "INFO: " << tag << ": " << text << std::endl;
        logFile.close();
    }

    void Log::debug(std::string tag, std::string text)
    {
        if (!initialized)
        {
            std::cout << "El sistema de log no ha sido inicializado, por favor, inicielo mediante GDE::Log::init()" << std::endl;
            return;
        }
        std::ofstream logFile(logFileName.c_str(), std::ofstream::app);
        time (&rawtime);
        char buffer [20];
        struct tm * timeinfo;
        timeinfo = localtime (&rawtime);
        strftime (buffer,20,"%d/%m/%y %X ",timeinfo);
        logFile << buffer << "DEBUG: " << tag << ": " << text << std::endl;
        logFile.close();
    }

    void Log::error(std::string tag, std::string text)
    {
        if (!initialized)
        {
            std::cout << "El sistema de log no ha sido inicializado, por favor, inicielo mediante GDE::Log::init()" << std::endl;
            return;
        }
        std::ofstream logFile(logFileName.c_str(), std::ofstream::app);
        time (&rawtime);
        char buffer [20];
        struct tm * timeinfo;
        timeinfo = localtime (&rawtime);
        strftime (buffer,20,"%d/%m/%y %X ",timeinfo);
        logFile << buffer << "ERROR: " << tag << ": " << text << std::endl;
        logFile.close();
    }
}
<commit_msg>Fix de estilo<commit_after>#include <GDE/Core/Log.hpp>

namespace GDE
{

bool Log::initialized = false;
time_t Log::rawtime;
std::string Log::logFileName;

    void Log::init(){
        if(!initialized)
		{
            logFileName = "log.txt";
            initialized = true;
            std::cout << "Sistema de Log inicializado" << std::endl;
            Log::info("init", "Sistema de Log inicializado");
        }
    }

    void Log::info(std::string tag, std::string text)
    {
        if (!initialized)
        {
            std::cout << "El sistema de log no ha sido inicializado, por favor, inicielo mediante GDE::Log::init()" << std::endl;
            return;
        }
        std::ofstream logFile(logFileName.c_str(), std::ofstream::app);
        time (&rawtime);
        char buffer[20];
        struct tm * timeinfo;
        timeinfo = localtime(&rawtime);
        strftime(buffer, 20, "%d/%m/%y %X ", timeinfo);
        logFile << buffer << "INFO: " << tag << ": " << text << std::endl;
        logFile.close();
    }

    void Log::debug(std::string tag, std::string text)
    {
        if (!initialized)
        {
            std::cout << "El sistema de log no ha sido inicializado, por favor, inicielo mediante GDE::Log::init()" << std::endl;
            return;
        }
        std::ofstream logFile(logFileName.c_str(), std::ofstream::app);
        time (&rawtime);
        char buffer [20];
        struct tm * timeinfo;
        timeinfo = localtime (&rawtime);
        strftime (buffer,20,"%d/%m/%y %X ",timeinfo);
        logFile << buffer << "DEBUG: " << tag << ": " << text << std::endl;
        logFile.close();
    }

    void Log::error(std::string tag, std::string text)
    {
        if (!initialized)
        {
            std::cout << "El sistema de log no ha sido inicializado, por favor, inicielo mediante GDE::Log::init()" << std::endl;
            return;
        }
        std::ofstream logFile(logFileName.c_str(), std::ofstream::app);
        time (&rawtime);
        char buffer [20];
        struct tm * timeinfo;
        timeinfo = localtime (&rawtime);
        strftime (buffer,20,"%d/%m/%y %X ",timeinfo);
        logFile << buffer << "ERROR: " << tag << ": " << text << std::endl;
        logFile.close();
    }
}
<|endoftext|>


<commit_before>#include "Globals.h"  // NOTE: MSVC stupidness requires this to be the same across all modules

#include "GroupManager.h"
#include "Group.h"
#include "inifile/iniFile.h"
#include "ChatColor.h"
#include "Root.h"

typedef std::map< AString, cGroup* > GroupMap;

struct cGroupManager::sGroupManagerState
{
	GroupMap Groups;
};

cGroupManager::~cGroupManager()
{
	for (GroupMap::iterator itr = m_pState->Groups.begin(); itr != m_pState->Groups.end(); ++itr)
	{
		delete itr->second;
		itr->second = NULL;
	}
	m_pState->Groups.clear();

	delete m_pState;
	m_pState = NULL;
}

cGroupManager::cGroupManager()
	: m_pState( new sGroupManagerState)
{
	LOGD("-- Loading Groups --");

	if (!LoadGroups())
	{
		LOGWARNING("ERROR: Groups could not load!");
	}
	if (!CheckUsers())
	{
		LOGWARNING("ERROR: User file could not be found!");
	}

	LOGD("-- Groups Successfully Loaded --");
}

void cGroupManager::GenerateDefaultUsersIni(cIniFile & a_IniFile)
{
	LOGWARN("Regenerating users.ini, all users will be reset");
	a_IniFile.AddHeaderComment(" This file stores the players' groups.");
	a_IniFile.AddHeaderComment(" The format is:");
	a_IniFile.AddHeaderComment(" [PlayerName]");
	a_IniFile.AddHeaderComment(" Groups = GroupName1, GroupName2, ...");

	a_IniFile.WriteFile("users.ini");
}

bool cGroupManager::CheckUsers()
{
	cIniFile IniFile;
	if (!IniFile.ReadFile("users.ini"))
	{
		GenerateDefaultUsersIni(IniFile);
		return true;
	}

	int NumKeys = IniFile.GetNumKeys();
	for (int i = 0; i < NumKeys; i++)
	{
		AString Player = IniFile.GetKeyName(i);
		AString Groups = IniFile.GetValue(Player, "Groups", "");
		if (Groups.empty())
		{
			continue;
		}
		AStringVector Split = StringSplitAndTrim(Groups, ",");
		for (AStringVector::const_iterator itr = Split.begin(), end = Split.end(); itr != end; ++itr)
		{
			if (!ExistsGroup(*itr))
			{
				LOGWARNING("The group %s for player %s was not found!", Split[i].c_str(), Player.c_str());
			}
		}  // for itr - Split[]
	}  // for i - ini file keys
	// Always return true for now, just but we can handle writefile fails later.
	return true;
}

bool cGroupManager::LoadGroups()
{
	cIniFile IniFile;
	if (!IniFile.ReadFile("groups.ini"))
	{
		LOGWARNING("Regenerating groups.ini, all groups will be reset");
		IniFile.AddHeaderComment(" This is the MCServer permissions manager groups file");
		IniFile.AddHeaderComment(" It stores all defined groups such as Administrators, Players, or Moderators");

		IniFile.SetValue("Owner", "Permissions", "*", true);
		IniFile.SetValue("Owner", "Color", "2", true);

		IniFile.SetValue("Moderator", "Permissions", "core.time, core.item, core.tpa, core.tpaccept, core.ban, core.unban, core.save-all, core.toggledownfall");
		IniFile.SetValue("Moderator", "Color", "2", true);
		IniFile.SetValue("Moderator", "Inherits", "Player", true);

		IniFile.SetValue("Player", "Permissions", "core.portal", true);
		IniFile.SetValue("Player", "Color", "f", true);
		IniFile.SetValue("Player", "Inherits", "Default", true);

		IniFile.SetValue("Default", "Permissions", "core.help, core.plugins, core.spawn, core.worlds, core.back, core.motd, core.build, core.locate, core.viewdistance", true);
		IniFile.SetValue("Default", "Color", "f", true);

		IniFile.WriteFile("groups.ini");
	}

	int NumKeys = IniFile.GetNumKeys();
	for (int i = 0; i < NumKeys; i++)
	{
		AString KeyName = IniFile.GetKeyName(i);
		cGroup * Group = GetGroup(KeyName.c_str());

		Group->ClearPermission();  // Needed in case the groups are reloaded.

		LOGD("Loading group %s", KeyName.c_str());

		Group->SetName(KeyName);
		AString Color = IniFile.GetValue(KeyName, "Color", "-");
		if ((Color != "-") && (Color.length() >= 1))
		{
			Group->SetColor(cChatColor::Delimiter + AString(&Color[0]));
		}
		else
		{
			Group->SetColor(cChatColor::White);
		}

		AString Commands = IniFile.GetValue(KeyName, "Commands", "");
		if (!Commands.empty())
		{
			AStringVector Split = StringSplitAndTrim(Commands, ",");
			for (size_t i = 0; i < Split.size(); i++)
			{
				Group->AddCommand(Split[i]);
			}
		}

		AString Permissions = IniFile.GetValue(KeyName, "Permissions", "");
		if (!Permissions.empty())
		{
			AStringVector Split = StringSplitAndTrim(Permissions, ",");
			for (size_t i = 0; i < Split.size(); i++)
			{
				Group->AddPermission(Split[i]);
			}
		}

		AString Groups = IniFile.GetValue(KeyName, "Inherits", "");
		if (!Groups.empty())
		{
			AStringVector Split = StringSplitAndTrim(Groups, ",");
			for (size_t i = 0; i < Split.size(); i++)
			{
				Group->InheritFrom(GetGroup(Split[i].c_str()));
			}
		}
	}
	// Always return true, we can handle writefile fails later.
	return true;
}

bool cGroupManager::ExistsGroup( const AString & a_Name)
{
	GroupMap::iterator itr = m_pState->Groups.find( a_Name);
	return ( itr != m_pState->Groups.end());
}

cGroup* cGroupManager::GetGroup( const AString & a_Name)
{
	GroupMap::iterator itr = m_pState->Groups.find( a_Name);
	if (itr != m_pState->Groups.end())
	{
		return itr->second;
	}

	cGroup* Group = new cGroup();
	m_pState->Groups[a_Name] = Group;

	return Group;
}

<commit_msg>Use AString(1, Color[0])<commit_after>#include "Globals.h"  // NOTE: MSVC stupidness requires this to be the same across all modules

#include "GroupManager.h"
#include "Group.h"
#include "inifile/iniFile.h"
#include "ChatColor.h"
#include "Root.h"

typedef std::map< AString, cGroup* > GroupMap;

struct cGroupManager::sGroupManagerState
{
	GroupMap Groups;
};

cGroupManager::~cGroupManager()
{
	for (GroupMap::iterator itr = m_pState->Groups.begin(); itr != m_pState->Groups.end(); ++itr)
	{
		delete itr->second;
		itr->second = NULL;
	}
	m_pState->Groups.clear();

	delete m_pState;
	m_pState = NULL;
}

cGroupManager::cGroupManager()
	: m_pState( new sGroupManagerState)
{
	LOGD("-- Loading Groups --");

	if (!LoadGroups())
	{
		LOGWARNING("ERROR: Groups could not load!");
	}
	if (!CheckUsers())
	{
		LOGWARNING("ERROR: User file could not be found!");
	}

	LOGD("-- Groups Successfully Loaded --");
}

void cGroupManager::GenerateDefaultUsersIni(cIniFile & a_IniFile)
{
	LOGWARN("Regenerating users.ini, all users will be reset");
	a_IniFile.AddHeaderComment(" This file stores the players' groups.");
	a_IniFile.AddHeaderComment(" The format is:");
	a_IniFile.AddHeaderComment(" [PlayerName]");
	a_IniFile.AddHeaderComment(" Groups = GroupName1, GroupName2, ...");

	a_IniFile.WriteFile("users.ini");
}

bool cGroupManager::CheckUsers()
{
	cIniFile IniFile;
	if (!IniFile.ReadFile("users.ini"))
	{
		GenerateDefaultUsersIni(IniFile);
		return true;
	}

	int NumKeys = IniFile.GetNumKeys();
	for (int i = 0; i < NumKeys; i++)
	{
		AString Player = IniFile.GetKeyName(i);
		AString Groups = IniFile.GetValue(Player, "Groups", "");
		if (Groups.empty())
		{
			continue;
		}
		AStringVector Split = StringSplitAndTrim(Groups, ",");
		for (AStringVector::const_iterator itr = Split.begin(), end = Split.end(); itr != end; ++itr)
		{
			if (!ExistsGroup(*itr))
			{
				LOGWARNING("The group %s for player %s was not found!", Split[i].c_str(), Player.c_str());
			}
		}  // for itr - Split[]
	}  // for i - ini file keys
	// Always return true for now, just but we can handle writefile fails later.
	return true;
}

bool cGroupManager::LoadGroups()
{
	cIniFile IniFile;
	if (!IniFile.ReadFile("groups.ini"))
	{
		LOGWARNING("Regenerating groups.ini, all groups will be reset");
		IniFile.AddHeaderComment(" This is the MCServer permissions manager groups file");
		IniFile.AddHeaderComment(" It stores all defined groups such as Administrators, Players, or Moderators");

		IniFile.SetValue("Owner", "Permissions", "*", true);
		IniFile.SetValue("Owner", "Color", "2", true);

		IniFile.SetValue("Moderator", "Permissions", "core.time, core.item, core.tpa, core.tpaccept, core.ban, core.unban, core.save-all, core.toggledownfall");
		IniFile.SetValue("Moderator", "Color", "2", true);
		IniFile.SetValue("Moderator", "Inherits", "Player", true);

		IniFile.SetValue("Player", "Permissions", "core.portal", true);
		IniFile.SetValue("Player", "Color", "f", true);
		IniFile.SetValue("Player", "Inherits", "Default", true);

		IniFile.SetValue("Default", "Permissions", "core.help, core.plugins, core.spawn, core.worlds, core.back, core.motd, core.build, core.locate, core.viewdistance", true);
		IniFile.SetValue("Default", "Color", "f", true);

		IniFile.WriteFile("groups.ini");
	}

	int NumKeys = IniFile.GetNumKeys();
	for (int i = 0; i < NumKeys; i++)
	{
		AString KeyName = IniFile.GetKeyName(i);
		cGroup * Group = GetGroup(KeyName.c_str());

		Group->ClearPermission();  // Needed in case the groups are reloaded.

		LOGD("Loading group %s", KeyName.c_str());

		Group->SetName(KeyName);
		AString Color = IniFile.GetValue(KeyName, "Color", "-");
		if ((Color != "-") && (Color.length() >= 1))
		{
			Group->SetColor(cChatColor::Delimiter + AString(1, Color[0]));
		}
		else
		{
			Group->SetColor(cChatColor::White);
		}

		AString Commands = IniFile.GetValue(KeyName, "Commands", "");
		if (!Commands.empty())
		{
			AStringVector Split = StringSplitAndTrim(Commands, ",");
			for (size_t i = 0; i < Split.size(); i++)
			{
				Group->AddCommand(Split[i]);
			}
		}

		AString Permissions = IniFile.GetValue(KeyName, "Permissions", "");
		if (!Permissions.empty())
		{
			AStringVector Split = StringSplitAndTrim(Permissions, ",");
			for (size_t i = 0; i < Split.size(); i++)
			{
				Group->AddPermission(Split[i]);
			}
		}

		AString Groups = IniFile.GetValue(KeyName, "Inherits", "");
		if (!Groups.empty())
		{
			AStringVector Split = StringSplitAndTrim(Groups, ",");
			for (size_t i = 0; i < Split.size(); i++)
			{
				Group->InheritFrom(GetGroup(Split[i].c_str()));
			}
		}
	}
	// Always return true, we can handle writefile fails later.
	return true;
}

bool cGroupManager::ExistsGroup( const AString & a_Name)
{
	GroupMap::iterator itr = m_pState->Groups.find( a_Name);
	return ( itr != m_pState->Groups.end());
}

cGroup* cGroupManager::GetGroup( const AString & a_Name)
{
	GroupMap::iterator itr = m_pState->Groups.find( a_Name);
	if (itr != m_pState->Groups.end())
	{
		return itr->second;
	}

	cGroup* Group = new cGroup();
	m_pState->Groups[a_Name] = Group;

	return Group;
}

<|endoftext|>


<commit_before>#include "InputManager.h"

InputManager::InputManager()
{
  // TODO Make this map configurable / built from a file
  mKeyToEventMappings.insert(
      std::pair<int, int>(Key::Q, Event::Quit) );
  mKeyToEventMappings.insert(
      std::pair<int, int>(Key::Escape, Event::Quit) );

  mKeyToEventMappings.insert(
      std::pair<int, int>(Key::Left, Event::MoveLeft) );
  mKeyToEventMappings.insert(
      std::pair<int, int>(Key::Right, Event::MoveRight) );
  mKeyToEventMappings.insert(
      std::pair<int, int>(Key::Up, Event::MoveForward) );
  mKeyToEventMappings.insert(
      std::pair<int, int>(Key::Up, Event::MoveBack) );
}

InputManager::~InputManager()
{
  EventMapping_T::iterator it;

  for(it = mEventMappings.begin(); it != mEventMappings.end(); it++) {
    if(it->second) {
      delete it->second;
    }
  }

  mEventMappings.clear();
}

void InputManager::injectKeyDown(KeyboardEvent event)
{
  event.isDown = true;
  mEventMappings[ mKeyToEventMappings[event.key] ]->call(event);
}

void InputManager::injectKeyUp(KeyboardEvent event)
{
  event.isDown = false;
  mEventMappings[ mKeyToEventMappings[event.key] ]->call(event);
}

void InputManager::injectMouseDown()
{
}

void InputManager::injectMouseUp()
{
}

void InputManager::injectMouseMoved()
{
}

void InputManager::injectMouseDoubleClick()
{
}

void InputManager::injectMouseWheel()
{
}
<commit_msg>Ok, I have no idea how that was working. Some strange coincidence with the key codes I guess<commit_after>#include "InputManager.h"

InputManager::InputManager()
{
  // TODO Make this map configurable / built from a file
  mKeyToEventMappings.insert(
      std::pair<int, int>(Key::Q, Event::Quit) );
  mKeyToEventMappings.insert(
      std::pair<int, int>(Key::Escape, Event::Quit) );

  mKeyToEventMappings.insert(
      std::pair<int, int>(Key::Left, Event::MoveLeft) );
  mKeyToEventMappings.insert(
      std::pair<int, int>(Key::Right, Event::MoveRight) );
  mKeyToEventMappings.insert(
      std::pair<int, int>(Key::Up, Event::MoveForward) );
  mKeyToEventMappings.insert(
      std::pair<int, int>(Key::Down, Event::MoveBack) );
}

InputManager::~InputManager()
{
  EventMapping_T::iterator it;

  for(it = mEventMappings.begin(); it != mEventMappings.end(); it++) {
    if(it->second) {
      delete it->second;
    }
  }

  mEventMappings.clear();
}

void InputManager::injectKeyDown(KeyboardEvent event)
{
  event.isDown = true;
  mEventMappings[ mKeyToEventMappings[event.key] ]->call(event);
}

void InputManager::injectKeyUp(KeyboardEvent event)
{
  event.isDown = false;
  mEventMappings[ mKeyToEventMappings[event.key] ]->call(event);
}

void InputManager::injectMouseDown()
{
}

void InputManager::injectMouseUp()
{
}

void InputManager::injectMouseMoved()
{
}

void InputManager::injectMouseDoubleClick()
{
}

void InputManager::injectMouseWheel()
{
}
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: textconversion_zh.cxx,v $
 *
 *  $Revision: 1.5 $
 *
 *  last change: $Author: rt $ $Date: 2005-09-07 17:25:31 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

#include <assert.h>
#include <textconversion.hxx>
#include <com/sun/star/i18n/TextConversionType.hpp>
#include <com/sun/star/i18n/TextConversionOption.hpp>
#include <com/sun/star/linguistic2/ConversionDirection.hpp>
#include <com/sun/star/linguistic2/ConversionDictionaryType.hpp>
#include <i18nutil/x_rtl_ustring.h>

using namespace com::sun::star::lang;
using namespace com::sun::star::i18n;
using namespace com::sun::star::linguistic2;
using namespace com::sun::star::uno;
using namespace rtl;

namespace com { namespace sun { namespace star { namespace i18n {

// defined in stc_char.cxx and stc_word.cxx  generated from stc_char.dic and stc_word.dic
// by genconv_dict
extern const sal_uInt16* getSTC_CharIndex_S2T();
extern const sal_Unicode* getSTC_CharData_S2T();
extern const sal_uInt16* getSTC_CharIndex_S2V();
extern const sal_Unicode* getSTC_CharData_S2V();
extern const sal_uInt16* getSTC_CharIndex_T2S();
extern const sal_Unicode* getSTC_CharData_T2S();

extern const sal_uInt16* getSTC_WordIndex_S2T(sal_Int32 &count);
extern const sal_uInt16* getSTC_WordIndex_T2S(sal_Int32 &count);
extern const sal_uInt16* getSTC_WordEntry_S2T();
extern const sal_uInt16* getSTC_WordEntry_T2S();
extern const sal_Unicode* getSTC_WordData(sal_Int32 &count);

TextConversion_zh::TextConversion_zh( const Reference < XMultiServiceFactory >& xMSF )
{
    Reference < XInterface > xI;
    xI = xMSF->createInstance(
        OUString::createFromAscii( "com.sun.star.linguistic2.ConversionDictionaryList" ));
    if ( xI.is() )
        xI->queryInterface( getCppuType((const Reference< XConversionDictionaryList>*)0) ) >>= xCDL;

    implementationName = "com.sun.star.i18n.TextConversion_zh";
}

sal_Unicode SAL_CALL getOneCharConversion(sal_Unicode ch, const sal_Unicode* Data, const sal_uInt16* Index)
{
    sal_Unicode address = Index[ch>>8];
    if (address != 0xFFFF)
        address = Data[address + (ch & 0xFF)];
    return (address != 0xFFFF) ? address : ch;
}

OUString SAL_CALL getCharConversion(const OUString& aText, sal_Int32 nStartPos, sal_Int32 nLength, sal_Bool toSChinese, sal_Int32 nConversionOptions)
{
    const sal_Unicode *Data;
    const sal_uInt16 *Index;

    if (toSChinese) {
        Data = getSTC_CharData_T2S();
        Index = getSTC_CharIndex_T2S();
    } else if (nConversionOptions & TextConversionOption::USE_CHARACTER_VARIANTS) {
        Data = getSTC_CharData_S2V();
        Index = getSTC_CharIndex_S2V();
    } else {
        Data = getSTC_CharData_S2T();
        Index = getSTC_CharIndex_S2T();
    }

    rtl_uString * newStr = x_rtl_uString_new_WithLength( nLength ); // defined in x_rtl_ustring.h
    for (sal_Int32 i = 0; i < nLength; i++)
        newStr->buffer[i] =
            getOneCharConversion(aText[nStartPos+i], Data, Index);
    return OUString( newStr->buffer, nLength);
}

OUString SAL_CALL TextConversion_zh::getWordConversion(const OUString& aText, sal_Int32 nStartPos, sal_Int32 nLength, sal_Bool toSChinese, sal_Int32 nConversionOptions)
{
    sal_Int32 dictLen = 0;
    const sal_Unicode *wordData = getSTC_WordData(dictLen);
    sal_Int32 maxLen = 0;
    const sal_uInt16 *index;
    const sal_uInt16 *entry;
    const sal_Unicode *charData;
    const sal_uInt16 *charIndex;

    if (toSChinese) {
        index = getSTC_WordIndex_T2S(maxLen);
        entry = getSTC_WordEntry_T2S();
        charData = getSTC_CharData_T2S();
        charIndex = getSTC_CharIndex_T2S();
    } else {
        index = getSTC_WordIndex_S2T(maxLen);
        entry = getSTC_WordEntry_S2T();
        if (nConversionOptions & TextConversionOption::USE_CHARACTER_VARIANTS) {
            charData = getSTC_CharData_S2V();
            charIndex = getSTC_CharIndex_S2V();
        } else {
            charData = getSTC_CharData_S2T();
            charIndex = getSTC_CharIndex_S2T();
        }
    }

    if ((!wordData || !index || !entry) && !xCDL.is()) // no word mapping defined, do char2char conversion.
        return getCharConversion(aText, nStartPos, nLength, toSChinese, nConversionOptions);

    rtl_uString * newStr = x_rtl_uString_new_WithLength( nLength*2 ); // defined in x_rtl_ustring.h
    sal_Int32 currPos = 0, count = 0;
    while (currPos < nLength) {
        sal_Int32 len = nLength - currPos;
        sal_Bool found = sal_False;
        if (len > maxLen)
            len = maxLen;
        for (; len > 0 && ! found; len--) {
            OUString word = aText.copy(nStartPos + currPos, len);
            sal_Int32 current = 0;
            // user dictionary
            if (xCDL.is()) {
                Sequence < OUString > conversions;
                try {
                    conversions = xCDL->queryConversions(word, 0, len,
                            aLocale, ConversionDictionaryType::SCHINESE_TCHINESE,
                            /*toSChinese ?*/ ConversionDirection_FROM_LEFT /*: ConversionDirection_FROM_RIGHT*/,
                            nConversionOptions);
                }
                catch ( NoSupportException & ) {
                    // clear reference (when there is no user dictionary) in order
                    // to not always have to catch this exception again
                    // in further calls. (save time)
                    xCDL = 0;
                }
                catch (...) {
                    // catch all other exceptions to allow
                    // querying the system dictionary in the next line
                }
                if (conversions.getLength() > 0) {
                    while (current < conversions[0].getLength())
                        newStr->buffer[count++] = conversions[0][current++];
                    currPos += word.getLength();
                    found = sal_True;
                }
            }

            if (!found && index[len+1] - index[len] > 0) {
                sal_Int32 bottom = (sal_Int32) index[len];
                sal_Int32 top = (sal_Int32) index[len+1] - 1;

                while (bottom <= top && !found) {
                    current = (top + bottom) / 2;
                    const sal_Int32 result = word.compareTo(wordData + entry[current]);
                    if (result < 0)
                        top = current - 1;
                    else if (result > 0)
                        bottom = current + 1;
                    else {
                        if (toSChinese)   // Traditionary/Simplified conversion,
                            for (current = entry[current]-1; current > 0 && wordData[current-1]; current--);
                        else  // Simplified/Traditionary conversion, forwards search for next word
                            current = entry[current] + word.getLength() + 1;
                        while (wordData[current])
                            newStr->buffer[count++] = wordData[current++];
                        currPos += word.getLength();
                        found = sal_True;
                    }
                }
            }
        }
        if (!found) {
            newStr->buffer[count++] =
                getOneCharConversion(aText[nStartPos+currPos], charData, charIndex);
            currPos++;
        }
    }
    return OUString( newStr->buffer, count);
}

TextConversionResult SAL_CALL
TextConversion_zh::getConversions( const OUString& aText, sal_Int32 nStartPos, sal_Int32 nLength,
    const Locale& aLocale, sal_Int16 nConversionType, sal_Int32 nConversionOptions)
    throw(  RuntimeException, IllegalArgumentException, NoSupportException )
{
    TextConversionResult result;

    result.Candidates.realloc(1);
    result.Candidates[0] = getConversion( aText, nStartPos, nLength, aLocale, nConversionType, nConversionOptions);
    result.Boundary.startPos = nStartPos;
    result.Boundary.endPos = nStartPos + nLength;

    return result;
}

OUString SAL_CALL
TextConversion_zh::getConversion( const OUString& aText, sal_Int32 nStartPos, sal_Int32 nLength,
    const Locale& rLocale, sal_Int16 nConversionType, sal_Int32 nConversionOptions)
    throw(  RuntimeException, IllegalArgumentException, NoSupportException )
{
    if (rLocale.Language.equalsAscii("zh") &&
            ( nConversionType == TextConversionType::TO_SCHINESE ||
            nConversionType == TextConversionType::TO_TCHINESE) ) {

        aLocale=rLocale;
        sal_Bool toSChinese = nConversionType == TextConversionType::TO_SCHINESE;

        if (nConversionOptions & TextConversionOption::CHARACTER_BY_CHARACTER)
            // char to char dictionary
            return getCharConversion(aText, nStartPos, nLength, toSChinese, nConversionOptions);
        else
            // word to word dictionary
            return  getWordConversion(aText, nStartPos, nLength, toSChinese, nConversionOptions);
    } else
        throw NoSupportException(); // Conversion type is not supported in this service.
}

sal_Bool SAL_CALL
TextConversion_zh::interactiveConversion( const Locale& aLocale, sal_Int16 nTextConversionType, sal_Int32 nTextConversionOptions )
    throw(  RuntimeException, IllegalArgumentException, NoSupportException )
{
    return sal_False;
}

} } } }
<commit_msg>INTEGRATION: CWS textconversion (1.4.30); FILE MERGED 2005/10/27 01:05:55 tl 1.4.30.3: RESYNC: (1.4-1.5); FILE MERGED 2005/08/15 18:26:59 khong 1.4.30.2: #124006# return zero length offset when in/out strings are simple one to one mapping 2005/07/18 20:55:21 khong 1.4.30.1: #124006# add extended text conversion API<commit_after>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: textconversion_zh.cxx,v $
 *
 *  $Revision: 1.6 $
 *
 *  last change: $Author: rt $ $Date: 2005-11-08 09:15:56 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

#include <assert.h>
#include <textconversion.hxx>
#include <com/sun/star/i18n/TextConversionType.hpp>
#include <com/sun/star/i18n/TextConversionOption.hpp>
#include <com/sun/star/linguistic2/ConversionDirection.hpp>
#include <com/sun/star/linguistic2/ConversionDictionaryType.hpp>
#include <i18nutil/x_rtl_ustring.h>

using namespace com::sun::star::lang;
using namespace com::sun::star::i18n;
using namespace com::sun::star::linguistic2;
using namespace com::sun::star::uno;
using namespace rtl;

namespace com { namespace sun { namespace star { namespace i18n {

// defined in stc_char.cxx and stc_word.cxx  generated from stc_char.dic and stc_word.dic
// by genconv_dict
extern const sal_uInt16* getSTC_CharIndex_S2T();
extern const sal_Unicode* getSTC_CharData_S2T();
extern const sal_uInt16* getSTC_CharIndex_S2V();
extern const sal_Unicode* getSTC_CharData_S2V();
extern const sal_uInt16* getSTC_CharIndex_T2S();
extern const sal_Unicode* getSTC_CharData_T2S();

extern const sal_uInt16* getSTC_WordIndex_S2T(sal_Int32 &count);
extern const sal_uInt16* getSTC_WordIndex_T2S(sal_Int32 &count);
extern const sal_uInt16* getSTC_WordEntry_S2T();
extern const sal_uInt16* getSTC_WordEntry_T2S();
extern const sal_Unicode* getSTC_WordData(sal_Int32 &count);

TextConversion_zh::TextConversion_zh( const Reference < XMultiServiceFactory >& xMSF )
{
    Reference < XInterface > xI;
    xI = xMSF->createInstance(
        OUString::createFromAscii( "com.sun.star.linguistic2.ConversionDictionaryList" ));
    if ( xI.is() )
        xI->queryInterface( getCppuType((const Reference< XConversionDictionaryList>*)0) ) >>= xCDL;

    implementationName = "com.sun.star.i18n.TextConversion_zh";
}

sal_Unicode SAL_CALL getOneCharConversion(sal_Unicode ch, const sal_Unicode* Data, const sal_uInt16* Index)
{
    sal_Unicode address = Index[ch>>8];
    if (address != 0xFFFF)
        address = Data[address + (ch & 0xFF)];
    return (address != 0xFFFF) ? address : ch;
}

OUString SAL_CALL getCharConversion(const OUString& aText, sal_Int32 nStartPos, sal_Int32 nLength, sal_Bool toSChinese, sal_Int32 nConversionOptions)
{
    const sal_Unicode *Data;
    const sal_uInt16 *Index;

    if (toSChinese) {
        Data = getSTC_CharData_T2S();
        Index = getSTC_CharIndex_T2S();
    } else if (nConversionOptions & TextConversionOption::USE_CHARACTER_VARIANTS) {
        Data = getSTC_CharData_S2V();
        Index = getSTC_CharIndex_S2V();
    } else {
        Data = getSTC_CharData_S2T();
        Index = getSTC_CharIndex_S2T();
    }

    rtl_uString * newStr = x_rtl_uString_new_WithLength( nLength ); // defined in x_rtl_ustring.h
    for (sal_Int32 i = 0; i < nLength; i++)
        newStr->buffer[i] =
            getOneCharConversion(aText[nStartPos+i], Data, Index);
    return OUString( newStr->buffer, nLength);
}

OUString SAL_CALL TextConversion_zh::getWordConversion(const OUString& aText, sal_Int32 nStartPos, sal_Int32 nLength, sal_Bool toSChinese, sal_Int32 nConversionOptions, Sequence<sal_Int32>& offset)
{
    sal_Int32 dictLen = 0;
    const sal_Unicode *wordData = getSTC_WordData(dictLen);
    sal_Int32 maxLen = 0;
    const sal_uInt16 *index;
    const sal_uInt16 *entry;
    const sal_Unicode *charData;
    const sal_uInt16 *charIndex;
    sal_Bool one2one=sal_True;

    if (toSChinese) {
        index = getSTC_WordIndex_T2S(maxLen);
        entry = getSTC_WordEntry_T2S();
        charData = getSTC_CharData_T2S();
        charIndex = getSTC_CharIndex_T2S();
    } else {
        index = getSTC_WordIndex_S2T(maxLen);
        entry = getSTC_WordEntry_S2T();
        if (nConversionOptions & TextConversionOption::USE_CHARACTER_VARIANTS) {
            charData = getSTC_CharData_S2V();
            charIndex = getSTC_CharIndex_S2V();
        } else {
            charData = getSTC_CharData_S2T();
            charIndex = getSTC_CharIndex_S2T();
        }
    }

    if ((!wordData || !index || !entry) && !xCDL.is()) // no word mapping defined, do char2char conversion.
        return getCharConversion(aText, nStartPos, nLength, toSChinese, nConversionOptions);

    rtl_uString * newStr = x_rtl_uString_new_WithLength( nLength * 2 ); // defined in x_rtl_ustring.h
    sal_Int32 currPos = 0, count = 0;
    while (currPos < nLength) {
        sal_Int32 len = nLength - currPos;
        sal_Bool found = sal_False;
        if (len > maxLen)
            len = maxLen;
        for (; len > 0 && ! found; len--) {
            OUString word = aText.copy(nStartPos + currPos, len);
            sal_Int32 current = 0;
            // user dictionary
            if (xCDL.is()) {
                Sequence < OUString > conversions;
                try {
                    conversions = xCDL->queryConversions(word, 0, len,
                            aLocale, ConversionDictionaryType::SCHINESE_TCHINESE,
                            /*toSChinese ?*/ ConversionDirection_FROM_LEFT /*: ConversionDirection_FROM_RIGHT*/,
                            nConversionOptions);
                }
                catch ( NoSupportException & ) {
                    // clear reference (when there is no user dictionary) in order
                    // to not always have to catch this exception again
                    // in further calls. (save time)
                    xCDL = 0;
                }
                catch (...) {
                    // catch all other exceptions to allow
                    // querying the system dictionary in the next line
                }
                if (conversions.getLength() > 0) {
                    if (offset.getLength() > 0) {
                        if (word.getLength() != conversions[0].getLength())
                            one2one=sal_False;
                        while (current < conversions[0].getLength()) {
                            offset[count] = nStartPos + currPos + (current *
                                    word.getLength() / conversions[0].getLength());
                            newStr->buffer[count++] = conversions[0][current++];
                        }
                        offset[count-1] = nStartPos + currPos + word.getLength() - 1;
                    } else {
                        while (current < conversions[0].getLength())
                            newStr->buffer[count++] = conversions[0][current++];
                    }
                    currPos += word.getLength();
                    found = sal_True;
                }
            }

            if (!found && index[len+1] - index[len] > 0) {
                sal_Int32 bottom = (sal_Int32) index[len];
                sal_Int32 top = (sal_Int32) index[len+1] - 1;

                while (bottom <= top && !found) {
                    current = (top + bottom) / 2;
                    const sal_Int32 result = word.compareTo(wordData + entry[current]);
                    if (result < 0)
                        top = current - 1;
                    else if (result > 0)
                        bottom = current + 1;
                    else {
                        if (toSChinese)   // Traditionary/Simplified conversion,
                            for (current = entry[current]-1; current > 0 && wordData[current-1]; current--);
                        else  // Simplified/Traditionary conversion, forwards search for next word
                            current = entry[current] + word.getLength() + 1;
                        sal_Int32 start=current;
                        if (offset.getLength() > 0) {
                            if (word.getLength() != OUString(&wordData[current]).getLength())
                                one2one=sal_False;
                            sal_Int32 convertedLength=OUString(&wordData[current]).getLength();
                            while (wordData[current]) {
                                offset[count]=nStartPos + currPos + ((current-start) *
                                    word.getLength() / convertedLength);
                                newStr->buffer[count++] = wordData[current++];
                            }
                            offset[count-1]=nStartPos + currPos + word.getLength() - 1;
                        } else {
                            while (wordData[current])
                                newStr->buffer[count++] = wordData[current++];
                        }
                        currPos += word.getLength();
                        found = sal_True;
                    }
                }
            }
        }
        if (!found) {
            if (offset.getLength() > 0)
                offset[count]=nStartPos+currPos;
            newStr->buffer[count++] =
                getOneCharConversion(aText[nStartPos+currPos], charData, charIndex);
            currPos++;
        }
    }
    if (offset.getLength() > 0)
        offset.realloc(one2one ? 0 : count);
    return OUString( newStr->buffer, count);
}

TextConversionResult SAL_CALL
TextConversion_zh::getConversions( const OUString& aText, sal_Int32 nStartPos, sal_Int32 nLength,
    const Locale& rLocale, sal_Int16 nConversionType, sal_Int32 nConversionOptions)
    throw(  RuntimeException, IllegalArgumentException, NoSupportException )
{
    TextConversionResult result;

    result.Candidates.realloc(1);
    result.Candidates[0] = getConversion( aText, nStartPos, nLength, rLocale, nConversionType, nConversionOptions);
    result.Boundary.startPos = nStartPos;
    result.Boundary.endPos = nStartPos + nLength;

    return result;
}

OUString SAL_CALL
TextConversion_zh::getConversion( const OUString& aText, sal_Int32 nStartPos, sal_Int32 nLength,
    const Locale& rLocale, sal_Int16 nConversionType, sal_Int32 nConversionOptions)
    throw(  RuntimeException, IllegalArgumentException, NoSupportException )
{
    if (rLocale.Language.equalsAscii("zh") &&
            ( nConversionType == TextConversionType::TO_SCHINESE ||
            nConversionType == TextConversionType::TO_TCHINESE) ) {

        aLocale=rLocale;
        sal_Bool toSChinese = nConversionType == TextConversionType::TO_SCHINESE;

        if (nConversionOptions & TextConversionOption::CHARACTER_BY_CHARACTER)
            // char to char dictionary
            return getCharConversion(aText, nStartPos, nLength, toSChinese, nConversionOptions);
        else {
            Sequence <sal_Int32> offset;
            // word to word dictionary
            return  getWordConversion(aText, nStartPos, nLength, toSChinese, nConversionOptions, offset);
        }
    } else
        throw NoSupportException(); // Conversion type is not supported in this service.
}

OUString SAL_CALL
TextConversion_zh::getConversionWithOffset( const OUString& aText, sal_Int32 nStartPos, sal_Int32 nLength,
    const Locale& rLocale, sal_Int16 nConversionType, sal_Int32 nConversionOptions, Sequence<sal_Int32>& offset)
    throw(  RuntimeException, IllegalArgumentException, NoSupportException )
{
    if (rLocale.Language.equalsAscii("zh") &&
            ( nConversionType == TextConversionType::TO_SCHINESE ||
            nConversionType == TextConversionType::TO_TCHINESE) ) {

        aLocale=rLocale;
        sal_Bool toSChinese = nConversionType == TextConversionType::TO_SCHINESE;

        if (nConversionOptions & TextConversionOption::CHARACTER_BY_CHARACTER) {
            offset.realloc(0);
            // char to char dictionary
            return getCharConversion(aText, nStartPos, nLength, toSChinese, nConversionOptions);
        } else {
            if (offset.getLength() < 2*nLength)
                offset.realloc(2*nLength);
            // word to word dictionary
            return  getWordConversion(aText, nStartPos, nLength, toSChinese, nConversionOptions, offset);
        }
    } else
        throw NoSupportException(); // Conversion type is not supported in this service.
}

sal_Bool SAL_CALL
TextConversion_zh::interactiveConversion( const Locale& rLocale, sal_Int16 nTextConversionType, sal_Int32 nTextConversionOptions )
    throw(  RuntimeException, IllegalArgumentException, NoSupportException )
{
    return sal_False;
}

} } } }
<|endoftext|>


<commit_before>void VZEROSurveyToAlignment(){

  // Macro to convert survey data into alignment data.
  // The position of four fiducial marks, sticked on the
  // entrance face of the V0C box is converted into the
  // global position of the box. Positions given by surveyers
  // are extracted from Survey Data Base.

  if(!gGeoManager) TGeoManager::Import("geometry.root");

  TClonesArray *array = new TClonesArray("AliAlignObjMatrix",10);
  TClonesArray &mobj = *array;

  Double_t l_vect[3]={0.,0.,0.}; // a local vector (the origin)
  Double_t g_vect[3];            // vector corresp. to it in global RS
  Double_t m_vect[3];            // vector corresp. to it in mother RS

  // ************* get global matrix *******************
  TGeoHMatrix *g3 = AliGeomManager::GetMatrix("VZERO/V0C");

  // this is used below as the ideal global matrix

  // ************* get local matrix *******************
  TGeoNode* n3 = gGeoManager->GetCurrentNode();
  TGeoHMatrix* l3 = n3->GetMatrix();

 // point coordinates in the global RS
  g3->LocalToMaster(l_vect,g_vect);
  cout<<endl<<"Point coordinates in the global RS: "<<
    g_vect[0]<<" "<<g_vect[1]<<" "<<g_vect[2];

 // point coordinates in the mother volume RS
  l3->LocalToMaster(l_vect,m_vect);
  cout<<endl<<"Point coordinates in the mother's volume RS: "<<
    m_vect[0]<<" "<<m_vect[1]<<" "<<m_vect[2]<<" "<<endl;

 // Hereafter are the four ideal fiducial marks on the V0C box,
 // expressed in local coordinates and in cms - hard coded.

  const Double_t xside   = 22.627;
  const Double_t yside   = 22.627;
  const Double_t zsize   = 2.35;
  const Double_t zoffset = 0.001;

  const Double_t zdepth  = zsize+zoffset;
  Double_t A[3]={-xside,-yside,zdepth};
  Double_t B[3]={xside,-yside,zdepth};
  Double_t C[3]={xside,yside,zdepth};
  Double_t D[3]={-xside,yside,zdepth};

  TGeoTranslation* Atr = new TGeoTranslation("Atr",-xside,-yside,zdepth);
  TGeoTranslation* Btr = new TGeoTranslation("Btr",xside,-yside,zdepth);
  TGeoTranslation* Ctr = new TGeoTranslation("Ctr",xside,yside,zdepth);
  TGeoTranslation* Dtr = new TGeoTranslation("Dtr",-xside,yside,zdepth);

  //                    ^ local y
  //                    |
  //      D-------------|-------------C
  //      |             |             |
  //      |             |             |
  //      |             |             |
  //      |             |             |
  //      |             |             |
  //      |             |             |
  //  ------------------|------------------> local x
  //      |             |             |
  //      |             |             |
  //      |             |             |
  //      |             |             |
  //      |             |             |
  //      |             |             |
  //      A-------------|-------------B
  //
  // local z exiting the plane of the screen

  Double_t gA[3], gB[3], gC[3], gD[3];
  g3->LocalToMaster(A,gA);
  g3->LocalToMaster(B,gB);
  g3->LocalToMaster(C,gC);
  g3->LocalToMaster(D,gD);
  cout<<endl<<"Ideal fiducial marks coordinates in the global RS:\n"<<
    "A "<<gA[0]<<" "<<gA[1]<<" "<<gA[2]<<" "<<endl<<
    "B "<<gB[0]<<" "<<gB[1]<<" "<<gB[2]<<" "<<endl<<
    "C "<<gC[0]<<" "<<gC[1]<<" "<<gC[2]<<" "<<endl<<
    "D "<<gD[0]<<" "<<gD[1]<<" "<<gD[2]<<" "<<endl;
  cout<<endl<<endl;

// Retrieval of real survey data from ALICE Survey Data Depot :

  AliSurveyObj *so = new AliSurveyObj();

  so->FillFromLocalFile("Survey_835615_V0.txt");
  size = so->GetEntries();

  Printf("Title: \"%s\"", so->GetReportTitle().Data());
  Printf("Date: \"%s\"", so->GetReportDate().Data());
  Printf("Detector: \"%s\"", so->GetDetector().Data());
  Printf("URL: \"%s\"", so->GetURL().Data());
  Printf("Number: \"%d\"", so->GetReportNumber());
  Printf("Version: \"%d\"", so->GetReportVersion());
  Printf("Observations: \"%s\"", so->GetObservations().Data());
  Printf("Coordinate System: \"%s\"", so->GetCoordSys().Data());
  Printf("Measurement Units: \"%s\"", so->GetUnits().Data());
  Printf("Nr Columns: \"%d\" \n", so->GetNrColumns());

  TObjArray *colNames = so->GetColumnNames();

  TObjArray   *points = so->GetData();
  const char  namePoint[4]  = "6001";
  Double_t    coordinates[4][3];
//  Printf("  ******* %c ******* \n\n ", namePoint[0]);
  Printf("Relevant points to be used for alignment procedure (in mm):");
  for (Int_t i = 0; i < points->GetEntries(); ++i) {
    if(((AliSurveyPoint *) points->At(i))->GetPointName()[0] == namePoint[0]) {
           Printf("Point %d --> \"%s\" %f %f %f ", i,
           ((AliSurveyPoint *) points->At(i))->GetPointName().Data(),
	   ((AliSurveyPoint *) points->At(i))->GetX(),
	   ((AliSurveyPoint *) points->At(i))->GetY(),
	   ((AliSurveyPoint *) points->At(i))->GetZ() );
	   if(i > 10){
	   coordinates[i-11][0]  = (AliSurveyPoint *) points->At(i))->GetX();
	   coordinates[i-11][1]  = (AliSurveyPoint *) points->At(i))->GetY();
	   coordinates[i-11][2]  = (AliSurveyPoint *) points->At(i))->GetZ(); }
     }
  }

   Double_t ngA[3], ngB[3], ngC[3], ngD[3];

     for(Int_t i=0; i<3; i++)
     { ngA[i]  = coordinates[0][i] / 10.0 ;
       ngD[i]  = coordinates[1][i] / 10.0 ;
       ngB[i]  = coordinates[2][i] / 10.0 ;
       ngC[i]  = coordinates[3][i] / 10.0 ; }

   cout<<endl<<"Fiducial marks coordinates in the global RS given by surveyers:\n"<<
    "A "<<ngA[0]<<" "<<ngA[1]<<" "<<ngA[2]<<" "<<endl<<
    "B "<<ngB[0]<<" "<<ngB[1]<<" "<<ngB[2]<<" "<<endl<<
    "C "<<ngC[0]<<" "<<ngC[1]<<" "<<ngC[2]<<" "<<endl<<
    "D "<<ngD[0]<<" "<<ngD[1]<<" "<<ngD[2]<<" "<<endl;

  // From the new fiducial marks coordinates derive back the new global position
  // of the surveyed volume
  //*** What follows is the actual survey-to-alignment procedure which assumes,
  //*** as is the case of the present example, 4 fiducial marks
  //*** at the corners of a square lying on a plane parallel to a surface
  //*** of the surveyed box at a certain offset and with
  //*** x and y sides parallel to the box's x and y axes.
  //*** If the code below is placed in a separate class or method, it needs
  //*** as input the four points and the offset from the origin (zdepth)
  //*** The algorithm can be easily modified for different placement
  //*** and/or cardinality of the fiducial marks.

  Double_t ab[3], bc[3], n[3];
  Double_t plane[4], s;

  // first vector on the plane of the fiducial marks
  for(i=0;i<3;i++){
    ab[i] = ngB[i] - ngA[i];
  }

  // second vector on the plane of the fiducial marks
  for(i=0;i<3;i++){
    bc[i] = ngC[i] - ngB[i];
  }

  // vector normal to the plane of the fiducial marks obtained
  // as cross product of the two vectors on the plane d0^d1
  n[0] = ab[1] * bc[2] - ab[2] * bc[1];
  n[1] = ab[2] * bc[0] - ab[0] * bc[2];
  n[2] = ab[0] * bc[1] - ab[1] * bc[0];

  Double_t sizen = TMath::Sqrt( n[0]*n[0] + n[1]*n[1] + n[2]*n[2] );
  if(sizen>1.e-8){
          s = Double_t(1.)/sizen ; //normalization factor
  }else{
          return 0;
  }

  // plane expressed in the hessian normal form, see:
  // http://mathworld.wolfram.com/HessianNormalForm.html
  // the first three are the coordinates of the orthonormal vector
  // the fourth coordinate is equal to the distance from the origin
  for(i=0;i<3;i++){
    plane[i] = n[i] * s;
  }
  plane[3] = -( plane[0] * ngA[0] + plane[1] * ngA[1] + plane[2] * ngA[2] );
//  cout<<plane[0]<<"  "<<plane[1]<<"  "<<plane[2]<<"  "<<plane[3]<<"  "<<endl;

  // The center of the square with fiducial marks as corners
  // as the middle point of one diagonal - md
  // Used below to get the center - orig - of the surveyed box

  Double_t orig[3], md[3];
  for(i=0;i<3;i++){
    md[i] = (ngA[i] + ngC[i]) * 0.5;
  }

  //  center of the box
  for(i=0;i<3;i++){
    orig[i] = md[i] - plane[i]*zdepth;
  }

  cout<<endl<<"Center of the box: "<<orig[0]<<"  "<<orig[1]<<"  "<<orig[2]<<endl;

  // get x,y local directions needed to write the global rotation matrix
  // for the surveyed volume by normalising vectors ab and bc

  Double_t sx = TMath::Sqrt(ab[0]*ab[0] + ab[1]*ab[1] + ab[2]*ab[2]);
  if(sx>1.e-8){
     for(i=0;i<3;i++){
          ab[i] /= sx;
     }
     cout<<"x direction "<<ab[0]<<"  "<<ab[1]<<"  "<<ab[2]<<endl;
  }
  Double_t sy = TMath::Sqrt(bc[0]*bc[0] + bc[1]*bc[1] + bc[2]*bc[2]);
  if(sy>1.e-8){
    for(i=0;i<3;i++){
          bc[i] /= sy;
    }
    cout<<"y direction "<<bc[0]<<"  "<<bc[1]<<"  "<<bc[2]<<endl;
  }

  // the global matrix for the surveyed volume - ng
  Double_t rot[9] = {ab[0],bc[0],plane[0],ab[1],bc[1],plane[1],ab[2],bc[2],plane[2]};
  TGeoHMatrix ng;
  ng.SetTranslation(orig);
  ng.SetRotation(rot);

  cout<<"\n********* global matrix inferred from surveyed fiducial marks ***********\n";
  ng.Print();

 // To produce the alignment object for the given volume you would
 // then do something like this:
 // Calculate the global delta transformation as ng * g3^-1

 TGeoHMatrix gdelta = g3->Inverse(); //now equal to the inverse of g3
 gdelta.MultiplyLeft(&ng);
 Int_t index = 0;

 // if the volume is in the look-up table use something like this instead:
 // AliGeomManager::LayerToVolUID(AliGeomManager::kTOF,i);

 //AliAlignObjMatrix* mobj[0] = new AliAlignObjMatrix("VZERO/V0C",index,gdelta,kTRUE);

  new(mobj[0]) AliAlignObjMatrix("VZERO/V0C",index,gdelta,kTRUE);

  if(!gSystem->Getenv("$TOCDB")){
    // save on file
     TFile f("V0Survey.root","RECREATE");
     if(!f) cerr<<"cannot open file for output\n";
     f.cd();
     f.WriteObject(array,"V0SurveyObjs ","kSingleKey");
     f.Close();
  }else{
    // save in CDB storage
     AliCDBManager* cdb = AliCDBManager::Instance();
     AliCDBStorage* storage = cdb->GetStorage("local://$ALICE_ROOT");
     AliCDBMetaData* mda = new AliCDBMetaData();
     mda->SetResponsible("Brigitte Cheynis");
     mda->SetComment("Alignment objects for V0 survey");
     mda->SetAliRootVersion(gSystem->Getenv("$ARVERSION"));
     AliCDBId id("VZERO/Align/Data",0,9999999);
     storage->Put(array,id,mda);
  }

  array->Delete();

}
<commit_msg>AliAlignObjMatrix replaced by AliAlignObjParams<commit_after>void VZEROSurveyToAlignment(){

  // Macro to convert survey data into alignment data.
  // The position of four fiducial marks, sticked on the
  // entrance face of the V0C box is converted into the
  // global position of the box. Positions given by surveyers
  // are extracted from Survey Data Base.

  if(!gGeoManager) TGeoManager::Import("geometry.root");

//  TClonesArray *array = new TClonesArray("AliAlignObjMatrix",10);
  TClonesArray *array = new TClonesArray("AliAlignObjParams",10);
  TClonesArray &mobj = *array;

  Double_t l_vect[3]={0.,0.,0.}; // local vector (the origin)
  Double_t g_vect[3];            // vector corresp. to it in global RS
  Double_t m_vect[3];            // vector corresp. to it in mother RS

  // ************* get global matrix g3 *******************
  //  TGeoHMatrix *g3 = AliGeomManager::GetMatrix("VZERO/V0C");
  TGeoHMatrix *g3 = gGeoManager->GetCurrentMatrix();
  // this is used below as the IDEAL global matrix

  // ************* get local matrix l3 *******************
  TGeoNode* n3 = gGeoManager->GetCurrentNode();
  TGeoHMatrix *l3 = n3->GetMatrix();

 // point coordinates in the global RS
  g3->LocalToMaster(l_vect,g_vect);
  cout<<endl<<"Point coordinates in the global RS: "
      <<g_vect[0]<<" "<<g_vect[1]<<" "<<g_vect[2];

 // point coordinates in the mother volume RS
  l3->LocalToMaster(l_vect,m_vect);
  cout<<endl<<"Point coordinates in the mother's volume RS: \n"
      <<m_vect[0]<<" "<<m_vect[1]<<" "<<m_vect[2]<<" "<<endl;

 // Hereafter are the four ideal fiducial marks on the V0C box,
 // expressed in local coordinates and in cms - hard coded.

  const Double_t xside   = 22.627;
  const Double_t yside   = 22.627;
  const Double_t zsize   = 2.35;
  const Double_t zoffset = 0.001;

  const Double_t zdepth  = zsize+zoffset;
  Double_t A[3]={-xside,-yside,zdepth};
  Double_t B[3]={xside,-yside,zdepth};
  Double_t C[3]={xside,yside,zdepth};
  Double_t D[3]={-xside,yside,zdepth};

  TGeoTranslation* Atr = new TGeoTranslation("Atr",-xside,-yside,zdepth);
  TGeoTranslation* Btr = new TGeoTranslation("Btr",xside,-yside,zdepth);
  TGeoTranslation* Ctr = new TGeoTranslation("Ctr",xside,yside,zdepth);
  TGeoTranslation* Dtr = new TGeoTranslation("Dtr",-xside,yside,zdepth);

  //                    ^ local y
  //                    |
  //      D-------------|-------------C
  //      |             |             |
  //      |             |             |
  //      |             |             |
  //      |             |             |
  //      |             |             |
  //      |             |             |
  //  ------------------|------------------> local x
  //      |             |             |
  //      |             |             |
  //      |             |             |
  //      |             |             |
  //      |             |             |
  //      |             |             |
  //      A-------------|-------------B
  //
  // local z exiting the plane of the screen

  Double_t gA[3], gB[3], gC[3], gD[3];
  g3->LocalToMaster(A,gA);
  g3->LocalToMaster(B,gB);
  g3->LocalToMaster(C,gC);
  g3->LocalToMaster(D,gD);
  cout<<endl<<"Ideal fiducial marks coordinates in the global RS: \n"
      <<"A "<<gA[0]<<" "<<gA[1]<<" "<<gA[2]<<" "<<endl
      <<"B "<<gB[0]<<" "<<gB[1]<<" "<<gB[2]<<" "<<endl
      <<"C "<<gC[0]<<" "<<gC[1]<<" "<<gC[2]<<" "<<endl
      <<"D "<<gD[0]<<" "<<gD[1]<<" "<<gD[2]<<" "<<endl;
  cout<<endl;

// Retrieval of REAL survey data from ALICE Survey Data Depot :

  AliSurveyObj *so = new AliSurveyObj();

  so->FillFromLocalFile("Survey_835615_V0.txt");
  Int_t size = so->GetEntries();

  Printf("Title: \"%s\"", so->GetReportTitle().Data());
  Printf("Date: \"%s\"", so->GetReportDate().Data());
  Printf("Detector: \"%s\"", so->GetDetector().Data());
  Printf("URL: \"%s\"", so->GetURL().Data());
  Printf("Number: \"%d\"", so->GetReportNumber());
  Printf("Version: \"%d\"", so->GetReportVersion());
  Printf("Observations: \"%s\"", so->GetObservations().Data());
  Printf("Coordinate System: \"%s\"", so->GetCoordSys().Data());
  Printf("Measurement Units: \"%s\"", so->GetUnits().Data());
  Printf("Nr Columns: \"%d\" \n", so->GetNrColumns());

  TObjArray *colNames = so->GetColumnNames();

  TObjArray   *points = so->GetData();
  const char  namePoint[4]  = "6001";
  Double_t    coordinates[4][3];
//  Printf("  ******* %c ******* \n\n ", namePoint[0]);
  Printf("Relevant points to be used for alignment procedure (in mm):");
  for (Int_t i = 0; i < points->GetEntries(); ++i) {
    if(((AliSurveyPoint *) points->At(i))->GetPointName()[0] == namePoint[0]) {
           Printf("Point %d --> \"%s\" %f %f %f ", i,
           ((AliSurveyPoint *) points->At(i))->GetPointName().Data(),
	   ((AliSurveyPoint *) points->At(i))->GetX(),
	   ((AliSurveyPoint *) points->At(i))->GetY(),
	   ((AliSurveyPoint *) points->At(i))->GetZ() );
	   if(i > 10){
	   coordinates[i-11][0]  = (AliSurveyPoint *) points->At(i))->GetX();
	   coordinates[i-11][1]  = (AliSurveyPoint *) points->At(i))->GetY();
	   coordinates[i-11][2]  = (AliSurveyPoint *) points->At(i))->GetZ(); }
     }
  }

   Double_t ngA[3], ngB[3], ngC[3], ngD[3];

   for(Int_t i=0; i<3; i++)
     { ngA[i]  = coordinates[0][i] / 10.0 ;
       ngD[i]  = coordinates[1][i] / 10.0 ;
       ngB[i]  = coordinates[2][i] / 10.0 ;
       ngC[i]  = coordinates[3][i] / 10.0 ; }

   cout<<endl<<"Fiducial marks coordinates in the global RS given by surveyers: \n"
       <<"A "<<ngA[0]<<" "<<ngA[1]<<" "<<ngA[2]<<" "<<endl
       <<"B "<<ngB[0]<<" "<<ngB[1]<<" "<<ngB[2]<<" "<<endl
       <<"C "<<ngC[0]<<" "<<ngC[1]<<" "<<ngC[2]<<" "<<endl
       <<"D "<<ngD[0]<<" "<<ngD[1]<<" "<<ngD[2]<<" "<<endl;

  // From the new fiducial marks coordinates derive back the new global position
  // of the surveyed volume
  //*** What follows is the actual survey-to-alignment procedure which assumes,
  //*** as is the case of the present example, 4 fiducial marks
  //*** at the corners of a square lying on a plane parallel to a surface
  //*** of the surveyed box at a certain offset and with
  //*** x and y sides parallel to the box's x and y axes.
  //*** If the code below is placed in a separate class or method, it needs
  //*** as input the four points and the offset from the origin (zdepth)
  //*** The algorithm can be easily modified for different placement
  //*** and/or cardinality of the fiducial marks.

  Double_t ab[3], bc[3], n[3];
  Double_t plane[4], s;

  // first vector on the plane of the fiducial marks
  for(i=0;i<3;i++){
    ab[i] = ngB[i] - ngA[i];
  }

  // second vector on the plane of the fiducial marks
  for(i=0;i<3;i++){
    bc[i] = ngC[i] - ngB[i];
  }

  // vector normal to the plane of the fiducial marks obtained
  // as cross product of the two vectors on the plane d0^d1
  n[0] = ab[1] * bc[2] - ab[2] * bc[1];
  n[1] = ab[2] * bc[0] - ab[0] * bc[2];
  n[2] = ab[0] * bc[1] - ab[1] * bc[0];

  Double_t sizen = TMath::Sqrt( n[0]*n[0] + n[1]*n[1] + n[2]*n[2] );
  if(sizen>1.e-8){
          s = Double_t(1.)/sizen ; //normalization factor
  }else{
          return 0;
  }

  // plane expressed in the hessian normal form, see:
  // http://mathworld.wolfram.com/HessianNormalForm.html
  // the first three are the coordinates of the orthonormal vector
  // the fourth coordinate is equal to the distance from the origin
  for(i=0;i<3;i++){
    plane[i] = n[i] * s;
  }
  plane[3] = -( plane[0] * ngA[0] + plane[1] * ngA[1] + plane[2] * ngA[2] );
//  cout<<plane[0]<<"  "<<plane[1]<<"  "<<plane[2]<<"  "<<plane[3]<<"  "<<endl;

  // The center of the square with fiducial marks as corners
  // as the middle point of one diagonal - md
  // Used below to get the center - orig - of the surveyed box

  Double_t orig[3], md[3];
  for(i=0;i<3;i++){
    md[i] = (ngA[i] + ngC[i]) * 0.5;
  }

  //  center of the box
  for(i=0;i<3;i++){
    orig[i] = md[i] - plane[i]*zdepth;
  }

  cout<<endl<<"Center of the box: "<<orig[0]<<"  "<<orig[1]<<"  "<<orig[2]<<endl;

  // get x,y local directions needed to write the global rotation matrix
  // for the surveyed volume by normalising vectors ab and bc

  Double_t sx = TMath::Sqrt(ab[0]*ab[0] + ab[1]*ab[1] + ab[2]*ab[2]);
  if(sx>1.e-8){
     for(i=0;i<3;i++){
          ab[i] /= sx;
     }
     cout<<"x direction "<<ab[0]<<"  "<<ab[1]<<"  "<<ab[2]<<endl;
  }
  Double_t sy = TMath::Sqrt(bc[0]*bc[0] + bc[1]*bc[1] + bc[2]*bc[2]);
  if(sy>1.e-8){
    for(i=0;i<3;i++){
          bc[i] /= sy;
    }
    cout<<"y direction "<<bc[0]<<"  "<<bc[1]<<"  "<<bc[2]<<endl;
  }

  // the global matrix for the surveyed volume - ng
  Double_t rot[9] = {ab[0],bc[0],plane[0],ab[1],bc[1],plane[1],ab[2],bc[2],plane[2]};
  TGeoHMatrix ng;
  ng.SetTranslation(orig);
  ng.SetRotation(rot);

//  cout<<"\n********* global matrix inferred from surveyed fiducial marks ***********\n";
//  ng.Print();

 // To produce the alignment object for the given volume you would
 // then do something like this:
 // Calculate the global delta transformation as ng * g3^-1

 TGeoHMatrix gdelta = g3->Inverse(); //now equal to the inverse of g3
 gdelta.MultiplyLeft(&ng);
 Int_t index = 0;

 // if the volume is in the look-up table use something like this instead:
 // AliGeomManager::LayerToVolUID(AliGeomManager::kTOF,i);

 //AliAlignObjMatrix* mobj[0] = new AliAlignObjMatrix("VZERO/V0C",index,gdelta,kTRUE);
 //  new(mobj[0]) AliAlignObjMatrix("VZERO/V0C",index,gdelta,kTRUE);

  new(mobj[0]) AliAlignObjParams("VZERO/V0C",index,gdelta,kTRUE);

  if(!gSystem->Getenv("$TOCDB")){
    // save on file
     TFile f("V0Survey.root","RECREATE");
     if(!f) cerr<<"cannot open file for output\n";
     f.cd();
     f.WriteObject(array,"V0SurveyObjs ","kSingleKey");
     f.Close();
  }else{
    // save in CDB storage
     AliCDBManager* cdb = AliCDBManager::Instance();
     AliCDBStorage* storage = cdb->GetStorage("local://$ALICE_ROOT");
     AliCDBMetaData* mda = new AliCDBMetaData();
     mda->SetResponsible("Brigitte Cheynis");
     mda->SetComment("Alignment objects for V0 survey");
     mda->SetAliRootVersion(gSystem->Getenv("$ARVERSION"));
     AliCDBId id("VZERO/Align/Data",0,9999999);
     storage->Put(array,id,mda);
  }

  cout<<"\n********* Alignment constants contained in alignment object ***********\n";
  cout<<"*************** deduced from surveyed fiducial marks : ****************\n";
  array->Print();

  AliAlignObjParams* itsalobj = (AliAlignObjParams*) mobj.UncheckedAt(0);
  itsalobj->ApplyToGeometry();

  array->Delete();

}
<|endoftext|>


<commit_before>#include <stdexcept>
#include <string>

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/TransformStamped.h>
#include <goal_sender_msgs/ApplyGoals.h>
#include <goal_sender_msgs/GoalSequence.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <move_base_msgs/MoveBaseGoal.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>

inline double squaring_distance(const geometry_msgs::Point& a, const geometry_msgs::Point& b) {
  const auto x {a.x - b.x};
  const auto y {a.y - b.y};
  return x*x + y*y;
}

/**
 * Waypoint update service manager
 */
class WaypointManager
{
public:
  WaypointManager()
    : sequence_ {},
      now_goal_ {sequence_.waypoints.end()}
  {
  }

  bool operator()(goal_sender_msgs::ApplyGoals::Request& req,
                  goal_sender_msgs::ApplyGoals::Response& res){
    sequence_ = req.goal_sequence;
    now_goal_ = sequence_.waypoints.begin();
    res.success = true;
    res.message = "update waypoints";
    return true;
  }

  const geometry_msgs::Point& point() const
  {
    if (is_end()) throw std::logic_error {"range error: Please check is_end() before point()"};
    return now_goal_->position;
  }

  const geometry_msgs::Quaternion& quaternion() const
  {
    if (is_end()) throw std::logic_error {"range error: Please check is_end() before quaternion()"};
    return now_goal_->orientation;
  }

  double radius() const
  {
    if (is_end()) throw std::logic_error {"range error: Please check is_end() before radius()"};
    return now_goal_->radius;
  }

  bool next()
  {
    if (is_end()) throw std::logic_error {"range error: Please check is_end() before next()"}; // wrong way.
    if (++now_goal_ == sequence_.waypoints.end())
      return false; // correct way: this is last one.
    return true;
  }

  bool is_end() const noexcept
  {
    return sequence_.waypoints.end() == now_goal_;
  }

  explicit operator bool() noexcept
  {
    return !is_end();
  }

  [[deprecated]]
  const goal_sender_msgs::Waypoint& get() const
  {
    if (is_end()) throw std::logic_error {"range error: Please check is_end() before get()"};
    return *now_goal_;
  }

private:
  goal_sender_msgs::GoalSequence sequence_;
  decltype(sequence_.waypoints)::iterator now_goal_;
};

/**
 * Tf lookup API
 */
class TfPositionManager
{
public:
  explicit TfPositionManager(tf2_ros::Buffer& tfBuffer) // buffer is lvalue
    : buffer_ {tfBuffer}
  {
  }

  geometry_msgs::Point operator()(std::string parent, std::string child) const
  {
    const auto ts {buffer_.lookupTransform(parent, child, ros::Time(0))};
    geometry_msgs::Point point;
    point.x = ts.transform.translation.x;
    point.y = ts.transform.translation.y;
    return point;
  }

private:
  const tf2_ros::Buffer& buffer_;
};

class GoalSender
{
public:
  using MoveBaseActionClient = actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>;

  GoalSender(WaypointManager& point_manager,
             MoveBaseActionClient& move_base_client,
             TfPositionManager lookupper)
    : point_manager_ {point_manager},
      move_base_client_ {move_base_client},
      lookupper_ {lookupper}
  {
  }

  void once()
  {
    if (!point_manager_)
      return; // no work
    if (is_reach()) {
      point_manager_.next();
      send_goal();
    }
  }

private:
  bool is_reach() const
  {
    const auto robot_point {lookupper_("/map", "/base_link")};
    const auto waypoint_point {point_manager_.point()};
    const auto sqr_distance {squaring_distance(robot_point, waypoint_point)};

    const auto radius {point_manager_.radius()};
    const auto sqr_radius {radius * radius};

    if (sqr_distance < sqr_radius) // into valid range
      return true;
    return false;
  }

  void send_goal() const
  {
    if (!point_manager_) { // finish waypoint
      move_base_client_.cancelGoal(); // cancel moveing
      ROS_INFO("Finish waypoints");
      return;
    }

    move_base_msgs::MoveBaseGoal goal;
    goal.target_pose.pose.position = point_manager_.point();
    goal.target_pose.pose.orientation = point_manager_.quaternion();
    goal.target_pose.header.frame_id = "/map";
    goal.target_pose.header.stamp = ros::Time::now();
    move_base_client_.sendGoal(goal); // send waypoint
  }

  WaypointManager& point_manager_;
  MoveBaseActionClient& move_base_client_;
  TfPositionManager lookupper_;
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "goal_sender");
  ros::NodeHandle nh {};

  WaypointManager point_manager {};
  ros::ServiceServer srv {
      nh.advertiseService<
          goal_sender_msgs::ApplyGoals::Request,
          goal_sender_msgs::ApplyGoals::Response>(
              "apply_goals", point_manager)};

  tf2_ros::Buffer tfBuffer;
  tf2_ros::TransformListener tfListener {tfBuffer};
  TfPositionManager lookupper {tfBuffer};

  GoalSender::MoveBaseActionClient move_base_client {"move_base", true};
  move_base_client.waitForServer();

  GoalSender goal_sender {point_manager, move_base_client, lookupper};

  ros::Rate rate {10};
  while (ros::ok()) {
    ros::spinOnce();
    goal_sender.once();
  }
}
<commit_msg>Fix running speed<commit_after>#include <stdexcept>
#include <string>

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/TransformStamped.h>
#include <goal_sender_msgs/ApplyGoals.h>
#include <goal_sender_msgs/GoalSequence.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <move_base_msgs/MoveBaseGoal.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>

inline double squaring_distance(const geometry_msgs::Point& a, const geometry_msgs::Point& b) {
  const auto x {a.x - b.x};
  const auto y {a.y - b.y};
  return x*x + y*y;
}

/**
 * Waypoint update service manager
 */
class WaypointManager
{
public:
  WaypointManager()
    : sequence_ {},
      now_goal_ {sequence_.waypoints.end()}
  {
  }

  bool operator()(goal_sender_msgs::ApplyGoals::Request& req,
                  goal_sender_msgs::ApplyGoals::Response& res){
    sequence_ = req.goal_sequence;
    now_goal_ = sequence_.waypoints.begin();
    res.success = true;
    res.message = "update waypoints";
    return true;
  }

  const geometry_msgs::Point& point() const
  {
    if (is_end()) throw std::logic_error {"range error: Please check is_end() before point()"};
    return now_goal_->position;
  }

  const geometry_msgs::Quaternion& quaternion() const
  {
    if (is_end()) throw std::logic_error {"range error: Please check is_end() before quaternion()"};
    return now_goal_->orientation;
  }

  double radius() const
  {
    if (is_end()) throw std::logic_error {"range error: Please check is_end() before radius()"};
    return now_goal_->radius;
  }

  bool next()
  {
    if (is_end()) throw std::logic_error {"range error: Please check is_end() before next()"}; // wrong way.
    if (++now_goal_ == sequence_.waypoints.end())
      return false; // correct way: this is last one.
    return true;
  }

  bool is_end() const noexcept
  {
    return sequence_.waypoints.end() == now_goal_;
  }

  explicit operator bool() noexcept
  {
    return !is_end();
  }

  [[deprecated]]
  const goal_sender_msgs::Waypoint& get() const
  {
    if (is_end()) throw std::logic_error {"range error: Please check is_end() before get()"};
    return *now_goal_;
  }

private:
  goal_sender_msgs::GoalSequence sequence_;
  decltype(sequence_.waypoints)::iterator now_goal_;
};

/**
 * Tf lookup API
 */
class TfPositionManager
{
public:
  explicit TfPositionManager(tf2_ros::Buffer& tfBuffer) // buffer is lvalue
    : buffer_ {tfBuffer}
  {
  }

  geometry_msgs::Point operator()(std::string parent, std::string child) const
  {
    const auto ts {buffer_.lookupTransform(parent, child, ros::Time(0))};
    geometry_msgs::Point point;
    point.x = ts.transform.translation.x;
    point.y = ts.transform.translation.y;
    return point;
  }

private:
  const tf2_ros::Buffer& buffer_;
};

class GoalSender
{
public:
  using MoveBaseActionClient = actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>;

  GoalSender(WaypointManager& point_manager,
             MoveBaseActionClient& move_base_client,
             TfPositionManager lookupper)
    : point_manager_ {point_manager},
      move_base_client_ {move_base_client},
      lookupper_ {lookupper}
  {
  }

  void once()
  {
    if (!point_manager_)
      return; // no work
    if (is_reach()) {
      point_manager_.next();
      send_goal();
    }
  }

private:
  bool is_reach() const
  {
    const auto robot_point {lookupper_("/map", "/base_link")};
    const auto waypoint_point {point_manager_.point()};
    const auto sqr_distance {squaring_distance(robot_point, waypoint_point)};

    const auto radius {point_manager_.radius()};
    const auto sqr_radius {radius * radius};

    if (sqr_distance < sqr_radius) // into valid range
      return true;
    return false;
  }

  void send_goal() const
  {
    if (!point_manager_) { // finish waypoint
      move_base_client_.cancelGoal(); // cancel moveing
      ROS_INFO("Finish waypoints");
      return;
    }

    move_base_msgs::MoveBaseGoal goal;
    goal.target_pose.pose.position = point_manager_.point();
    goal.target_pose.pose.orientation = point_manager_.quaternion();
    goal.target_pose.header.frame_id = "/map";
    goal.target_pose.header.stamp = ros::Time::now();
    move_base_client_.sendGoal(goal); // send waypoint
  }

  WaypointManager& point_manager_;
  MoveBaseActionClient& move_base_client_;
  TfPositionManager lookupper_;
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "goal_sender");
  ros::NodeHandle nh {};

  WaypointManager point_manager {};
  ros::ServiceServer srv {
      nh.advertiseService<
          goal_sender_msgs::ApplyGoals::Request,
          goal_sender_msgs::ApplyGoals::Response>(
              "apply_goals", point_manager)};

  tf2_ros::Buffer tfBuffer;
  tf2_ros::TransformListener tfListener {tfBuffer};
  TfPositionManager lookupper {tfBuffer};

  GoalSender::MoveBaseActionClient move_base_client {"move_base", true};
  move_base_client.waitForServer();

  GoalSender goal_sender {point_manager, move_base_client, lookupper};

  ros::Rate rate {10};
  while (ros::ok()) {
    ros::spinOnce();
    goal_sender.once();
    rate.sleep();
  }
}
<|endoftext|>