<commit_before>#include <iostream>                                         /* allows to perform standard input and output operations */
#include <fstream>
#include <stdio.h>                                          /* Standard input/output definitions */
#include <stdint.h>                                         /* Standard input/output definitions */
#include <stdlib.h>                                         /* defines several general purpose functions */
#include <unistd.h>                                         /* UNIX standard function definitions */
#include <fcntl.h>                                          /* File control definitions */
#include <ctype.h>                                          /* isxxx() */
#include <termios.h>                                        /* POSIX terminal control definitions */
#include <ros/ros.h>                                        /* ROS */
#include <geometry_msgs/Twist.h>                            /* ROS Twist message */
#include <base_controller/encoders.h>                       /* Custom message /encoders */
#include <base_controller/md49data.h>                       /* Custom message /encoders */

const char* serialport_name="/dev/ttyS2";                   /* defines used serialport on BPi. Use "/dev/ttyAMA0" for RPi*/
int serialport_bps=B9600;                                  /* defines used baudrate on serialport */
int fd;                                                     /* serial port file descriptor */
struct termios orig;                                        // backuped port options
int32_t EncoderL;                                           /* stores encoder value left read from md49 */
int32_t EncoderR;                                           /* stores encoder value right read from md49 */
char speed_l=128;
char speed_r=128;                                  /* speed to set for MD49 */
char last_speed_l=128, last_speed_r=128;           /* speed to set for MD49 */
double vr = 0.0;
double vl = 0.0;
double max_vr = 0.2;
double max_vl = 0.2;
double min_vr = 0.2;
double min_vl = 0.2;
double base_width = 0.4;                                    /* Base width in meters */

unsigned char serialBuffer[18];                             /* Serial buffer to store uart data */
void read_MD49_Data (void);
void set_MD49_speed (void);
char* itoa(int value, char* result, int base);

int openSerialPort(const char * device, int bps);
void writeBytes(int descriptor, int count);
void readBytes(int descriptor, int count);

base_controller::encoders encoders;
base_controller::md49data md49data;

void cmd_vel_callback(const geometry_msgs::Twist& vel_cmd){

        if (vel_cmd.linear.x>0){
            speed_l = 255;
            speed_r = 255;
            //set_MD49_speed();
        }
        if (vel_cmd.linear.x<0){
            speed_l = 0;
            speed_r = 0;
            //set_MD49_speed();
        }
        if (vel_cmd.linear.x==0 && vel_cmd.angular.z==0){
            speed_l = 128;
            speed_r = 128;
            //set_MD49_speed();
        }
        if (vel_cmd.angular.z>0){
            speed_l = 0;
            speed_r = 255;
            //set_MD49_speed();
        }
        if (vel_cmd.angular.z<0){
            speed_l = 255;
            speed_r = 0;
            //set_MD49_speed();
        }

    /*
        //ANFANG Alternative
        if (vel_cmd.linear.x==0 && vel_cmd.angular.z==0){vl=0;vr=0;}
        else if(vel_cmd.linear.x == 0){
            // turning
            vr = vel_cmd.angular.z * base_width / 2.0;
            vl = (-1) * vr;
        }
        else if(vel_cmd.angular.z == 0){
            // forward / backward
            vl = vr = vel_cmd.linear.x;
        }
        else{
            // moving doing arcs
            vl = vel_cmd.linear.x - vel_cmd.angular.z * base_width / 2.0;
            if (vl > max_vl) {vl=max_vl;}
            if (vl < min_vl) {vl=min_vl;}
            vr = vel_cmd.linear.x + vel_cmd.angular.z * base_width / 2.0;
            if (vr > max_vr) {vr=max_vr;}
            if (vr < min_vr) {vr=min_vr;}
        }
        //ENDE Alternative
    */
}

int main( int argc, char* argv[] ){

    ros::init(argc, argv, "base_controller" );
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/cmd_vel", 10, cmd_vel_callback);
    ros::Publisher encoders_pub = n.advertise<base_controller::encoders>("encoders",10);
    ros::Publisher md49data_pub = n.advertise<base_controller::md49data>("md49data",10);

    // Init node
    // *********
    ros::Rate loop_rate(10);
    ROS_INFO("base_controller running...");
    ROS_INFO("=============================");
    ROS_INFO("Subscribing to topic /cmd_vel");
    ROS_INFO("Publishing to topic /encoders");
    ROS_INFO("Publishing to topic /md49data");

    // Open serial port
    // ****************
    fd = openSerialPort(serialport_name, serialport_bps);
    if (fd == -1) exit(1);
    ROS_INFO("Opend serial port at %s with %i Bps",serialport_name,serialport_bps);
    usleep(10000);                                          // Sleep for UART to power up and set options

    while(n.ok())
    {
        // Read encoder and other data from MD49
        // (data is read from serial_controller_node
        //  and avaiable through md49_data.txt)
        // *****************************************
//      read_MD49_Data();

        // set speed as in md49speed.txt
        // *****************************
        if ((speed_l != last_speed_l) || (speed_r != last_speed_r)){
            // gewünschte werte in textfile
            //write_MD49_speed(speed_l,speed_r);
            set_MD49_speed();
            last_speed_l=speed_l;
            last_speed_r=speed_r;
        }

        // Publish encoder values to topic /encoders (custom message)
        // **********************************************************
        encoders.encoder_l=EncoderL;
        encoders.encoder_r=EncoderR;
        encoders_pub.publish(encoders);

        // Publish MD49 data to topic /md49data (custom message)
        // *****************************************************
        md49data.speed_l = serialBuffer[8];
        md49data.speed_r = serialBuffer[9];
        md49data.volt = serialBuffer[10];
        md49data.current_l = serialBuffer[11];
        md49data.current_r = serialBuffer[12];
        md49data.error = serialBuffer[13];
        md49data.acceleration = serialBuffer[14];
        md49data.mode = serialBuffer[15];
        md49data.regulator = serialBuffer[16];
        md49data.timeout = serialBuffer[17];
        md49data_pub.publish(md49data);

        // ****
        ros::spinOnce();
        loop_rate.sleep();

    }// end.mainloop

    return 1;
} // end.main

void read_MD49_Data (void){

/*
    // Output MD49 data on screen
    // **************************
    printf("\033[2J");                                      //  clear the screen
    printf("\033[H");                                       //  position cursor at top-left corner
    printf ("MD49-Data read from AVR-Master: \n");
    printf("========================================\n");
    printf("Encoder1 Byte1: %i ",reply[0]);
    printf("Byte2: %i ",reply[1]);
    printf("Byte3: % i ",reply[2]);
    printf("Byte4: %i \n",reply[3]);
    printf("Encoder2 Byte1: %i ",reply[4]);
    printf("Byte2: %i ",reply[5]);
    printf("Byte3: %i ",reply[6]);
    printf("Byte4: %i \n",reply[7]);
    printf("EncoderL: %i ",EncoderL);
    printf("EncoderR: %i \n",EncoderR);
    printf("========================================\n");
    printf("Speed1: %i ",reply[8]);
    printf("Speed2: %i \n",reply[9]);
    printf("Volts: %i \n",reply[10]);
    printf("Current1: %i ",reply[11]);
    printf("Current2: %i \n",reply[12]);
    printf("Error: %i \n",reply[13]);
    printf("Acceleration: %i \n",reply[14]);
    printf("Mode: %i \n",reply[15]);
    printf("Regulator: %i \n",reply[16]);
    printf("Timeout: %i \n",reply[17]);
*/

}

void set_MD49_speed(void){

    serialBuffer[0] = 0;
    serialBuffer[1] = 0x31;					// Command to set motor speed
    serialBuffer[2] = speed_l;				// Speed to be set

    serialBuffer[3] = 0;
    serialBuffer[4] = 0x32;
    serialBuffer[5] = speed_r;

    writeBytes(fd, 6);
}

// Open serialport
// ***************
int openSerialPort(const char * device, int bps){

   struct termios neu;
   char buf[128];

   //fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
   fd = open(device, O_RDWR | O_NOCTTY);

   if (fd == -1)
   {
      sprintf(buf, "openSerialPort %s error", device);
      perror(buf);
   }
   else
   {
      tcgetattr(fd, &orig);                                 /* save current serial settings */
      tcgetattr(fd, &neu);
      cfmakeraw(&neu);
      cfsetispeed(&neu, bps);
      cfsetospeed(&neu, bps);
      tcflush(fd, TCIFLUSH);
      tcsetattr(fd, TCSANOW, &neu);                         /* set new serial settings */
      fcntl (fd, F_SETFL, O_RDWR);
   }
   return fd;
}

char* itoa(int value, char* result, int base) {
        // check that the base if valid
        if (base < 2 || base > 36) { *result = '\0'; return result; }

        char* ptr = result, *ptr1 = result, tmp_char;
        int tmp_value;

        do {
            tmp_value = value;
            value /= base;
            *ptr++ = "zyxwvutsrqponmlkjihgfedcba9876543210123456789abcdefghijklmnopqrstuvwxyz" [35 + (tmp_value - value * base)];
        } while ( value );

        // Apply negative sign
        if (tmp_value < 0) *ptr++ = '-';
        *ptr-- = '\0';
        while(ptr1 < ptr) {
            tmp_char = *ptr;
            *ptr--= *ptr1;
            *ptr1++ = tmp_char;
        }
        return result;
}

// Write bytes serial to UART
// **************************
void writeBytes(int descriptor, int count) {
    if ((write(descriptor, serialBuffer, count)) == -1) {   // Send data out
        perror("Error writing");
        close(descriptor);                                  // Close port if there is an error
        exit(1);
    }

}

// Read bytes serial from UART
// ***************************
void readBytes(int descriptor, int count) {
    if (read(descriptor, serialBuffer, count) == -1) {      // Read back data into buf[]
        perror("Error reading ");
        close(descriptor);                                  // Close port if there is an error
        exit(1);
    }
}
<commit_msg>Update code<commit_after>#include <iostream>                                         /* allows to perform standard input and output operations */
#include <fstream>
#include <stdio.h>                                          /* Standard input/output definitions */
#include <stdint.h>                                         /* Standard input/output definitions */
#include <stdlib.h>                                         /* defines several general purpose functions */
#include <unistd.h>                                         /* UNIX standard function definitions */
#include <fcntl.h>                                          /* File control definitions */
#include <ctype.h>                                          /* isxxx() */
#include <termios.h>                                        /* POSIX terminal control definitions */
#include <ros/ros.h>                                        /* ROS */
#include <geometry_msgs/Twist.h>                            /* ROS Twist message */
#include <base_controller/encoders.h>                       /* Custom message /encoders */
#include <base_controller/md49data.h>                       /* Custom message /encoders */

const char* serialport_name="/dev/ttyS2";                   /* defines used serialport on BPi. Use "/dev/ttyAMA0" for RPi*/
int serialport_bps=B9600;                                  /* defines used baudrate on serialport */
int fd;                                                     /* serial port file descriptor */
struct termios orig;                                        // backuped port options
int32_t EncoderL;                                           /* stores encoder value left read from md49 */
int32_t EncoderR;                                           /* stores encoder value right read from md49 */
char speed_l=128;
char speed_r=128;                                  /* speed to set for MD49 */
char last_speed_l=128, last_speed_r=128;           /* speed to set for MD49 */
double vr = 0.0;
double vl = 0.0;
double max_vr = 0.2;
double max_vl = 0.2;
double min_vr = 0.2;
double min_vl = 0.2;
double base_width = 0.4;                                    /* Base width in meters */

unsigned char serialBuffer[18];                             /* Serial buffer to store uart data */
void read_MD49_Data (void);
void set_MD49_speed (void);
char* itoa(int value, char* result, int base);

int openSerialPort(const char * device, int bps);
void writeBytes(int descriptor, int count);
void readBytes(int descriptor, int count);

base_controller::encoders encoders;
base_controller::md49data md49data;

void cmd_vel_callback(const geometry_msgs::Twist& vel_cmd){

        if (vel_cmd.linear.x>0){
            speed_l = 255;
            speed_r = 255;
            //set_MD49_speed();
        }
        if (vel_cmd.linear.x<0){
            speed_l = 0;
            speed_r = 0;
            //set_MD49_speed();
        }
        if (vel_cmd.linear.x==0 && vel_cmd.angular.z==0){
            speed_l = 128;
            speed_r = 128;
            //set_MD49_speed();
        }
        if (vel_cmd.angular.z>0){
            speed_l = 0;
            speed_r = 255;
            //set_MD49_speed();
        }
        if (vel_cmd.angular.z<0){
            speed_l = 255;
            speed_r = 0;
            //set_MD49_speed();
        }

    /*
        //ANFANG Alternative
        if (vel_cmd.linear.x==0 && vel_cmd.angular.z==0){vl=0;vr=0;}
        else if(vel_cmd.linear.x == 0){
            // turning
            vr = vel_cmd.angular.z * base_width / 2.0;
            vl = (-1) * vr;
        }
        else if(vel_cmd.angular.z == 0){
            // forward / backward
            vl = vr = vel_cmd.linear.x;
        }
        else{
            // moving doing arcs
            vl = vel_cmd.linear.x - vel_cmd.angular.z * base_width / 2.0;
            if (vl > max_vl) {vl=max_vl;}
            if (vl < min_vl) {vl=min_vl;}
            vr = vel_cmd.linear.x + vel_cmd.angular.z * base_width / 2.0;
            if (vr > max_vr) {vr=max_vr;}
            if (vr < min_vr) {vr=min_vr;}
        }
        //ENDE Alternative
    */
}

int main( int argc, char* argv[] ){

    ros::init(argc, argv, "base_controller" );
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/cmd_vel", 10, cmd_vel_callback);
    ros::Publisher encoders_pub = n.advertise<base_controller::encoders>("encoders",10);
    ros::Publisher md49data_pub = n.advertise<base_controller::md49data>("md49data",10);

    // Init node
    // *********
    ros::Rate loop_rate(10);
    ROS_INFO("base_controller running...");
    ROS_INFO("=============================");
    ROS_INFO("Subscribing to topic /cmd_vel");
    ROS_INFO("Publishing to topic /encoders");
    ROS_INFO("Publishing to topic /md49data");

    // Open serial port
    // ****************
    fd = openSerialPort(serialport_name, serialport_bps);
    if (fd == -1) exit(1);
    ROS_INFO("Opend serial port at %s with %i Bps",serialport_name,serialport_bps);
    usleep(10000);                                          // Sleep for UART to power up and set options

    while(n.ok())
    {
        // Read encoder and other data from MD49
        // (data is read from serial_controller_node
        //  and avaiable through md49_data.txt)
        // *****************************************
//      read_MD49_Data();

        // set speed as in md49speed.txt
        // *****************************
        //if ((speed_l != last_speed_l) || (speed_r != last_speed_r)){
            // gewünschte werte in textfile
            //write_MD49_speed(speed_l,speed_r);
        set_MD49_speed();
        //    last_speed_l=speed_l;
          //  last_speed_r=speed_r;
        //}

        // Publish encoder values to topic /encoders (custom message)
        // **********************************************************
        encoders.encoder_l=EncoderL;
        encoders.encoder_r=EncoderR;
        encoders_pub.publish(encoders);

        // Publish MD49 data to topic /md49data (custom message)
        // *****************************************************
        md49data.speed_l = serialBuffer[8];
        md49data.speed_r = serialBuffer[9];
        md49data.volt = serialBuffer[10];
        md49data.current_l = serialBuffer[11];
        md49data.current_r = serialBuffer[12];
        md49data.error = serialBuffer[13];
        md49data.acceleration = serialBuffer[14];
        md49data.mode = serialBuffer[15];
        md49data.regulator = serialBuffer[16];
        md49data.timeout = serialBuffer[17];
        md49data_pub.publish(md49data);

        // ****
        ros::spinOnce();
        loop_rate.sleep();

    }// end.mainloop

    return 1;
} // end.main

void read_MD49_Data (void){

/*
    // Output MD49 data on screen
    // **************************
    printf("\033[2J");                                      //  clear the screen
    printf("\033[H");                                       //  position cursor at top-left corner
    printf ("MD49-Data read from AVR-Master: \n");
    printf("========================================\n");
    printf("Encoder1 Byte1: %i ",reply[0]);
    printf("Byte2: %i ",reply[1]);
    printf("Byte3: % i ",reply[2]);
    printf("Byte4: %i \n",reply[3]);
    printf("Encoder2 Byte1: %i ",reply[4]);
    printf("Byte2: %i ",reply[5]);
    printf("Byte3: %i ",reply[6]);
    printf("Byte4: %i \n",reply[7]);
    printf("EncoderL: %i ",EncoderL);
    printf("EncoderR: %i \n",EncoderR);
    printf("========================================\n");
    printf("Speed1: %i ",reply[8]);
    printf("Speed2: %i \n",reply[9]);
    printf("Volts: %i \n",reply[10]);
    printf("Current1: %i ",reply[11]);
    printf("Current2: %i \n",reply[12]);
    printf("Error: %i \n",reply[13]);
    printf("Acceleration: %i \n",reply[14]);
    printf("Mode: %i \n",reply[15]);
    printf("Regulator: %i \n",reply[16]);
    printf("Timeout: %i \n",reply[17]);
*/

}

void set_MD49_speed(void){

    serialBuffer[0] = 0;
    serialBuffer[1] = 0x31;					// Command to set motor speed
    serialBuffer[2] = speed_l;				// Speed to be set

    serialBuffer[3] = 0;
    serialBuffer[4] = 0x32;
    serialBuffer[5] = speed_r;

    writeBytes(fd, 6);
}

// Open serialport
// ***************
int openSerialPort(const char * device, int bps){

   struct termios neu;
   char buf[128];

   //fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
   fd = open(device, O_RDWR | O_NOCTTY);

   if (fd == -1)
   {
      sprintf(buf, "openSerialPort %s error", device);
      perror(buf);
   }
   else
   {
      tcgetattr(fd, &orig);                                 /* save current serial settings */
      tcgetattr(fd, &neu);
      cfmakeraw(&neu);
      cfsetispeed(&neu, bps);
      cfsetospeed(&neu, bps);
      tcflush(fd, TCIFLUSH);
      tcsetattr(fd, TCSANOW, &neu);                         /* set new serial settings */
      fcntl (fd, F_SETFL, O_RDWR);
   }
   return fd;
}

char* itoa(int value, char* result, int base) {
        // check that the base if valid
        if (base < 2 || base > 36) { *result = '\0'; return result; }

        char* ptr = result, *ptr1 = result, tmp_char;
        int tmp_value;

        do {
            tmp_value = value;
            value /= base;
            *ptr++ = "zyxwvutsrqponmlkjihgfedcba9876543210123456789abcdefghijklmnopqrstuvwxyz" [35 + (tmp_value - value * base)];
        } while ( value );

        // Apply negative sign
        if (tmp_value < 0) *ptr++ = '-';
        *ptr-- = '\0';
        while(ptr1 < ptr) {
            tmp_char = *ptr;
            *ptr--= *ptr1;
            *ptr1++ = tmp_char;
        }
        return result;
}

// Write bytes serial to UART
// **************************
void writeBytes(int descriptor, int count) {
    if ((write(descriptor, serialBuffer, count)) == -1) {   // Send data out
        perror("Error writing");
        close(descriptor);                                  // Close port if there is an error
        exit(1);
    }

}

// Read bytes serial from UART
// ***************************
void readBytes(int descriptor, int count) {
    if (read(descriptor, serialBuffer, count) == -1) {      // Read back data into buf[]
        perror("Error reading ");
        close(descriptor);                                  // Close port if there is an error
        exit(1);
    }
}
<|endoftext|>


<commit_before>// Copyright (c) 2017 Advanced Micro Devices, Inc. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#ifndef ROCPRIM_ITERATOR_TRANSFORM_ITERATOR_HPP_
#define ROCPRIM_ITERATOR_TRANSFORM_ITERATOR_HPP_

#include <iterator>
#include <cstddef>
#include <type_traits>

#include "../config.hpp"

BEGIN_ROCPRIM_NAMESPACE

template<
    class InputIterator,
    class UnaryFunction,
#if defined(__cpp_lib_is_invocable) && !defined(DOXYGEN_SHOULD_SKIP_THIS) // C++17
    class ValueType =
        typename std::invoke_result<
            UnaryFunction, typename std::iterator_traits<InputIterator>::value_type
        >::type
#else
    class ValueType =
        typename std::result_of<
            UnaryFunction(typename std::iterator_traits<InputIterator>::value_type)
        >::type
#endif
>
class transform_iterator
{
private:
    using input_category = typename std::iterator_traits<InputIterator>::iterator_category;
    static_assert(
        std::is_same<input_category, std::random_access_iterator_tag>::value,
        "InputIterator must be a random-access iterator"
    );

public:
    using value_type = ValueType;
    using reference = const value_type&;
    using pointer = const value_type*;
    using difference_type = typename std::iterator_traits<InputIterator>::difference_type;
    using iterator_category = std::random_access_iterator_tag;
    using unary_function = UnaryFunction;

#ifndef DOXYGEN_SHOULD_SKIP_THIS
    using self_type = transform_iterator;
#endif

    ROCPRIM_HOST_DEVICE inline
    ~transform_iterator() = default;

    ROCPRIM_HOST_DEVICE inline
    transform_iterator(InputIterator iterator, UnaryFunction transform)
        : iterator_(iterator), transform_(transform)
    {
    }

    ROCPRIM_HOST_DEVICE inline
    transform_iterator& operator++()
    {
        iterator_++;
        return *this;
    }

    ROCPRIM_HOST_DEVICE inline
    transform_iterator operator++(int)
    {
        transform_iterator old = *this;
        iterator_++;
        return old;
    }

    ROCPRIM_HOST_DEVICE inline
    value_type operator*() const
    {
        return transform_(*iterator_);
    }

    ROCPRIM_HOST_DEVICE inline
    pointer operator->() const
    {
        return &(*(*this));
    }

    ROCPRIM_HOST_DEVICE inline
    value_type operator[](difference_type distance) const
    {
        transform_iterator i = (*this) + distance;
        return *i;
    }

    ROCPRIM_HOST_DEVICE inline
    transform_iterator operator+(difference_type distance) const
    {
        return transform_iterator(iterator_ + distance, transform_);
    }

    ROCPRIM_HOST_DEVICE inline
    transform_iterator& operator+=(difference_type distance)
    {
        iterator_ += distance;
        return *this;
    }

    ROCPRIM_HOST_DEVICE inline
    transform_iterator operator-(difference_type distance) const
    {
        return transform_iterator(iterator_ - distance, transform_);
    }

    ROCPRIM_HOST_DEVICE inline
    transform_iterator& operator-=(difference_type distance)
    {
        iterator_ -= distance;
        return *this;
    }

    ROCPRIM_HOST_DEVICE inline
    difference_type operator-(transform_iterator other) const
    {
        return iterator_ - other.iterator_;
    }

    ROCPRIM_HOST_DEVICE inline
    bool operator==(transform_iterator other) const
    {
        return iterator_ == other.iterator_;
    }

    ROCPRIM_HOST_DEVICE inline
    bool operator!=(transform_iterator other) const
    {
        return iterator_ != other.iterator_;
    }

    ROCPRIM_HOST_DEVICE inline
    bool operator<(transform_iterator other) const
    {
        return iterator_ < other.iterator_;
    }

    ROCPRIM_HOST_DEVICE inline
    bool operator<=(transform_iterator other) const
    {
        return iterator_ <= other.iterator_;
    }

    ROCPRIM_HOST_DEVICE inline
    bool operator>(transform_iterator other) const
    {
        return iterator_ > other.iterator_;
    }

    ROCPRIM_HOST_DEVICE inline
    bool operator>=(transform_iterator other) const
    {
        return iterator_ >= other.iterator_;
    }

    friend std::ostream& operator<<(std::ostream& os, const transform_iterator& /* iter */)
    {
        return os;
    }

private:
    InputIterator iterator_;
    UnaryFunction transform_;
};

template<
    class InputIterator,
    class UnaryFunction,
    class ValueType
>
ROCPRIM_HOST_DEVICE inline
transform_iterator<InputIterator, UnaryFunction, ValueType>
operator+(typename transform_iterator<InputIterator, UnaryFunction, ValueType>::difference_type distance,
          const transform_iterator<InputIterator, UnaryFunction, ValueType>& iterator)
{
    return iterator + distance;
}

template<
    class InputIterator,
    class UnaryFunction
>
ROCPRIM_HOST_DEVICE inline
transform_iterator<InputIterator, UnaryFunction>
make_transform_iterator(InputIterator iterator, UnaryFunction transform)
{
    return transform_iterator<InputIterator, UnaryFunction>(iterator, transform);
}

END_ROCPRIM_NAMESPACE

#endif // ROCPRIM_ITERATOR_TRANSFORM_ITERATOR_HPP_
<commit_msg>Add docs for transform_iterator<commit_after>// Copyright (c) 2017 Advanced Micro Devices, Inc. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#ifndef ROCPRIM_ITERATOR_TRANSFORM_ITERATOR_HPP_
#define ROCPRIM_ITERATOR_TRANSFORM_ITERATOR_HPP_

#include <iterator>
#include <cstddef>
#include <type_traits>

#include "../config.hpp"

/// \addtogroup iteratormodule
/// @{

BEGIN_ROCPRIM_NAMESPACE

/// \class transform_iterator
/// \brief A random-access input (read-only) iterator adaptor for transforming dereferenced values.
///
/// \par Overview
/// * A transform_iterator uses functor of type UnaryFunction to transform value obtained
/// by dereferencing underlying iterator.
/// * Using it for simulating a range filled with results of applying functor of type
/// \p UnaryFunction to another range saves memory capacity and/or bandwidth.
///
/// \tparam InputIterator - type of the underlying random-access input iterator. Must be
/// a random-access iterator.
/// \tparam UnaryFunction - type of the transform functor.
/// \tparam ValueType - type of value that can be obtained by dereferencing the iterator.
/// By default it is the return type of \p UnaryFunction.
template<
    class InputIterator,
    class UnaryFunction,
#if defined(__cpp_lib_is_invocable) && !defined(DOXYGEN_SHOULD_SKIP_THIS) // C++17
    class ValueType =
        typename std::invoke_result<
            UnaryFunction, typename std::iterator_traits<InputIterator>::value_type
        >::type
#else
    class ValueType =
        typename std::result_of<
            UnaryFunction(typename std::iterator_traits<InputIterator>::value_type)
        >::type
#endif
>
class transform_iterator
{
private:
    using input_category = typename std::iterator_traits<InputIterator>::iterator_category;
    static_assert(
        std::is_same<input_category, std::random_access_iterator_tag>::value,
        "InputIterator must be a random-access iterator"
    );

public:
    /// The type of the value that can be obtained by dereferencing the iterator.
    using value_type = ValueType;
    /// \brief A reference type of the type iterated over (\p value_type).
    /// It's `const` since transform_iterator is a read-only iterator.
    using reference = const value_type&;
    /// \brief A pointer type of the type iterated over (\p value_type).
    /// It's `const` since transform_iterator is a read-only iterator.
    using pointer = const value_type*;
    /// A type used for identify distance between iterators.
    using difference_type = typename std::iterator_traits<InputIterator>::difference_type;
    /// The category of the iterator.
    using iterator_category = std::random_access_iterator_tag;
    /// The type of unary function used to transform input range.
    using unary_function = UnaryFunction;

#ifndef DOXYGEN_SHOULD_SKIP_THIS
    using self_type = transform_iterator;
#endif

    ROCPRIM_HOST_DEVICE inline
    ~transform_iterator() = default;

    /// \brief Creates a new transform_iterator.
    ///
    /// \param iterator input iterator to iterate over and transform.
    /// \param transform unary function used to transform values obtained
    /// from range pointed by \p iterator.
    ROCPRIM_HOST_DEVICE inline
    transform_iterator(InputIterator iterator, UnaryFunction transform)
        : iterator_(iterator), transform_(transform)
    {
    }

    ROCPRIM_HOST_DEVICE inline
    transform_iterator& operator++()
    {
        iterator_++;
        return *this;
    }

    ROCPRIM_HOST_DEVICE inline
    transform_iterator operator++(int)
    {
        transform_iterator old = *this;
        iterator_++;
        return old;
    }

    ROCPRIM_HOST_DEVICE inline
    value_type operator*() const
    {
        return transform_(*iterator_);
    }

    ROCPRIM_HOST_DEVICE inline
    pointer operator->() const
    {
        return &(*(*this));
    }

    ROCPRIM_HOST_DEVICE inline
    value_type operator[](difference_type distance) const
    {
        transform_iterator i = (*this) + distance;
        return *i;
    }

    ROCPRIM_HOST_DEVICE inline
    transform_iterator operator+(difference_type distance) const
    {
        return transform_iterator(iterator_ + distance, transform_);
    }

    ROCPRIM_HOST_DEVICE inline
    transform_iterator& operator+=(difference_type distance)
    {
        iterator_ += distance;
        return *this;
    }

    ROCPRIM_HOST_DEVICE inline
    transform_iterator operator-(difference_type distance) const
    {
        return transform_iterator(iterator_ - distance, transform_);
    }

    ROCPRIM_HOST_DEVICE inline
    transform_iterator& operator-=(difference_type distance)
    {
        iterator_ -= distance;
        return *this;
    }

    ROCPRIM_HOST_DEVICE inline
    difference_type operator-(transform_iterator other) const
    {
        return iterator_ - other.iterator_;
    }

    ROCPRIM_HOST_DEVICE inline
    bool operator==(transform_iterator other) const
    {
        return iterator_ == other.iterator_;
    }

    ROCPRIM_HOST_DEVICE inline
    bool operator!=(transform_iterator other) const
    {
        return iterator_ != other.iterator_;
    }

    ROCPRIM_HOST_DEVICE inline
    bool operator<(transform_iterator other) const
    {
        return iterator_ < other.iterator_;
    }

    ROCPRIM_HOST_DEVICE inline
    bool operator<=(transform_iterator other) const
    {
        return iterator_ <= other.iterator_;
    }

    ROCPRIM_HOST_DEVICE inline
    bool operator>(transform_iterator other) const
    {
        return iterator_ > other.iterator_;
    }

    ROCPRIM_HOST_DEVICE inline
    bool operator>=(transform_iterator other) const
    {
        return iterator_ >= other.iterator_;
    }

    friend std::ostream& operator<<(std::ostream& os, const transform_iterator& /* iter */)
    {
        return os;
    }

private:
    InputIterator iterator_;
    UnaryFunction transform_;
};

template<
    class InputIterator,
    class UnaryFunction,
    class ValueType
>
ROCPRIM_HOST_DEVICE inline
transform_iterator<InputIterator, UnaryFunction, ValueType>
operator+(typename transform_iterator<InputIterator, UnaryFunction, ValueType>::difference_type distance,
          const transform_iterator<InputIterator, UnaryFunction, ValueType>& iterator)
{
    return iterator + distance;
}

/// make_transform_iterator creates a transform_iterator using \p iterator as
/// the underlying iterator and \p transform as the unary function.
///
/// \tparam InputIterator - type of the underlying random-access input iterator.
/// \tparam UnaryFunction - type of the transform functor.
///
/// \param iterator - input iterator.
/// \param transform - transform functor to use in created transform_iterator.
/// \return A new transform_iterator object which transforms the range pointed
/// by \p iterator using \p transform functor.
template<
    class InputIterator,
    class UnaryFunction
>
ROCPRIM_HOST_DEVICE inline
transform_iterator<InputIterator, UnaryFunction>
make_transform_iterator(InputIterator iterator, UnaryFunction transform)
{
    return transform_iterator<InputIterator, UnaryFunction>(iterator, transform);
}

/// @}
// end of group iteratormodule

END_ROCPRIM_NAMESPACE

#endif // ROCPRIM_ITERATOR_TRANSFORM_ITERATOR_HPP_
<|endoftext|>


<commit_before>//
// Created by Ivan Shynkarenka on 09.07.2015.
//

#include "catch.hpp"

#include "benchmark/environment.h"

using namespace CppBenchmark;

TEST_CASE("Environment information", "[CppBenchmark][Environment]")
{
    REQUIRE((Environment::Is32BitOS() || Environment::Is64BitOS()));
    REQUIRE((Environment::Is32BitProcess() || Environment::Is64BitProcess()));
    REQUIRE((Environment::IsDebug() || Environment::IsRelease()));
    REQUIRE((Environment::IsBigEndian() || Environment::IsLittleEndian()));
    REQUIRE(Environment::OSVersion().length() > 0);
    REQUIRE(Environment::Timestamp() > 0);
}
<commit_msg>Tests<commit_after>//
// Created by Ivan Shynkarenka on 09.07.2015.
//

#include "catch.hpp"

#include "benchmark/environment.h"

using namespace CppBenchmark;

TEST_CASE("Environment information", "[CppBenchmark][Environment]")
{
    REQUIRE((Environment::Is32BitOS() || Environment::Is64BitOS()));
    REQUIRE((Environment::Is32BitProcess() || Environment::Is64BitProcess()));
    REQUIRE((Environment::IsDebug() || Environment::IsRelease()));
    REQUIRE((Environment::IsBigEndian() || Environment::IsLittleEndian()));
    REQUIRE(Environment::OSVersion().length() > 0);
    REQUIRE(Environment::EndLine().length() > 0);
    REQUIRE(Environment::Timestamp() > 0);
}
<|endoftext|>


<commit_before>/*! \brief serial_controller_node.
 *         Manages serial communications with AVR-Master.
 *
 *  This node manages serial communications with the AVR-Master.
 *  Therefore it first requests all data from AVR-Master and then sends all
 *  commands to AVR-Master in a Loop. Data read is stored in md49_data.txt,
 *  commands to be send are read from md49_commands.txt.
 *
 */

// Includes
// *********************************************************
#include <iostream>                                         /* allows to perform standard input and output operations */
#include <fstream>                                          /* Input/output stream class to operate on files. */
#include <stdio.h>                                          /* Standard input/output definitions */
#include <stdint.h>                                         /* Standard input/output definitions */
#include <stdlib.h>                                         /* defines several general purpose functions */
#include <unistd.h>                                         /* UNIX standard function definitions */
#include <fcntl.h>                                          /* File control definitions */
#include <errno.h>                                          /* Error number definitions */
#include <termios.h>                                        /* POSIX terminal control definitions */
#include <ctype.h>                                          /* isxxx() */
#include<ros/ros.h>

// Global variables
const char* serialport_name="/dev/ttyAMA0";                      /* defines used serialport */
int serialport_bps=B38400;                                  /* defines used baudrate on serialport */
//int filedesc;                                               // File descriptor of serial port we will talk to
int fd;                                                     /* serial port file descriptor */
int32_t EncoderL;                                           /* stores encoder value left read from md49 */
int32_t EncoderR;                                           /* stores encoder value right read from md49 */
unsigned char speed_l=128, speed_r=128;                     /* speed to set for MD49 */
unsigned char last_speed_l=128, last_speed_r=128;                     /* speed to set for MD49 */
unsigned char serialBuffer[16];                             /* Serial buffer to store uart data */
unsigned char md49_data[18];                                /* keeps data from MD49, read from AVR-Master */
struct termios orig;                                        // Port options

using namespace std;

// Declare functions
// *********************************************************
int openSerialPort(const char * device, int bps);
void writeBytes(int descriptor, int count);
void readBytes(int descriptor, int count);
void read_MD49_Data_serial (void);
void read_md49_commands_txt(void);
void set_MD49_speed (unsigned char speed_l, unsigned char speed_r);
char* itoa(int value, char* result, int base);

int main( int argc, char* argv[] ){

    ros::init(argc, argv, "serial_controller");
    ros::NodeHandle n;

    // Open serial port
    // ****************
    fd = openSerialPort("/dev/ttyAMA0", serialport_bps);
    if (fd == -1) exit(1);
    //ROS_INFO("Opend serial port at %s with %i Bps",serialport_name,serialport_bps);
    usleep(10000);                                          // Sleep for UART to power up and set options

    ROS_DEBUG("Starting Mainloop...");
    ROS_DEBUG("reading data from MD49 and pushing commands to MD49 @ 10Hz...");

    while( 1 ){

        // Read encoder and other data from MD49
        // serial. Data ist stored in md49_data.txt
        // ****************************************
        read_MD49_Data_serial();
        usleep(50000);

        // Read commands from md49_commands.txt:
        // *************************************
        read_md49_commands_txt();

        // Set speed and other commands as
        // read from md49_commands.txt
        // ************************************
        set_MD49_speed(speed_l, speed_r);
        usleep(50000);

    }// end.mainloop
    return 1;
} // end.main

void read_MD49_Data_serial (void){
    serialBuffer[0] = 82;                                   // 82=R Steuerbyte um alle Daten vom MD49 zu lesen
    writeBytes(fd, 1);
    //Daten lesen und in Array schreiben
    readBytes(fd, 18);

    ofstream myfile;
    myfile.open ("md49_data.txt");
    //myfile << "Writing this to a file.\n";
    char buffer[33];

    // Put toghether encoder values from their
    // corresponding bytes, read from MD49
    // ***************************************
    EncoderL = serialBuffer[0] << 24;                       // Put together first encoder value
    EncoderL |= (serialBuffer[1] << 16);
    EncoderL |= (serialBuffer[2] << 8);
    EncoderL |= (serialBuffer[3]);
    EncoderR = serialBuffer[4] << 24;                       // Put together second encoder value
    EncoderR |= (serialBuffer[5] << 16);
    EncoderR |= (serialBuffer[6] << 8);
    EncoderR |= (serialBuffer[7]);

    // write read data from MD49 into md49_data.txt
    // ********************************************
    int i=0;
    for (i=0;i<18;i++){
        if (serialBuffer[i]==0){
            myfile << "000";
        }
        else if (serialBuffer[i]<10){
            myfile << "00";
            myfile << itoa(serialBuffer[i],buffer,10);
        }
        else if (serialBuffer[i]<100){
            myfile << "0";
            myfile << itoa(serialBuffer[i],buffer,10);
        }
        else{
            myfile << itoa(serialBuffer[i],buffer,10);
        }
        myfile << "\n";
    }

    printf("\033[2J");                                      /*  clear the screen  */
    printf("\033[H");                                       /*  position cursor at top-left corner */
    printf ("MD49-Data read from AVR-Master: \n");
    printf("========================================\n");
    printf("Encoder1 Byte1: %i ",serialBuffer[0]);
    printf("Byte2: %i ",serialBuffer[1]);
    printf("Byte3: % i ",serialBuffer[2]);
    printf("Byte4: %i \n",serialBuffer[3]);
    printf("Encoder2 Byte1: %i ",serialBuffer[4]);
    printf("Byte2: %i ",serialBuffer[5]);
    printf("Byte3: %i ",serialBuffer[6]);
    printf("Byte4: %i \n",serialBuffer[7]);
    printf("EncoderL: %i ",EncoderL);
    printf("EncoderR: %i \n",EncoderR);
    printf("========================================\n");
    printf("Speed1: %i ",serialBuffer[8]);
    printf("Speed2: %i \n",serialBuffer[9]);
    printf("Volts: %i \n",serialBuffer[10]);
    printf("Current1: %i ",serialBuffer[11]);
    printf("Current2: %i \n",serialBuffer[12]);
    printf("Error: %i \n",serialBuffer[13]);
    printf("Acceleration: %i \n",serialBuffer[14]);
    printf("Mode: %i \n",serialBuffer[15]);
    printf("Regulator: %i \n",serialBuffer[16]);
    printf("Timeout: %i \n",serialBuffer[17]);
    printf("speed_l = %i \n",speed_l);
    printf("speed_r = %i \n",speed_r);

    myfile.close();
}

void set_MD49_speed (unsigned char speed_l, unsigned char speed_r){
    serialBuffer[0] = 88;                                   // 88 =X Steuerbyte um Commands an MD49 zu senden
    serialBuffer[1] = 115;                                  // 115=s Steuerbyte setSpeed
    serialBuffer[2] = speed_l;                              // set speed1
    serialBuffer[3] = speed_r;                              // set speed2
    writeBytes(fd, 4);
}

void read_md49_commands_txt(void){
    string line;
    ifstream myfile ("md49_commands.txt");
    if (myfile.is_open())
    {
        int i=0;
        while ( getline (myfile,line) )
        {
            //cout << line << '\n';
            char data[10];
            std::copy(line.begin(), line.end(), data);
            md49_data[i]=atoi(data);
            i =i++;
        }
        myfile.close();
        speed_l=md49_data[0];
        speed_r=md49_data[1];
    }
    else cout << "Unable to open file";
}

int openSerialPort(const char * device, int bps){
   struct termios neu;
   char buf[128];

   //fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
   fd = open(device, O_RDWR | O_NOCTTY);

   if (fd == -1)
   {
      sprintf(buf, "openSerialPort %s error", device);
      perror(buf);
   }
   else
   {
      tcgetattr(fd, &orig);                                 /* save current serial settings */
      tcgetattr(fd, &neu);
      cfmakeraw(&neu);
      cfsetispeed(&neu, bps);
      cfsetospeed(&neu, bps);
      tcflush(fd, TCIFLUSH);
      tcsetattr(fd, TCSANOW, &neu);                         /* set new serial settings */
      fcntl (fd, F_SETFL, O_RDWR);
   }
   return fd;
}

void writeBytes(int descriptor, int count) {
    if ((write(descriptor, serialBuffer, count)) == -1) {   // Send data out
        perror("Error writing");
        close(descriptor);                                  // Close port if there is an error
        exit(1);
    }

}

void readBytes(int descriptor, int count) {
    if (read(descriptor, serialBuffer, count) == -1) {      // Read back data into buf[]
        perror("Error reading ");
        close(descriptor);                                  // Close port if there is an error
        exit(1);
    }
}

char* itoa(int value, char* result, int base) {
        // check that the base if valid
        if (base < 2 || base > 36) { *result = '\0'; return result; }

        char* ptr = result, *ptr1 = result, tmp_char;
        int tmp_value;

        do {
            tmp_value = value;
            value /= base;
            *ptr++ = "zyxwvutsrqponmlkjihgfedcba9876543210123456789abcdefghijklmnopqrstuvwxyz" [35 + (tmp_value - value * base)];
        } while ( value );

        // Apply negative sign
        if (tmp_value < 0) *ptr++ = '-';
        *ptr-- = '\0';
        while(ptr1 < ptr) {
            tmp_char = *ptr;
            *ptr--= *ptr1;
            *ptr1++ = tmp_char;
        }
        return result;
    }
<commit_msg>Update code<commit_after>/*! \brief serial_controller_node.
 *         Manages serial communications with AVR-Master.
 *
 *  This node manages serial communications with the AVR-Master.
 *  Therefore it first requests all data from AVR-Master and then sends all
 *  commands to AVR-Master in a Loop. Data read is stored in md49_data.txt,
 *  commands to be send are read from md49_commands.txt.
 *
 */

// Includes
// *********************************************************
#include <iostream>                                         /* allows to perform standard input and output operations */
#include <fstream>                                          /* Input/output stream class to operate on files. */
#include <stdio.h>                                          /* Standard input/output definitions */
#include <stdint.h>                                         /* Standard input/output definitions */
#include <stdlib.h>                                         /* defines several general purpose functions */
#include <unistd.h>                                         /* UNIX standard function definitions */
#include <fcntl.h>                                          /* File control definitions */
#include <errno.h>                                          /* Error number definitions */
#include <termios.h>                                        /* POSIX terminal control definitions */
#include <ctype.h>                                          /* isxxx() */
#include<ros/ros.h>

// Global variables
const char* serialport_name="/dev/ttyAMA0";                      /* defines used serialport */
int serialport_bps=B38400;                                  /* defines used baudrate on serialport */
//int filedesc;                                               // File descriptor of serial port we will talk to
int fd;                                                     /* serial port file descriptor */
int32_t EncoderL;                                           /* stores encoder value left read from md49 */
int32_t EncoderR;                                           /* stores encoder value right read from md49 */
unsigned char speed_l=128, speed_r=128;                     /* speed to set for MD49 */
unsigned char last_speed_l=128, last_speed_r=128;                     /* speed to set for MD49 */
unsigned char serialBuffer[16];                             /* Serial buffer to store uart data */
unsigned char md49_data[18];                                /* keeps data from MD49, read from AVR-Master */
struct termios orig;                                        // Port options

using namespace std;

// Declare functions
// *********************************************************
int openSerialPort(const char * device, int bps);
void writeBytes(int descriptor, int count);
void readBytes(int descriptor, int count);
void read_MD49_Data_serial (void);
void read_md49_commands_txt(void);
void set_MD49_speed (unsigned char speed_l, unsigned char speed_r);
char* itoa(int value, char* result, int base);

int main( int argc, char* argv[] ){

    ros::init(argc, argv, "serial_controller");
    ros::NodeHandle n;

    // Open serial port
    // ****************
    fd = openSerialPort("/dev/ttyAMA0", serialport_bps);
    if (fd == -1) exit(1);
    //ROS_INFO("Opend serial port at %s with %i Bps",serialport_name,serialport_bps);
    usleep(10000);                                          // Sleep for UART to power up and set options

    ROS_DEBUG("Starting Mainloop...");
    ROS_DEBUG("reading data from MD49 and pushing commands to MD49 @ 10Hz...");

    while(  n.ok() ){

        // Read encoder and other data from MD49
        // serial. Data ist stored in md49_data.txt
        // ****************************************
        read_MD49_Data_serial();
        usleep(50000);

        // Read commands from md49_commands.txt:
        // *************************************
        read_md49_commands_txt();

        // Set speed and other commands as
        // read from md49_commands.txt
        // ************************************
        set_MD49_speed(speed_l, speed_r);
        usleep(50000);

    }// end.mainloop
    return 1;
} // end.main

void read_MD49_Data_serial (void){
    serialBuffer[0] = 82;                                   // 82=R Steuerbyte um alle Daten vom MD49 zu lesen
    writeBytes(fd, 1);
    //Daten lesen und in Array schreiben
    readBytes(fd, 18);

    ofstream myfile;
    myfile.open ("md49_data.txt");
    //myfile << "Writing this to a file.\n";
    char buffer[33];

    // Put toghether encoder values from their
    // corresponding bytes, read from MD49
    // ***************************************
    EncoderL = serialBuffer[0] << 24;                       // Put together first encoder value
    EncoderL |= (serialBuffer[1] << 16);
    EncoderL |= (serialBuffer[2] << 8);
    EncoderL |= (serialBuffer[3]);
    EncoderR = serialBuffer[4] << 24;                       // Put together second encoder value
    EncoderR |= (serialBuffer[5] << 16);
    EncoderR |= (serialBuffer[6] << 8);
    EncoderR |= (serialBuffer[7]);

    // write read data from MD49 into md49_data.txt
    // ********************************************
    int i=0;
    for (i=0;i<18;i++){
        if (serialBuffer[i]==0){
            myfile << "000";
        }
        else if (serialBuffer[i]<10){
            myfile << "00";
            myfile << itoa(serialBuffer[i],buffer,10);
        }
        else if (serialBuffer[i]<100){
            myfile << "0";
            myfile << itoa(serialBuffer[i],buffer,10);
        }
        else{
            myfile << itoa(serialBuffer[i],buffer,10);
        }
        myfile << "\n";
    }

    printf("\033[2J");                                      /*  clear the screen  */
    printf("\033[H");                                       /*  position cursor at top-left corner */
    printf ("MD49-Data read from AVR-Master: \n");
    printf("========================================\n");
    printf("Encoder1 Byte1: %i ",serialBuffer[0]);
    printf("Byte2: %i ",serialBuffer[1]);
    printf("Byte3: % i ",serialBuffer[2]);
    printf("Byte4: %i \n",serialBuffer[3]);
    printf("Encoder2 Byte1: %i ",serialBuffer[4]);
    printf("Byte2: %i ",serialBuffer[5]);
    printf("Byte3: %i ",serialBuffer[6]);
    printf("Byte4: %i \n",serialBuffer[7]);
    printf("EncoderL: %i ",EncoderL);
    printf("EncoderR: %i \n",EncoderR);
    printf("========================================\n");
    printf("Speed1: %i ",serialBuffer[8]);
    printf("Speed2: %i \n",serialBuffer[9]);
    printf("Volts: %i \n",serialBuffer[10]);
    printf("Current1: %i ",serialBuffer[11]);
    printf("Current2: %i \n",serialBuffer[12]);
    printf("Error: %i \n",serialBuffer[13]);
    printf("Acceleration: %i \n",serialBuffer[14]);
    printf("Mode: %i \n",serialBuffer[15]);
    printf("Regulator: %i \n",serialBuffer[16]);
    printf("Timeout: %i \n",serialBuffer[17]);
    printf("speed_l = %i \n",speed_l);
    printf("speed_r = %i \n",speed_r);

    myfile.close();
}

void set_MD49_speed (unsigned char speed_l, unsigned char speed_r){
    serialBuffer[0] = 88;                                   // 88 =X Steuerbyte um Commands an MD49 zu senden
    serialBuffer[1] = 115;                                  // 115=s Steuerbyte setSpeed
    serialBuffer[2] = speed_l;                              // set speed1
    serialBuffer[3] = speed_r;                              // set speed2
    writeBytes(fd, 4);
}

void read_md49_commands_txt(void){
    string line;
    ifstream myfile ("md49_commands.txt");
    if (myfile.is_open())
    {
        int i=0;
        while ( getline (myfile,line) )
        {
            //cout << line << '\n';
            char data[10];
            std::copy(line.begin(), line.end(), data);
            md49_data[i]=atoi(data);
            i =i++;
        }
        myfile.close();
        speed_l=md49_data[0];
        speed_r=md49_data[1];
    }
    else cout << "Unable to open file";
}

int openSerialPort(const char * device, int bps){
   struct termios neu;
   char buf[128];

   //fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
   fd = open(device, O_RDWR | O_NOCTTY);

   if (fd == -1)
   {
      sprintf(buf, "openSerialPort %s error", device);
      perror(buf);
   }
   else
   {
      tcgetattr(fd, &orig);                                 /* save current serial settings */
      tcgetattr(fd, &neu);
      cfmakeraw(&neu);
      cfsetispeed(&neu, bps);
      cfsetospeed(&neu, bps);
      tcflush(fd, TCIFLUSH);
      tcsetattr(fd, TCSANOW, &neu);                         /* set new serial settings */
      fcntl (fd, F_SETFL, O_RDWR);
   }
   return fd;
}

void writeBytes(int descriptor, int count) {
    if ((write(descriptor, serialBuffer, count)) == -1) {   // Send data out
        perror("Error writing");
        close(descriptor);                                  // Close port if there is an error
        exit(1);
    }

}

void readBytes(int descriptor, int count) {
    if (read(descriptor, serialBuffer, count) == -1) {      // Read back data into buf[]
        perror("Error reading ");
        close(descriptor);                                  // Close port if there is an error
        exit(1);
    }
}

char* itoa(int value, char* result, int base) {
        // check that the base if valid
        if (base < 2 || base > 36) { *result = '\0'; return result; }

        char* ptr = result, *ptr1 = result, tmp_char;
        int tmp_value;

        do {
            tmp_value = value;
            value /= base;
            *ptr++ = "zyxwvutsrqponmlkjihgfedcba9876543210123456789abcdefghijklmnopqrstuvwxyz" [35 + (tmp_value - value * base)];
        } while ( value );

        // Apply negative sign
        if (tmp_value < 0) *ptr++ = '-';
        *ptr-- = '\0';
        while(ptr1 < ptr) {
            tmp_char = *ptr;
            *ptr--= *ptr1;
            *ptr1++ = tmp_char;
        }
        return result;
    }
<|endoftext|>


<commit_before>/*! \brief serial_controller_node.
 *         Manages serial communications with AVR-Master.
 *
 *  This node manages serial communications with the AVR-Master.
 *  Therefore it first requests all data from AVR-Master and then sends all
 *  commands to AVR-Master in a Loop. Data read is stored in md49_data.txt,
 *  commands to be send are read from md49_commands.txt.
 *
 */

// Includes
// *********************************************************
#include <iostream>                                         /* allows to perform standard input and output operations */
//#include <fstream>                                          /* Input/output stream class to operate on files. */
#include <stdio.h>                                          /* Standard input/output definitions */
#include <stdint.h>                                         /* Standard input/output definitions */
#include <stdlib.h>                                         /* defines several general purpose functions */
//#include <unistd.h>                                         /* UNIX standard function definitions */
#include <fcntl.h>                                          /* File control definitions */
#include <errno.h>                                          /* Error number definitions */
#include <termios.h>                                        /* POSIX terminal control definitions */
//#include <ctype.h>                                          /* isxxx() */
//#include<ros/ros.h>
#include <sqlite3.h>

// switched to experimental, MD49 is now directly connected to RPi
// All AVR- Boards discarded for the moment

// Global variables
const char* serialport_name="//dev/ttyS2";                   /* defines used serialport on BPi. Use "/dev/ttyAMA0" for RPi*/
int serialport_bps=B9600;                                  /* defines used baudrate on serialport */
//int filedesc;                                             /* File descriptor of serial port we will talk to*/
int fd;                                                     /* serial port file descriptor */
int32_t EncoderL;                                           /* stores encoder value left read from md49 */
int32_t EncoderR;                                           /* stores encoder value right read from md49 */
unsigned char speed_l=128, speed_r=128;                     /* speed to set for MD49 */
unsigned char last_speed_l=128, last_speed_r=128;           /* speed to set for MD49 */
unsigned char serialBuffer[16];                             /* Serial buffer to store uart data */
struct termios orig;                                        // backuped port options

// sqlite globals
sqlite3 *db;
char *zErrMsg = 0;
int  rc;
const char *sql;
const char* data = "Callback function called";

using namespace std;

// Declare functions
// *********************************************************
int openSerialPort(const char * device, int bps);
void writeBytes(int descriptor, int count);
void readBytes(int descriptor, int count);
void open_sqlite_db_md49data(void);
static int sql_callback(void *data, int argc, char **argv, char **azColName);
void read_MD49_Data_serial (void);
void read_md49_commands(void);
void set_MD49_speed (unsigned char speed_l, unsigned char speed_r);

int main( int argc, char* argv[] ){

    // Init as ROS node
    // ****************
    //ros::init(argc, argv, "serial_controller");
    //ros::NodeHandle n;

    // Open sqlite database md49data.db
    // ********************************
    open_sqlite_db_md49data();

    // Open serial port
    // ****************
    fd = openSerialPort(serialport_name, serialport_bps);
    if (fd == -1) exit(1);
    //ROS_INFO("Opend serial port at %s with %i Bps",serialport_name,serialport_bps);
    usleep(10000);                                          // Sleep for UART to power up and set options

    //ROS_DEBUG("Starting Mainloop...");
    //ROS_DEBUG("reading data from MD49 and pushing commands to MD49 @ 5Hz...");

    // Mainloop
    // ***********************************
    //while(  n.ok() ){
    while( 1 ){
        // Read encodervalues and other data from MD49
        // serial. Data ist stored in md49_data.txt
        // ****************************************
        read_MD49_Data_serial();
        usleep(200000);

        // Read commands from md49_commands.txt:
        // *************************************
        read_md49_commands();

        // Set speed and other commands as
        // read from md49_commands.txt
        // *******************************
        if ((speed_l != last_speed_l) || (speed_r != last_speed_r)){
            set_MD49_speed(speed_l, speed_r);
            last_speed_l=speed_l;
            last_speed_r=speed_r;
        }

        usleep(200000);

    }// end.mainloop
    sqlite3_close(db);
    return 1;
} // end.main

void read_MD49_Data_serial (void){
    // Read serial MD49 encoder data from AVR-Master
    // *********************************************
    serialBuffer[0] = 0;
	serialBuffer[1] = 0x25;					// Command to return encoder values
	writeBytes(fd, 2);
	readBytes(fd, 8);

    // Put toghether encoder values from their
    // corresponding bytes, read from MD49
    // ***************************************
    EncoderL = serialBuffer[0] << 24;                       // Put together first encoder value
    EncoderL |= (serialBuffer[1] << 16);
    EncoderL |= (serialBuffer[2] << 8);
    EncoderL |= (serialBuffer[3]);
    EncoderR = serialBuffer[4] << 24;                       // Put together second encoder value
    EncoderR |= (serialBuffer[5] << 16);
    EncoderR |= (serialBuffer[6] << 8);
    EncoderR |= (serialBuffer[7]);

    // Write data read from MD49 into
    // sqlite3 database md49data.db
    // ******************************
    char sql_buffer[400];
    int cx;
    // EncoderL & EncoderR
    cx = snprintf (sql_buffer,400,"UPDATE md49data SET EncoderL=%i, EncoderR=%i WHERE ID=1;", EncoderL, EncoderR);
    rc = sqlite3_exec(db, sql_buffer, NULL, 0, &zErrMsg);
    if( rc != SQLITE_OK ){
        fprintf(stderr, "SQL message: %s\n", zErrMsg);
        sqlite3_free(zErrMsg);
    }else{
        //fprintf(stdout, "Operation done successfully\n");
    }
    // Encoderbytes 1-4 left
    cx = snprintf (sql_buffer,400,"UPDATE md49data SET Encoderbyte1L=%i, Encoderbyte2L=%i, " \
                   "Encoderbyte3L=%i, Encoderbyte4L=%i WHERE ID=1;", serialBuffer[0], serialBuffer[1], serialBuffer[2], serialBuffer[3]);
    rc = sqlite3_exec(db, sql_buffer, NULL, 0, &zErrMsg);
    if( rc != SQLITE_OK ){
        fprintf(stderr, "SQL message: %s\n", zErrMsg);
        sqlite3_free(zErrMsg);
    }else{
        //fprintf(stdout, "Operation done successfully\n");
    }
    // Encoderbytes 1-4 right
    cx = snprintf (sql_buffer,400,"UPDATE md49data SET Encoderbyte1R=%i, Encoderbyte2R=%i, " \
                   "Encoderbyte3R=%i, Encoderbyte4R=%i WHERE ID=1;", serialBuffer[4], serialBuffer[5], serialBuffer[6], serialBuffer[7]);
    rc = sqlite3_exec(db, sql_buffer, NULL, 0, &zErrMsg);
    if( rc != SQLITE_OK ){
        fprintf(stderr, "SQL message: %s\n", zErrMsg);
        sqlite3_free(zErrMsg);
    }else{
        //fprintf(stdout, "Operation done successfully\n");
    }

/*
    // SpeedL, SpeedR, Volts, CurrentL, CurrentR, Error, Acceleration, Mode, Regulator, Timeout
    cx = snprintf (sql_buffer,400,"UPDATE md49data SET SpeedL=%i, SpeedR=%i, " \
                   "Volts=%i, CurrentL=%i, CurrentR=%i, Error=%i, Acceleration=%i, Mode=%i, " \
                   "Regulator=%i, Timeout=%i " \
                   "WHERE ID=1;", serialBuffer[8], serialBuffer[9], serialBuffer[10], serialBuffer[11], serialBuffer[12], serialBuffer[13], serialBuffer[14], serialBuffer[15], serialBuffer[16], serialBuffer[17]);
    rc = sqlite3_exec(db, sql_buffer, NULL, 0, &zErrMsg);
    if( rc != SQLITE_OK ){
        fprintf(stderr, "SQL message: %s\n", zErrMsg);
        sqlite3_free(zErrMsg);
    }else{
        //fprintf(stdout, "Operation done successfully\n");
    }
*/
    // Output MD49 data on screen
    // **************************
    printf("\033[2J");                                      //  clear the screen
    printf("\033[H");                                       //  position cursor at top-left corner
    printf ("MD49-Data read from AVR-Master: \n");
    printf("========================================\n");
    printf("Encoder1 Byte1: %i ",serialBuffer[0]);
    printf("Byte2: %i ",serialBuffer[1]);
    printf("Byte3: % i ",serialBuffer[2]);
    printf("Byte4: %i \n",serialBuffer[3]);
    printf("Encoder2 Byte1: %i ",serialBuffer[4]);
    printf("Byte2: %i ",serialBuffer[5]);
    printf("Byte3: %i ",serialBuffer[6]);
    printf("Byte4: %i \n",serialBuffer[7]);
    printf("EncoderL: %i ",EncoderL);
    printf("EncoderR: %i \n",EncoderR);
    printf("========================================\n");
    //printf("SpeedL: %i ",serialBuffer[8]);
    //printf("SpeedR: %i \n",serialBuffer[9]);
    //printf("Volts: %i \n",serialBuffer[10]);
    //printf("CurrentL: %i ",serialBuffer[11]);
    //printf("CurrentR: %i \n",serialBuffer[12]);
    //printf("Error: %i \n",serialBuffer[13]);
    //printf("Acceleration: %i \n",serialBuffer[14]);
    //printf("Mode: %i \n",serialBuffer[15]);
    //printf("Regulator: %i \n",serialBuffer[16]);
    //printf("Timeout: %i \n",serialBuffer[17]);

}

// Write serial command to change left and right speed
// ***************************************************
void set_MD49_speed (unsigned char speed_l, unsigned char speed_r){

    serialBuffer[0] = 0;
	serialBuffer[1] = 0x31;					// Command to set motor speed
	serialBuffer[2] = speed_l;				// Speed to be set

	serialBuffer[3] = 0;
	serialBuffer[4] = 0x32;
	serialBuffer[5] = speed_r;

	writeBytes(fd, 6);
}

// Read SpeedL and SpeedR from
// table md49commands(md49data.db)
// *******************************
void read_md49_commands(void){

   // Create SQL statement
   sql = "SELECT * from md49commands WHERE ID=1";

   // Execute SQL statement
   rc = sqlite3_exec(db, sql, sql_callback, (void*)data, &zErrMsg);
   if( rc != SQLITE_OK ){
      fprintf(stderr, "SQL message: %s\n", zErrMsg);
      sqlite3_free(zErrMsg);
   }else{
      //fprintf(stdout, "Query done successfully\n");
   }
}

// Open serialport
// ***************
int openSerialPort(const char * device, int bps){

   struct termios neu;
   char buf[128];

   //fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
   fd = open(device, O_RDWR | O_NOCTTY);

   if (fd == -1)
   {
      sprintf(buf, "openSerialPort %s error", device);
      perror(buf);
   }
   else
   {
      tcgetattr(fd, &orig);                                 /* save current serial settings */
      tcgetattr(fd, &neu);
      cfmakeraw(&neu);
      cfsetispeed(&neu, bps);
      cfsetospeed(&neu, bps);
      tcflush(fd, TCIFLUSH);
      tcsetattr(fd, TCSANOW, &neu);                         /* set new serial settings */
      fcntl (fd, F_SETFL, O_RDWR);
   }
   return fd;
}

// Write bytes serial to UART
// **************************
void writeBytes(int descriptor, int count) {
    if ((write(descriptor, serialBuffer, count)) == -1) {   // Send data out
        perror("Error writing");
        close(descriptor);                                  // Close port if there is an error
        exit(1);
    }

}

// Read bytes serial from UART
// ***************************
void readBytes(int descriptor, int count) {
    if (read(descriptor, serialBuffer, count) == -1) {      // Read back data into buf[]
        perror("Error reading ");
        close(descriptor);                                  // Close port if there is an error
        exit(1);
    }
}

// Open sqlite db md49data.db and create
// table md49data if not existing
// *************************************
void open_sqlite_db_md49data(void){

    // Open database md49data.db and add table md49data
    // ************************************************
    rc = sqlite3_open("data/md49data.db", &db);
    if( rc ){
        fprintf(stderr, "Can't open database: %s\n", sqlite3_errmsg(db));
        exit(0);
    }else{
        fprintf(stdout, "Opened database successfully,\n");
    }

    // Create table md49data
    // *********************
    sql = "CREATE TABLE md49data("  \
     "ID INT PRIMARY KEY     NOT NULL," \
     "Encoderbyte1L  INT DEFAULT 0, Encoderbyte2L  INT DEFAULT 0," \
     "Encoderbyte3L  INT DEFAULT 0, Encoderbyte4L  INT DEFAULT 0," \
     "Encoderbyte1R  INT DEFAULT 0, Encoderbyte2R  INT DEFAULT 0," \
     "Encoderbyte3R  INT DEFAULT 0, Encoderbyte4R  INT DEFAULT 0," \
     "EncoderL       INT DEFAULT 0, EncoderR       INT DEFAULT 0," \
     "SpeedL         INT DEFAULT 0, SpeedR         INT DEFAULT 0," \
     "Volts          INT DEFAULT 0," \
     "CurrentL       INT DEFAULT 0, CurrentR       INT DEFAULT 0," \
     "Error          INT DEFAULT 0, Acceleration   INT DEFAULT 0," \
     "Mode           INT DEFAULT 0, Regulator      INT DEFAULT 0," \
     "Timeout        INT DEFAULT 0 );" \
     "INSERT INTO md49data (ID) VALUES (1);";

    /* Execute SQL statement */
    rc = sqlite3_exec(db, sql, NULL, 0, &zErrMsg);
    if( rc != SQLITE_OK ){
        fprintf(stderr, "SQL message: %s\n", zErrMsg);
        sqlite3_free(zErrMsg);
    }else{
        fprintf(stdout, "table created successfully\n");
    }
}

// Callbackfunction executed if table md49commands in md49data.db is queried
// *************************************************************************
static int sql_callback(void *data, int argc, char **argv, char **azColName){
   speed_l= atoi(argv[1]);
   speed_r= atoi(argv[2]);
   return 0;
}

<commit_msg>Update code<commit_after>/*! \brief serial_controller_node.
 *         Manages serial communications with AVR-Master.
 *
 *  This node manages serial communications with the AVR-Master.
 *  Therefore it first requests all data from AVR-Master and then sends all
 *  commands to AVR-Master in a Loop. Data read is stored in md49_data.txt,
 *  commands to be send are read from md49_commands.txt.
 *
 */

// Includes
// *********************************************************
#include <iostream>                                         /* allows to perform standard input and output operations */
//#include <fstream>                                          /* Input/output stream class to operate on files. */
#include <stdio.h>                                          /* Standard input/output definitions */
#include <stdint.h>                                         /* Standard input/output definitions */
#include <stdlib.h>                                         /* defines several general purpose functions */
//#include <unistd.h>                                         /* UNIX standard function definitions */
#include <fcntl.h>                                          /* File control definitions */
#include <errno.h>                                          /* Error number definitions */
#include <termios.h>                                        /* POSIX terminal control definitions */
//#include <ctype.h>                                          /* isxxx() */
//#include<ros/ros.h>
#include <sqlite3.h>

// switched to experimental, MD49 is now directly connected to RPi
// All AVR- Boards discarded for the moment

// Global variables
const char* serialport_name="/dev/ttyS2";                   /* defines used serialport on BPi. Use "/dev/ttyAMA0" for RPi*/
int serialport_bps=B9600;                                  /* defines used baudrate on serialport */
//int filedesc;                                             /* File descriptor of serial port we will talk to*/
int fd;                                                     /* serial port file descriptor */
int32_t EncoderL;                                           /* stores encoder value left read from md49 */
int32_t EncoderR;                                           /* stores encoder value right read from md49 */
unsigned char speed_l=128, speed_r=128;                     /* speed to set for MD49 */
unsigned char last_speed_l=128, last_speed_r=128;           /* speed to set for MD49 */
unsigned char serialBuffer[16];                             /* Serial buffer to store uart data */
struct termios orig;                                        // backuped port options

// sqlite globals
sqlite3 *db;
char *zErrMsg = 0;
int  rc;
const char *sql;
const char* data = "Callback function called";

using namespace std;

// Declare functions
// *********************************************************
int openSerialPort(const char * device, int bps);
void writeBytes(int descriptor, int count);
void readBytes(int descriptor, int count);
void open_sqlite_db_md49data(void);
static int sql_callback(void *data, int argc, char **argv, char **azColName);
void read_MD49_Data_serial (void);
void read_md49_commands(void);
void set_MD49_speed (unsigned char speed_l, unsigned char speed_r);

int main( int argc, char* argv[] ){

    // Init as ROS node
    // ****************
    //ros::init(argc, argv, "serial_controller");
    //ros::NodeHandle n;

    // Open sqlite database md49data.db
    // ********************************
    open_sqlite_db_md49data();

    // Open serial port
    // ****************
    fd = openSerialPort(serialport_name, serialport_bps);
    if (fd == -1) exit(1);
    //ROS_INFO("Opend serial port at %s with %i Bps",serialport_name,serialport_bps);
    usleep(10000);                                          // Sleep for UART to power up and set options

    //ROS_DEBUG("Starting Mainloop...");
    //ROS_DEBUG("reading data from MD49 and pushing commands to MD49 @ 5Hz...");

    // Mainloop
    // ***********************************
    //while(  n.ok() ){
    while( 1 ){
        // Read encodervalues and other data from MD49
        // serial. Data ist stored in md49_data.txt
        // ****************************************
        read_MD49_Data_serial();
        usleep(200000);

        // Read commands from md49_commands.txt:
        // *************************************
        read_md49_commands();

        // Set speed and other commands as
        // read from md49_commands.txt
        // *******************************
        if ((speed_l != last_speed_l) || (speed_r != last_speed_r)){
            set_MD49_speed(speed_l, speed_r);
            last_speed_l=speed_l;
            last_speed_r=speed_r;
        }

        usleep(200000);

    }// end.mainloop
    sqlite3_close(db);
    return 1;
} // end.main

void read_MD49_Data_serial (void){
    // Read serial MD49 encoder data from AVR-Master
    // *********************************************
    printf("read serial data");
    serialBuffer[0] = 0;
	serialBuffer[1] = 0x25;					// Command to return encoder values
	writeBytes(fd, 2);
	readBytes(fd, 8);

    // Put toghether encoder values from their
    // corresponding bytes, read from MD49
    // ***************************************
    EncoderL = serialBuffer[0] << 24;                       // Put together first encoder value
    EncoderL |= (serialBuffer[1] << 16);
    EncoderL |= (serialBuffer[2] << 8);
    EncoderL |= (serialBuffer[3]);
    EncoderR = serialBuffer[4] << 24;                       // Put together second encoder value
    EncoderR |= (serialBuffer[5] << 16);
    EncoderR |= (serialBuffer[6] << 8);
    EncoderR |= (serialBuffer[7]);

    // Write data read from MD49 into
    // sqlite3 database md49data.db
    // ******************************
    char sql_buffer[400];
    int cx;
    // EncoderL & EncoderR
    cx = snprintf (sql_buffer,400,"UPDATE md49data SET EncoderL=%i, EncoderR=%i WHERE ID=1;", EncoderL, EncoderR);
    rc = sqlite3_exec(db, sql_buffer, NULL, 0, &zErrMsg);
    if( rc != SQLITE_OK ){
        fprintf(stderr, "SQL message: %s\n", zErrMsg);
        sqlite3_free(zErrMsg);
    }else{
        //fprintf(stdout, "Operation done successfully\n");
    }
    // Encoderbytes 1-4 left
    cx = snprintf (sql_buffer,400,"UPDATE md49data SET Encoderbyte1L=%i, Encoderbyte2L=%i, " \
                   "Encoderbyte3L=%i, Encoderbyte4L=%i WHERE ID=1;", serialBuffer[0], serialBuffer[1], serialBuffer[2], serialBuffer[3]);
    rc = sqlite3_exec(db, sql_buffer, NULL, 0, &zErrMsg);
    if( rc != SQLITE_OK ){
        fprintf(stderr, "SQL message: %s\n", zErrMsg);
        sqlite3_free(zErrMsg);
    }else{
        //fprintf(stdout, "Operation done successfully\n");
    }
    // Encoderbytes 1-4 right
    cx = snprintf (sql_buffer,400,"UPDATE md49data SET Encoderbyte1R=%i, Encoderbyte2R=%i, " \
                   "Encoderbyte3R=%i, Encoderbyte4R=%i WHERE ID=1;", serialBuffer[4], serialBuffer[5], serialBuffer[6], serialBuffer[7]);
    rc = sqlite3_exec(db, sql_buffer, NULL, 0, &zErrMsg);
    if( rc != SQLITE_OK ){
        fprintf(stderr, "SQL message: %s\n", zErrMsg);
        sqlite3_free(zErrMsg);
    }else{
        //fprintf(stdout, "Operation done successfully\n");
    }

/*
    // SpeedL, SpeedR, Volts, CurrentL, CurrentR, Error, Acceleration, Mode, Regulator, Timeout
    cx = snprintf (sql_buffer,400,"UPDATE md49data SET SpeedL=%i, SpeedR=%i, " \
                   "Volts=%i, CurrentL=%i, CurrentR=%i, Error=%i, Acceleration=%i, Mode=%i, " \
                   "Regulator=%i, Timeout=%i " \
                   "WHERE ID=1;", serialBuffer[8], serialBuffer[9], serialBuffer[10], serialBuffer[11], serialBuffer[12], serialBuffer[13], serialBuffer[14], serialBuffer[15], serialBuffer[16], serialBuffer[17]);
    rc = sqlite3_exec(db, sql_buffer, NULL, 0, &zErrMsg);
    if( rc != SQLITE_OK ){
        fprintf(stderr, "SQL message: %s\n", zErrMsg);
        sqlite3_free(zErrMsg);
    }else{
        //fprintf(stdout, "Operation done successfully\n");
    }
*/
    // Output MD49 data on screen
    // **************************
    printf("\033[2J");                                      //  clear the screen
    printf("\033[H");                                       //  position cursor at top-left corner
    printf ("MD49-Data read from AVR-Master: \n");
    printf("========================================\n");
    printf("Encoder1 Byte1: %i ",serialBuffer[0]);
    printf("Byte2: %i ",serialBuffer[1]);
    printf("Byte3: % i ",serialBuffer[2]);
    printf("Byte4: %i \n",serialBuffer[3]);
    printf("Encoder2 Byte1: %i ",serialBuffer[4]);
    printf("Byte2: %i ",serialBuffer[5]);
    printf("Byte3: %i ",serialBuffer[6]);
    printf("Byte4: %i \n",serialBuffer[7]);
    printf("EncoderL: %i ",EncoderL);
    printf("EncoderR: %i \n",EncoderR);
    printf("========================================\n");
    //printf("SpeedL: %i ",serialBuffer[8]);
    //printf("SpeedR: %i \n",serialBuffer[9]);
    //printf("Volts: %i \n",serialBuffer[10]);
    //printf("CurrentL: %i ",serialBuffer[11]);
    //printf("CurrentR: %i \n",serialBuffer[12]);
    //printf("Error: %i \n",serialBuffer[13]);
    //printf("Acceleration: %i \n",serialBuffer[14]);
    //printf("Mode: %i \n",serialBuffer[15]);
    //printf("Regulator: %i \n",serialBuffer[16]);
    //printf("Timeout: %i \n",serialBuffer[17]);

}

// Write serial command to change left and right speed
// ***************************************************
void set_MD49_speed (unsigned char speed_l, unsigned char speed_r){

    serialBuffer[0] = 0;
	serialBuffer[1] = 0x31;					// Command to set motor speed
	serialBuffer[2] = speed_l;				// Speed to be set

	serialBuffer[3] = 0;
	serialBuffer[4] = 0x32;
	serialBuffer[5] = speed_r;

	writeBytes(fd, 6);
}

// Read SpeedL and SpeedR from
// table md49commands(md49data.db)
// *******************************
void read_md49_commands(void){

   // Create SQL statement
   sql = "SELECT * from md49commands WHERE ID=1";

   // Execute SQL statement
   rc = sqlite3_exec(db, sql, sql_callback, (void*)data, &zErrMsg);
   if( rc != SQLITE_OK ){
      fprintf(stderr, "SQL message: %s\n", zErrMsg);
      sqlite3_free(zErrMsg);
   }else{
      //fprintf(stdout, "Query done successfully\n");
   }
}

// Open serialport
// ***************
int openSerialPort(const char * device, int bps){

   struct termios neu;
   char buf[128];

   //fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
   fd = open(device, O_RDWR | O_NOCTTY);

   if (fd == -1)
   {
      sprintf(buf, "openSerialPort %s error", device);
      perror(buf);
   }
   else
   {
      tcgetattr(fd, &orig);                                 /* save current serial settings */
      tcgetattr(fd, &neu);
      cfmakeraw(&neu);
      cfsetispeed(&neu, bps);
      cfsetospeed(&neu, bps);
      tcflush(fd, TCIFLUSH);
      tcsetattr(fd, TCSANOW, &neu);                         /* set new serial settings */
      fcntl (fd, F_SETFL, O_RDWR);
   }
   return fd;
}

// Write bytes serial to UART
// **************************
void writeBytes(int descriptor, int count) {
    if ((write(descriptor, serialBuffer, count)) == -1) {   // Send data out
        perror("Error writing");
        close(descriptor);                                  // Close port if there is an error
        exit(1);
    }

}

// Read bytes serial from UART
// ***************************
void readBytes(int descriptor, int count) {
    if (read(descriptor, serialBuffer, count) == -1) {      // Read back data into buf[]
        perror("Error reading ");
        close(descriptor);                                  // Close port if there is an error
        exit(1);
    }
}

// Open sqlite db md49data.db and create
// table md49data if not existing
// *************************************
void open_sqlite_db_md49data(void){

    // Open database md49data.db and add table md49data
    // ************************************************
    rc = sqlite3_open("data/md49data.db", &db);
    if( rc ){
        fprintf(stderr, "Can't open database: %s\n", sqlite3_errmsg(db));
        exit(0);
    }else{
        fprintf(stdout, "Opened database successfully,\n");
    }

    // Create table md49data
    // *********************
    sql = "CREATE TABLE md49data("  \
     "ID INT PRIMARY KEY     NOT NULL," \
     "Encoderbyte1L  INT DEFAULT 0, Encoderbyte2L  INT DEFAULT 0," \
     "Encoderbyte3L  INT DEFAULT 0, Encoderbyte4L  INT DEFAULT 0," \
     "Encoderbyte1R  INT DEFAULT 0, Encoderbyte2R  INT DEFAULT 0," \
     "Encoderbyte3R  INT DEFAULT 0, Encoderbyte4R  INT DEFAULT 0," \
     "EncoderL       INT DEFAULT 0, EncoderR       INT DEFAULT 0," \
     "SpeedL         INT DEFAULT 0, SpeedR         INT DEFAULT 0," \
     "Volts          INT DEFAULT 0," \
     "CurrentL       INT DEFAULT 0, CurrentR       INT DEFAULT 0," \
     "Error          INT DEFAULT 0, Acceleration   INT DEFAULT 0," \
     "Mode           INT DEFAULT 0, Regulator      INT DEFAULT 0," \
     "Timeout        INT DEFAULT 0 );" \
     "INSERT INTO md49data (ID) VALUES (1);";

    /* Execute SQL statement */
    rc = sqlite3_exec(db, sql, NULL, 0, &zErrMsg);
    if( rc != SQLITE_OK ){
        fprintf(stderr, "SQL message: %s\n", zErrMsg);
        sqlite3_free(zErrMsg);
    }else{
        fprintf(stdout, "table created successfully\n");
    }
}

// Callbackfunction executed if table md49commands in md49data.db is queried
// *************************************************************************
static int sql_callback(void *data, int argc, char **argv, char **azColName){
   speed_l= atoi(argv[1]);
   speed_r= atoi(argv[2]);
   return 0;
}

<|endoftext|>


<commit_before>/*
** Author(s):
**  - Cedric GESTES <gestes@aldebaran-robotics.com>
**
** Copyright (C) 2012 Aldebaran Robotics
*/

#include <gtest/gtest.h>
#include <qi/signal.hpp>
#include <qi/future.hpp>
#include <qi/signalspy.hpp>
#include <qi/anyobject.hpp>
#include <qi/application.hpp>
#include <qi/type/objecttypebuilder.hpp>
#include <qi/type/dynamicobjectbuilder.hpp>

qiLogCategory("test");

class Foo
{
public:
  void func(int)               { }
  void func1(qi::Atomic<int>* r, int)
  {
    // hackish sleep so that asynchronous trigger detection is safer
    qi::os::msleep(1); ++*r;
  }
  void func2(qi::Atomic<int>* r, int, int) { ++*r; }
};
void foo(qi::Atomic<int>* r, int, int)     { ++*r; }
void foo2(qi::Atomic<int>* r, char, char)  { ++*r; }
void foo3(qi::Atomic<int>* r, Foo *)       { ++*r; }
void foolast(int, qi::Promise<void> prom, qi::Atomic<int>* r) { prom.setValue(0); ++*r; }

TEST(TestSignal, TestCompilation)
{
  qi::Atomic<int>        res = 0;
  qi::Signal<int> s;
  Foo*                   f = (Foo*)1;
  qi::Promise<void>      prom;

  //do not count
  s.connect(qi::AnyFunction::from(&Foo::func, f));
  s.connect(boost::bind<void>(&Foo::func, f, _1));

  s.connect(boost::bind(&foo, &res, 12, _1));
  s.connect(boost::bind(&foo2, &res, 12, _1));
  s.connect(boost::bind<void>(&Foo::func1, f, &res, _1));
  s.connect(boost::bind<void>(&Foo::func2, f, &res, 5, _1));
  s.connect(boost::bind(&foo3, &res, f));
  s.connect(boost::bind(&foolast, _1, prom, &res));

  s(42);

  while (*res != 6)
    qi::os::msleep(10);

  ASSERT_EQ(6, *res);
  ASSERT_TRUE(prom.future().isFinished());
  ASSERT_FALSE(prom.future().hasError());
}

void write42(boost::shared_ptr<int> ptr)
{
  *ptr = 42;
}

TEST(TestSignal, SharedPtr)
{
  // Redundant with Copy test, but just to be sure, check that shared_ptr
  // is correctly transmited.
  qi::Signal<boost::shared_ptr<int> > sig;
  sig.connect(qi::AnyFunction::from(&write42)).setCallType(qi::MetaCallType_Queued);
  {
    boost::shared_ptr<int> ptr(new int(12));
    sig(ptr);
  };
}

void byRef(int& i, bool* done)
{
  qiLogDebug() <<"byRef " << &i << ' ' << done;
  i = 12;
  *done = true;
}

TEST(TestSignal, AutoDisconnect)
{
  // Test automatic disconnection when passing shared_ptrs
  qi::Atomic<int> r = 0;
  boost::shared_ptr<Foo> foo(new Foo());
  qi::Signal<qi::Atomic<int>*, int> sig;
  sig.connect(&Foo::func1, boost::weak_ptr<Foo>(foo), _1, _2).setCallType(qi::MetaCallType_Direct);
  sig(&r, 0);
  ASSERT_EQ(1, *r);
  foo.reset();
  sig(&r, 0);
  ASSERT_EQ(1, *r);
}

TEST(TestSignal, BadArity)
{
  // Test runtime detection of arity errors
  qi::Signal<> s;
  // avoid using s.connect() which will catch the problem at compile-time
  EXPECT_ANY_THROW(s.connect(qi::SignalSubscriber(qi::AnyFunction::from(&foo))));
  EXPECT_ANY_THROW(s.connect(qi::SignalSubscriber(qi::AnyFunction::from(&foo2))));
  EXPECT_ANY_THROW(s.connect(qi::AnyFunction::from((boost::function<void(qi::Atomic<int>*, int)>)boost::bind(&Foo::func1, (Foo*)0, _1, _2))));
}

void lol(int v, int& target)
{
  target = v;
}
TEST(TestSignal, SignalSignal)
{
  qi::SignalF<void (int)> sig1;
  qi::SignalF<void (int)> *sig2 = new  qi::SignalF<void (int)>();
  int res = 0;
  sig1.connect(*sig2);
  sig2->connect(boost::bind<void>(&lol, _1, boost::ref(res)));
  sig1(10);
  qi::os::msleep(300);
  ASSERT_EQ(10, res);
  // Test autodisconnect
  delete sig2;
  sig1(20);
  qi::os::msleep(300);
  ASSERT_EQ(10, res);
}

class SignalTest {
public:
  SignalTest() : payload(0) {}
  qi::Signal<int> sig;
  qi::Signal<int> sig2;
  void callback(int val) {
    payload = val;
  }
  int payload;
};

TEST(TestSignal, SignalSignal2)
{
  SignalTest st;
  st.sig2.connect(&SignalTest::callback, boost::ref(st), _1);
  st.sig.connect(st.sig2);
  qiLogDebug() << "sigptrs are " << &st.sig << " " << &st.sig2;
  st.sig(4242);
  for (unsigned i=0; i<50 && st.payload != 4242; ++i)
    qi::os::msleep(20);
  EXPECT_EQ(st.payload, 4242);
}

TEST(TestSignal, SignalN)
{
  qi::Signal<int> sig;
  int res = 0;
  sig.connect(boost::bind<void>(&lol, _1, boost::ref(res)));
  sig(5);
  qi::os::msleep(300);
  ASSERT_EQ(5, res);
}

class SigHolder
{
public:
  qi::Signal<> s0;
  qi::Signal<int> s1;
  qi::Signal<int, int> s2;
  void fire0() { s0();}
  void fire1(int i) { s1(i); }
  void fire2(int i, int j) { s2(i, j);}
};

QI_REGISTER_OBJECT(SigHolder, s0, s1, s2, fire0, fire1, fire2);

TEST(TestSignal, SignalNBind)
{
  int res = 0;
  boost::shared_ptr<SigHolder> so(new SigHolder);
  qi::AnyObject op = qi::AnyReference::from(so).to<qi::AnyObject>();
  qi::details::printMetaObject(std::cerr, op.metaObject());
  op.connect("s1", (boost::function<void(int)>)boost::bind<void>(&lol, _1, boost::ref(res)));
  op.post("s1", 2);
  for (unsigned i=0; i<30 && res!=2; ++i) qi::os::msleep(10);
  ASSERT_EQ(2, res);
  so->s1(3);
  for (unsigned i=0; i<30 && res!=3; ++i) qi::os::msleep(10);
  ASSERT_EQ(3, res);
  so->s0.connect( boost::bind<void>(&lol, 42, boost::ref(res)));
  op.post("s0");
  for (unsigned i=0; i<30 && res!=42; ++i) qi::os::msleep(10);
  ASSERT_EQ(42, res);
}

void foothrow() {
  throw 42;
}

TEST(TestSignal, SignalThrow)
{
  qi::Signal<> sig;
  sig.connect(boost::bind<void>(&foothrow));
  ASSERT_NO_THROW(sig());
  qi::os::msleep(50);
}

void dynTest0(int& tgt)                     { tgt |= 1;}
void dynTest1(int& tgt, int)                { tgt |= 2;}
void dynTest2(int& tgt, int, int)           { tgt |= 4;}
void dynTest3(int& tgt, int, std::string)   { tgt |= 8;}
void dynTestN(int& tgt, const qi::AnyArguments& a)   { tgt |= 32;}
qi::AnyReference dynTestN2(int& tgt, const std::vector<qi::AnyReference>& a)   { tgt |= 16; return qi::AnyReference();}

TEST(TestSignal, Dynamic)
{
  qi::SignalBase s(qi::Signature("m"));
  int trig = 0;
  s.setCallType(qi::MetaCallType_Direct);
  s.connect((boost::function<void()>) boost::bind(&dynTest0, boost::ref(trig)));
  s.connect((boost::function<void(int)>) boost::bind(&dynTest1, boost::ref(trig), _1));
  s.connect((boost::function<void(int, int)>) boost::bind(&dynTest2, boost::ref(trig), _1, _2));
  s.connect((boost::function<void(int, std::string)>) boost::bind(&dynTest3, boost::ref(trig), _1, _2));
  s.connect(qi::AnyFunction::fromDynamicFunction(boost::bind(&dynTestN2, boost::ref(trig), _1)));
  s.connect((boost::function<void(const qi::AnyArguments&)>)(boost::bind(&dynTestN, boost::ref(trig), _1)));
  int a, b;
  std::string c;
  qi::GenericFunctionParameters params;
  s.trigger(params);
  EXPECT_EQ(49, trig);
  params.push_back(qi::AnyReference::from(a));
  trig = 0;
  s.trigger(params);
  EXPECT_EQ(50, trig);
  params.push_back(qi::AnyReference::from(b));
  trig = 0;
  s.trigger(params);
  EXPECT_EQ(52, trig);
  params[1] = qi::AnyReference::from(c);
  trig = 0;
  s.trigger(params);
  EXPECT_EQ(56, trig);
}

void onSubs(boost::atomic<bool>& var, bool subs)
{
  var = subs;
}

void callback(int i)
{}

TEST(TestSignal, OnSubscriber)
{
  boost::atomic<bool> subscribers(false);

  qi::Signal<int> sig(boost::bind(onSubs, boost::ref(subscribers), _1));
  ASSERT_FALSE(subscribers);
  qi::SignalLink l = sig.connect(&callback);
  ASSERT_TRUE(subscribers);
  ASSERT_TRUE(sig.disconnect(l));
  ASSERT_FALSE(subscribers);
}

TEST(TestSignalSpy, Counter)
{
  qi::Signal<int> sig;
  qi::SignalSpy sp(sig);
  QI_EMIT sig(1);
  QI_EMIT sig(1);
  qi::os::sleep(1);
  ASSERT_EQ(sp.getCounter(), 2);

  qi::DynamicObjectBuilder ob;
  ob.advertiseSignal("signal", &sig);
  qi::AnyObject obj(ob.object());
  qi::SignalSpy sp2(obj, "signal");
  QI_EMIT sig(1);
  QI_EMIT sig(1);
  qi::os::sleep(1);
  ASSERT_EQ(sp2.getCounter(), 2);
}

int main(int argc, char **argv) {
  qi::Application app(argc, argv);
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}
<commit_msg>Signals: remove an extra ";" in tests<commit_after>/*
** Author(s):
**  - Cedric GESTES <gestes@aldebaran-robotics.com>
**
** Copyright (C) 2012 Aldebaran Robotics
*/

#include <gtest/gtest.h>
#include <qi/signal.hpp>
#include <qi/future.hpp>
#include <qi/signalspy.hpp>
#include <qi/anyobject.hpp>
#include <qi/application.hpp>
#include <qi/type/objecttypebuilder.hpp>
#include <qi/type/dynamicobjectbuilder.hpp>

qiLogCategory("test");

class Foo
{
public:
  void func(int)               { }
  void func1(qi::Atomic<int>* r, int)
  {
    // hackish sleep so that asynchronous trigger detection is safer
    qi::os::msleep(1); ++*r;
  }
  void func2(qi::Atomic<int>* r, int, int) { ++*r; }
};
void foo(qi::Atomic<int>* r, int, int)     { ++*r; }
void foo2(qi::Atomic<int>* r, char, char)  { ++*r; }
void foo3(qi::Atomic<int>* r, Foo *)       { ++*r; }
void foolast(int, qi::Promise<void> prom, qi::Atomic<int>* r) { prom.setValue(0); ++*r; }

TEST(TestSignal, TestCompilation)
{
  qi::Atomic<int>        res = 0;
  qi::Signal<int> s;
  Foo*                   f = (Foo*)1;
  qi::Promise<void>      prom;

  //do not count
  s.connect(qi::AnyFunction::from(&Foo::func, f));
  s.connect(boost::bind<void>(&Foo::func, f, _1));

  s.connect(boost::bind(&foo, &res, 12, _1));
  s.connect(boost::bind(&foo2, &res, 12, _1));
  s.connect(boost::bind<void>(&Foo::func1, f, &res, _1));
  s.connect(boost::bind<void>(&Foo::func2, f, &res, 5, _1));
  s.connect(boost::bind(&foo3, &res, f));
  s.connect(boost::bind(&foolast, _1, prom, &res));

  s(42);

  while (*res != 6)
    qi::os::msleep(10);

  ASSERT_EQ(6, *res);
  ASSERT_TRUE(prom.future().isFinished());
  ASSERT_FALSE(prom.future().hasError());
}

void write42(boost::shared_ptr<int> ptr)
{
  *ptr = 42;
}

TEST(TestSignal, SharedPtr)
{
  // Redundant with Copy test, but just to be sure, check that shared_ptr
  // is correctly transmited.
  qi::Signal<boost::shared_ptr<int> > sig;
  sig.connect(qi::AnyFunction::from(&write42)).setCallType(qi::MetaCallType_Queued);
  {
    boost::shared_ptr<int> ptr(new int(12));
    sig(ptr);
  };
}

void byRef(int& i, bool* done)
{
  qiLogDebug() <<"byRef " << &i << ' ' << done;
  i = 12;
  *done = true;
}

TEST(TestSignal, AutoDisconnect)
{
  // Test automatic disconnection when passing shared_ptrs
  qi::Atomic<int> r = 0;
  boost::shared_ptr<Foo> foo(new Foo());
  qi::Signal<qi::Atomic<int>*, int> sig;
  sig.connect(&Foo::func1, boost::weak_ptr<Foo>(foo), _1, _2).setCallType(qi::MetaCallType_Direct);
  sig(&r, 0);
  ASSERT_EQ(1, *r);
  foo.reset();
  sig(&r, 0);
  ASSERT_EQ(1, *r);
}

TEST(TestSignal, BadArity)
{
  // Test runtime detection of arity errors
  qi::Signal<> s;
  // avoid using s.connect() which will catch the problem at compile-time
  EXPECT_ANY_THROW(s.connect(qi::SignalSubscriber(qi::AnyFunction::from(&foo))));
  EXPECT_ANY_THROW(s.connect(qi::SignalSubscriber(qi::AnyFunction::from(&foo2))));
  EXPECT_ANY_THROW(s.connect(qi::AnyFunction::from((boost::function<void(qi::Atomic<int>*, int)>)boost::bind(&Foo::func1, (Foo*)0, _1, _2))));
}

void lol(int v, int& target)
{
  target = v;
}
TEST(TestSignal, SignalSignal)
{
  qi::SignalF<void (int)> sig1;
  qi::SignalF<void (int)> *sig2 = new  qi::SignalF<void (int)>();
  int res = 0;
  sig1.connect(*sig2);
  sig2->connect(boost::bind<void>(&lol, _1, boost::ref(res)));
  sig1(10);
  qi::os::msleep(300);
  ASSERT_EQ(10, res);
  // Test autodisconnect
  delete sig2;
  sig1(20);
  qi::os::msleep(300);
  ASSERT_EQ(10, res);
}

class SignalTest {
public:
  SignalTest() : payload(0) {}
  qi::Signal<int> sig;
  qi::Signal<int> sig2;
  void callback(int val) {
    payload = val;
  }
  int payload;
};

TEST(TestSignal, SignalSignal2)
{
  SignalTest st;
  st.sig2.connect(&SignalTest::callback, boost::ref(st), _1);
  st.sig.connect(st.sig2);
  qiLogDebug() << "sigptrs are " << &st.sig << " " << &st.sig2;
  st.sig(4242);
  for (unsigned i=0; i<50 && st.payload != 4242; ++i)
    qi::os::msleep(20);
  EXPECT_EQ(st.payload, 4242);
}

TEST(TestSignal, SignalN)
{
  qi::Signal<int> sig;
  int res = 0;
  sig.connect(boost::bind<void>(&lol, _1, boost::ref(res)));
  sig(5);
  qi::os::msleep(300);
  ASSERT_EQ(5, res);
}

class SigHolder
{
public:
  qi::Signal<> s0;
  qi::Signal<int> s1;
  qi::Signal<int, int> s2;
  void fire0() { s0();}
  void fire1(int i) { s1(i); }
  void fire2(int i, int j) { s2(i, j);}
};

QI_REGISTER_OBJECT(SigHolder, s0, s1, s2, fire0, fire1, fire2)

TEST(TestSignal, SignalNBind)
{
  int res = 0;
  boost::shared_ptr<SigHolder> so(new SigHolder);
  qi::AnyObject op = qi::AnyReference::from(so).to<qi::AnyObject>();
  qi::details::printMetaObject(std::cerr, op.metaObject());
  op.connect("s1", (boost::function<void(int)>)boost::bind<void>(&lol, _1, boost::ref(res)));
  op.post("s1", 2);
  for (unsigned i=0; i<30 && res!=2; ++i) qi::os::msleep(10);
  ASSERT_EQ(2, res);
  so->s1(3);
  for (unsigned i=0; i<30 && res!=3; ++i) qi::os::msleep(10);
  ASSERT_EQ(3, res);
  so->s0.connect( boost::bind<void>(&lol, 42, boost::ref(res)));
  op.post("s0");
  for (unsigned i=0; i<30 && res!=42; ++i) qi::os::msleep(10);
  ASSERT_EQ(42, res);
}

void foothrow() {
  throw 42;
}

TEST(TestSignal, SignalThrow)
{
  qi::Signal<> sig;
  sig.connect(boost::bind<void>(&foothrow));
  ASSERT_NO_THROW(sig());
  qi::os::msleep(50);
}

void dynTest0(int& tgt)                     { tgt |= 1;}
void dynTest1(int& tgt, int)                { tgt |= 2;}
void dynTest2(int& tgt, int, int)           { tgt |= 4;}
void dynTest3(int& tgt, int, std::string)   { tgt |= 8;}
void dynTestN(int& tgt, const qi::AnyArguments& a)   { tgt |= 32;}
qi::AnyReference dynTestN2(int& tgt, const std::vector<qi::AnyReference>& a)   { tgt |= 16; return qi::AnyReference();}

TEST(TestSignal, Dynamic)
{
  qi::SignalBase s(qi::Signature("m"));
  int trig = 0;
  s.setCallType(qi::MetaCallType_Direct);
  s.connect((boost::function<void()>) boost::bind(&dynTest0, boost::ref(trig)));
  s.connect((boost::function<void(int)>) boost::bind(&dynTest1, boost::ref(trig), _1));
  s.connect((boost::function<void(int, int)>) boost::bind(&dynTest2, boost::ref(trig), _1, _2));
  s.connect((boost::function<void(int, std::string)>) boost::bind(&dynTest3, boost::ref(trig), _1, _2));
  s.connect(qi::AnyFunction::fromDynamicFunction(boost::bind(&dynTestN2, boost::ref(trig), _1)));
  s.connect((boost::function<void(const qi::AnyArguments&)>)(boost::bind(&dynTestN, boost::ref(trig), _1)));
  int a, b;
  std::string c;
  qi::GenericFunctionParameters params;
  s.trigger(params);
  EXPECT_EQ(49, trig);
  params.push_back(qi::AnyReference::from(a));
  trig = 0;
  s.trigger(params);
  EXPECT_EQ(50, trig);
  params.push_back(qi::AnyReference::from(b));
  trig = 0;
  s.trigger(params);
  EXPECT_EQ(52, trig);
  params[1] = qi::AnyReference::from(c);
  trig = 0;
  s.trigger(params);
  EXPECT_EQ(56, trig);
}

void onSubs(boost::atomic<bool>& var, bool subs)
{
  var = subs;
}

void callback(int i)
{}

TEST(TestSignal, OnSubscriber)
{
  boost::atomic<bool> subscribers(false);

  qi::Signal<int> sig(boost::bind(onSubs, boost::ref(subscribers), _1));
  ASSERT_FALSE(subscribers);
  qi::SignalLink l = sig.connect(&callback);
  ASSERT_TRUE(subscribers);
  ASSERT_TRUE(sig.disconnect(l));
  ASSERT_FALSE(subscribers);
}

TEST(TestSignalSpy, Counter)
{
  qi::Signal<int> sig;
  qi::SignalSpy sp(sig);
  QI_EMIT sig(1);
  QI_EMIT sig(1);
  qi::os::sleep(1);
  ASSERT_EQ(sp.getCounter(), 2);

  qi::DynamicObjectBuilder ob;
  ob.advertiseSignal("signal", &sig);
  qi::AnyObject obj(ob.object());
  qi::SignalSpy sp2(obj, "signal");
  QI_EMIT sig(1);
  QI_EMIT sig(1);
  qi::os::sleep(1);
  ASSERT_EQ(sp2.getCounter(), 2);
}

int main(int argc, char **argv) {
  qi::Application app(argc, argv);
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}
<|endoftext|>