text:
// this file mainly refers to libint2's example

#include "jlcxx/jlcxx.hpp"
#include <libint2.hpp>
#include <iostream>
#include <thread>
#include <mutex>
#include <Eigen/Dense>

using Matrix = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

static std::mutex mtx;

size_t nbasis(const std::vector<libint2::Shell> &shells)
{
    size_t n = 0;
    for (const auto &shell : shells)
        n += shell.size();
    return n;
}

size_t max_nprim(const std::vector<libint2::Shell> &shells)
{
    size_t n = 0;
    for (auto shell : shells)
        n = std::max(shell.nprim(), n);
    return n;
}

int max_l(const std::vector<libint2::Shell> &shells)
{
    int l = 0;
    for (auto shell : shells)
        for (auto c : shell.contr)
            l = std::max(c.l, l);
    return l;
}

std::vector<libint2::Shell> construct_shells(int Nshell, int *l, double *alpha, double *coeff, double *cart)
{
    std::vector<libint2::Shell> shells;
    for (int i = 0; i < Nshell; ++i)
    {
        double *a = alpha + 3 * i;
        double *d = coeff + 3 * i;
        double *r = cart + 3 * i;
        shells.push_back({{a[0], a[1], a[2]},
                          {{l[i], false, {d[0], d[1], d[2]}}},
                          {{r[0], r[1], r[2]}}});
    }
    return shells;
}

std::vector<size_t> map_shell_to_basis_function(const std::vector<libint2::Shell>& shells) {
  std::vector<size_t> result;
  result.reserve(shells.size());

  size_t n = 0;
  for (auto shell: shells) {
    result.push_back(n);
    n += shell.size();
  }

  return result;
}

void Ve(double *ve, int Nshell, int *l, double *alpha, double *d, double *R) {
  const auto shells = construct_shells(Nshell, l, alpha, d, R);
  const auto n = nbasis(shells);
  auto index = [&n](int i, int j, int k, int l) {
        return n * (n * (n * l + k) + j) + i;
  };
  std::memset(ve, 0, n*n*n*n*sizeof(double));
  libint2::Engine engine(libint2::Operator::coulomb, max_nprim(shells), max_l(shells), 0);

  auto shell2bf = map_shell_to_basis_function(shells);

  const auto& buf = engine.results();

  for(auto s1=0; s1!=shells.size(); ++s1) {

    auto bf1_first = shell2bf[s1];
    auto n1 = shells[s1].size();

    for(auto s2=0; s2!=shells.size(); ++s2) {

      auto bf2_first = shell2bf[s2];
      auto n2 = shells[s2].size();

      for(auto s3=0; s3!=shells.size(); ++s3) {

        auto bf3_first = shell2bf[s3];
        auto n3 = shells[s3].size();

        for(auto s4=0; s4!=shells.size(); ++s4) {

          auto bf4_first = shell2bf[s4];
          auto n4 = shells[s4].size();

          engine.compute(shells[s1], shells[s2], shells[s3], shells[s4]);
          const auto* buf_1234 = buf[0];
          if (buf_1234 == nullptr)
            continue;
          for(auto f1=0, f1234=0; f1!=n1; ++f1) {
            const auto bf1 = f1 + bf1_first;
            for(auto f2=0; f2!=n2; ++f2) {
              const auto bf2 = f2 + bf2_first;
              for(auto f3=0; f3!=n3; ++f3) {
                const auto bf3 = f3 + bf3_first;
                for(auto f4=0; f4!=n4; ++f4, ++f1234) {
                  const auto bf4 = f4 + bf4_first;
                  ve[index(bf1, bf4, bf3, bf2)] = buf_1234[f1234];
                }
              }
            }
          }
        }
      }
    }
  }
}

// copy from libint2's example
void compute_2body_fock(double *veff, double* rho, int Nshell, int *l, double *alpha, double *d, double *R) {
  const auto shells = construct_shells(Nshell, l, alpha, d, R);
  const auto n = nbasis(shells);
  Matrix G = Matrix::Zero(n,n);
  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>> D(rho, n, n);

  // construct the 2-electron repulsion integrals engine
  libint2::Engine engine(libint2::Operator::coulomb, max_nprim(shells), max_l(shells), 0);

  auto shell2bf = map_shell_to_basis_function(shells);

  const auto& buf = engine.results();

  // The problem with the simple Fock builder is that permutational symmetries of the Fock,
  // density, and two-electron integrals are not taken into account to reduce the cost.
  // To make the simple Fock builder efficient we must rearrange our computation.
  // The most expensive step in Fock matrix construction is the evaluation of 2-e integrals;
  // hence we must minimize the number of computed integrals by taking advantage of their permutational
  // symmetry. Due to the multiplicative and Hermitian nature of the Coulomb kernel (and realness
  // of the Gaussians) the permutational symmetry of the 2-e ints is given by the following relations:
  //
  // (12|34) = (21|34) = (12|43) = (21|43) = (34|12) = (43|12) = (34|21) = (43|21)
  //
  // (here we use chemists' notation for the integrals, i.e in (ab|cd) a and b correspond to
  // electron 1, and c and d -- to electron 2).
  //
  // It is easy to verify that the following set of nested loops produces a permutationally-unique
  // set of integrals:
  // foreach a = 0 .. n-1
  //   foreach b = 0 .. a
  //     foreach c = 0 .. a
  //       foreach d = 0 .. (a == c ? b : c)
  //         compute (ab|cd)
  //
  // The only complication is that we must compute integrals over shells. But it's not that complicated ...
  //
  // The real trick is figuring out to which matrix elements of the Fock matrix each permutationally-unique
  // (ab|cd) contributes. STOP READING and try to figure it out yourself. (to check your answer see below)

  // loop over permutationally-unique set of shells
  for(auto s1=0; s1!=shells.size(); ++s1) {

    auto bf1_first = shell2bf[s1]; // first basis function in this shell
    auto n1 = shells[s1].size();   // number of basis functions in this shell

    for(auto s2=0; s2<=s1; ++s2) {

      auto bf2_first = shell2bf[s2];
      auto n2 = shells[s2].size();

      for(auto s3=0; s3<=s1; ++s3) {

        auto bf3_first = shell2bf[s3];
        auto n3 = shells[s3].size();

        const auto s4_max = (s1 == s3) ? s2 : s3;
        for(auto s4=0; s4<=s4_max; ++s4) {

          auto bf4_first = shell2bf[s4];
          auto n4 = shells[s4].size();

          // compute the permutational degeneracy (i.e. # of equivalents) of the given shell set
          auto s12_deg = (s1 == s2) ? 1.0 : 2.0;
          auto s34_deg = (s3 == s4) ? 1.0 : 2.0;
          auto s12_34_deg = (s1 == s3) ? (s2 == s4 ? 1.0 : 2.0) : 2.0;
          auto s1234_deg = s12_deg * s34_deg * s12_34_deg;

          engine.compute(shells[s1], shells[s2], shells[s3], shells[s4]);
          const auto* buf_1234 = buf[0];
          if (buf_1234 == nullptr)
            continue; // if all integrals screened out, skip to next quartet

          // ANSWER
          // 1) each shell set of integrals contributes up to 6 shell sets of the Fock matrix:
          //    F(a,b) += (ab|cd) * D(c,d)
          //    F(c,d) += (ab|cd) * D(a,b)
          //    F(b,d) -= 1/4 * (ab|cd) * D(a,c)
          //    F(b,c) -= 1/4 * (ab|cd) * D(a,d)
          //    F(a,c) -= 1/4 * (ab|cd) * D(b,d)
          //    F(a,d) -= 1/4 * (ab|cd) * D(b,c)
          // 2) each permutationally-unique integral (shell set) must be scaled by its degeneracy,
          //    i.e. the number of the integrals/sets equivalent to it
          // 3) the end result must be symmetrized
          for(auto f1=0, f1234=0; f1!=n1; ++f1) {
            const auto bf1 = f1 + bf1_first;
            for(auto f2=0; f2!=n2; ++f2) {
              const auto bf2 = f2 + bf2_first;
              for(auto f3=0; f3!=n3; ++f3) {
                const auto bf3 = f3 + bf3_first;
                for(auto f4=0; f4!=n4; ++f4, ++f1234) {
                  const auto bf4 = f4 + bf4_first;

                  const auto value = buf_1234[f1234];

                  const auto value_scal_by_deg = value * s1234_deg;

                  G(bf1,bf2) += D(bf3,bf4) * value_scal_by_deg;
                  G(bf3,bf4) += D(bf1,bf2) * value_scal_by_deg;
                  G(bf1,bf3) -= 0.25 * D(bf2,bf4) * value_scal_by_deg;
                  G(bf2,bf4) -= 0.25 * D(bf1,bf3) * value_scal_by_deg;
                  G(bf1,bf4) -= 0.25 * D(bf2,bf3) * value_scal_by_deg;
                  G(bf2,bf3) -= 0.25 * D(bf1,bf4) * value_scal_by_deg;
                }
              }
            }
          }

        }
      }
    }
  }

  // symmetrize the result
  for(int i=0; i<n; ++i)
  {
    for(int j=0; j<n; ++j)
    {
      veff[i*n + j] = (G(i,j) + G(j,i))/4.;
    }
  }
}

JLCXX_MODULE define_julia_module(jlcxx::Module &mod)
{
    mod.method("initialize", [](){libint2::initialize();});
    mod.method("Ve", &Ve);
    mod.method("Veff", &compute_2body_fock);
}


text:
//==================================================================================================
/**
  Copyright 2016 Numscale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_AVX2_SIMD_FUNCTION_IS_GREATER_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_AVX2_SIMD_FUNCTION_IS_GREATER_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/constant/signmask.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;

   BOOST_DISPATCH_OVERLOAD( is_greater_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_<bd::unsigned_<A0>, bs::avx_>
                          , bs::pack_<bd::unsigned_<A0>, bs::avx_>
                          )
   {
      using result = bs::as_logical_t<A0>;
      BOOST_FORCEINLINE result operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        using stype =  bd::as_integer_t<A0, signed>;
        auto const m = Signmask<stype>();
        return bitwise_cast<result>((bitwise_cast<stype>(a0) - m) > (bitwise_cast<stype>(a1) - m));
      }
   };

   BOOST_DISPATCH_OVERLOAD( is_greater_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_< bd::int64_<A0>, bs::avx_>
                          , bs::pack_< bd::int64_<A0>, bs::avx_>
                          )
   {
      BOOST_FORCEINLINE bs::as_logical_t<A0>
      operator()(A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
      {
        return _mm256_cmpgt_epi64(a0,a1);
      }
   };

   BOOST_DISPATCH_OVERLOAD( is_greater_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_< bd::int32_<A0>, bs::avx_>
                          , bs::pack_< bd::int32_<A0>, bs::avx_>
                          )
   {
      BOOST_FORCEINLINE bs::as_logical_t<A0>
      operator()(A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
      {
        return _mm256_cmpgt_epi32(a0,a1);
      }
   };

   BOOST_DISPATCH_OVERLOAD( is_greater_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_< bd::int16_<A0>, bs::avx_>
                          , bs::pack_< bd::int16_<A0>, bs::avx_>
                          )
   {
      BOOST_FORCEINLINE bs::as_logical_t<A0>
      operator()(A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
      {
        return _mm256_cmpgt_epi16(a0,a1);
      }
   };

   BOOST_DISPATCH_OVERLOAD( is_greater_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_< bd::int8_<A0>, bs::avx_>
                          , bs::pack_< bd::int8_<A0>, bs::avx_>
                          )
   {
      BOOST_FORCEINLINE bs::as_logical_t<A0>
      operator()(A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
      {
        return _mm256_cmpgt_epi8(a0,a1);
      }
   };
} } }

#endif

ext: hpp

max_stars_repo_name: SylvainCorlay/pythran

max_issues_repo_name: SylvainCorlay/pythran

max_forks_repo_name: SylvainCorlay/pythran


text:
/**************************************************************************//**
 *
 * @file taurob_base_node.cpp
 * @author Martin Schenk, taurob GmbH
 * @date 30 May 2016
 * @brief Node to integrate taurob base ECU functionality with ROS Control
 *
 *
 *  Copyright (c) 2016 taurob GmbH. All rights reserved.
 *  Perfektastrasse 57/7, 1230 Wien, Austria. office@taurob.com
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 ******************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <string>

#include <ros/ros.h>
#include <ros/param.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Byte.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>

#include <boost/tuple/tuple.hpp>

#include <libtaurob_base/libtaurob_base.h>

#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>

using namespace ros;
using namespace std;

#define DEFAULT_GEAR_RATIO 30.0 // to be verified
#define DEFAULT_WHEEL_DIAMETER 0.250 // 0.37 for argos, 0.303 for tracker, 0.250 for mecanum wheels
#define DEFAULT_TRACK_WIDTH 0.55 // for tracker
#define DEFAULT_TURNING_GEOMETRY_FACTOR 1.476 	// by experimentation

#define DEFAULT_ODOM_LOW_COV 1.0
#define DEFAULT_ODOM_HIGH_COV 1000.0
#define DEFAULT_ODOM_TURN_HIGH_COV_TOLERANCE 0.08

double gear_ratio;
double wheel_diameter;
double track_width;
double turning_geometry_factor;

double angular_vel_z_covariance_threshold; // rad/s

double low_speed_linear_vel_variance_x; // m/s
double low_speed_linear_vel_variance_yz;

double high_speed_linear_vel_variance_x;
double high_speed_linear_vel_variance_yz;

double low_speed_angular_vel_variance_yaw; // rad/s
double low_speed_angular_vel_variance_roll_pitch;

double high_speed_angular_vel_variance_yaw;
double high_speed_angular_vel_variance_roll_pitch;

NodeHandle* nh;

std::string ecu_ip;
int ecu_port;

Subscriber sub_twist;
Subscriber sub_jointstate;
Subscriber sub_light;
Subscriber sub_bluelight;
Subscriber sub_watchdog;
Subscriber sub_enabler;

Publisher pub_twist;
Publisher pub_odom;
Publisher pub_jointstate;
Publisher pub_imu;
Publisher pub_imu_mag;
Publisher pub_imu_acc;
Publisher pub_imu_gyr;
Publisher pub_voltage;
Publisher pub_temperature;
Publisher pub_error_code;
Publisher pub_aux_bits;
Publisher pub_docked;
Publisher pub_remaining_optime;
Publisher pub_current;

Taurob_base* tb = 0;

double ecu_com_diag_min_freq = 4; 	// 1 msg every 250ms
double ecu_com_diag_max_freq = 40; 	// 1 msg every 25ms

diagnostic_updater::HeaderlessTopicDiagnostic* ecu_coms_diag = 0;
diagnostic_updater::Updater* updater;
diagnostic_updater::FunctionDiagnosticTask* temperature_checker;
diagnostic_updater::FunctionDiagnosticTask* voltage_checker;
diagnostic_updater::FunctionDiagnosticTask* current_checker;

float alpha = 0;
float x = 0;
float y = 0;
ros::Time last_rx_frame_time;
tf::TransformBroadcaster* tf_broadcaster;

string tf_from_frame;
string tf_to_frame;
bool publish_tf;
bool watchdog;
bool control_enabled;
string tracker_ip;
int tracker_port;
int protocol_version;

float supply_voltage = 0.0;
unsigned char error_code = 0;
float remaining_optime = 0.0;
float motor_driver_temperature = 0.0;
char dock_received = 0;

bool motor_overcurrent = false;

void publish_odometry_and_tf();
void publish_misc_data();
void publish_imu();
void publish_docked();

void sigint_handler(int sig)
{
    ros::shutdown();
}

double voltage_avg = 0;
const double VOLTAGE_AVG_ELEMENTS = 300;
const double REMAINING_V_TO_MIN_FACTOR = (3 * 30); 	// 1.5 hours, in minutes, with a full charge (1)
const double MAX_VOLTAGE = 25.5;
const double MIN_VOLTAGE = 21;
const int LOW_VOLTAGE_COUNT_THRESHOLD = 40; 		// we receive voltage with ~40 Hz, so 40 means ~1sec
const float LOW_VOLTAGE_THRESHOLD_NO_DRIVING = 21.0f;
const float LOW_VOLTAGE_THRESHOLD_WHILE_DRIVING = 19.5f;
int low_voltage_count = 0;

bool driving = false;

bool shutting_down = false;

// prepare system shutdown in case of low voltage
void prepare_shutdown()
{
	if (shutting_down == false)
	{
		::system("echo \"ATTENTION! THE ROBOT'S BATTERY IS VERY LOW!\" | wall");
		shutting_down = true;
	}
}

double calc_remaining_optime(float voltage)
{
    double ret = 0;

    // only update voltage if we're not driving
    if (tb != 0 && driving == false)
    {
		// update moving average
		if (voltage_avg == 0 && voltage != 0) 	// we just started up
		{
			ROS_INFO("initializing supply voltage averaging with %.1fV", voltage);
			voltage_avg = voltage;
		}
		else
		{
			voltage_avg -= (voltage_avg / VOLTAGE_AVG_ELEMENTS);
			voltage_avg += (voltage / VOLTAGE_AVG_ELEMENTS);
		}
    }

    // calculate remaining time based on avg
    // min voltage is 22, max is 27
    double remaining_v = voltage_avg - MIN_VOLTAGE;
    if (remaining_v < 0) remaining_v = 0;
    if (remaining_v > (MAX_VOLTAGE - MIN_VOLTAGE) + 1) remaining_v = (MAX_VOLTAGE - MIN_VOLTAGE) + 1; 	 	// now we're in [0..(MAX_VOLTAGE - MIN_VOLTAGE) + 1]
    remaining_v /= (MAX_VOLTAGE - MIN_VOLTAGE); 	// map to [0..1.1]
    ret = remaining_v * REMAINING_V_TO_MIN_FACTOR;

    return ret;
}

void check_temperature(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
	if (motor_driver_temperature > 70)
		stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Over 70 degrees");
	else if(motor_driver_temperature > 60)
		stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Over 60 degrees");
    else if(motor_driver_temperature > 50)
		stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Over 50 degrees");
    else
		stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Below 50 degrees");

   stat.add("Motor-driver temperature", motor_driver_temperature);
}

void check_current(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
    if (motor_overcurrent)
    {
	stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Base motor overcurrent!");
    }
    else
    {
	stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Base motor current OK");
    }
    stat.add("Base motor current ok", !motor_overcurrent);
}

void check_voltage(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
	if (tb != 0)
	{
	    float supply_voltage = voltage_avg; 	// we could also use the unfiltered tb->Get_supply_voltage() here
	    boost::tuple<float, float> dist = tb->Get_motor_distance();
	    float threshold = ((get<0>(dist) > 0 || get<1>(dist) > 0) ?
							    LOW_VOLTAGE_THRESHOLD_WHILE_DRIVING :
							    LOW_VOLTAGE_THRESHOLD_NO_DRIVING);

	    if (supply_voltage > 10)
	    {
			if (supply_voltage < 22)
				stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Below 22.0V");
			else if (supply_voltage < 23)
				stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Below 23.0V");
			else
				stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Above 23.0V");

			stat.add("Supply voltage", voltage_avg);

			if (supply_voltage < threshold)
			{
				++low_voltage_count;
				if (low_voltage_count > LOW_VOLTAGE_COUNT_THRESHOLD)
				{
					prepare_shutdown();
				}
			}
			else if (low_voltage_count > 0)
			{
				--low_voltage_count;
			}
	    }
	}
}

void On_frame_received()
{
	//ROS_INFO("A frame was received.\n");
	publish_odometry_and_tf();
	publish_imu();
	publish_misc_data();
	publish_docked();

    ecu_coms_diag->tick();
    updater->update();
}

void publish_docked()
{
	char auxbits = tb->Get_AUX_bits();
	if ((auxbits & 0x01) == 0) 	// in docking station
	{
		if (dock_received < 1)
		{
			dock_received++;
			std_msgs::Bool msg;
			msg.data = true;
			pub_docked.publish(msg);
		}
	}
	else
	{
		if (dock_received > 0)
		{
			dock_received--;
			std_msgs::Bool msg;
			msg.data = false;
			pub_docked.publish(msg);
		}
	}
}

void publish_misc_data()
{
	//ROS_INFO("publishing voltage %f", tb->Get_supply_voltage());
	std_msgs::Float32 fdata;

	motor_driver_temperature = calc_remaining_optime(tb->Get_supply_voltage());
    fdata.data = motor_driver_temperature;
    pub_remaining_optime.publish(fdata);

    supply_voltage = voltage_avg;
    fdata.data = supply_voltage;
    pub_voltage.publish(fdata);

	std_msgs::Byte bdata;

    error_code = tb->Get_error_code();
    bdata.data = error_code;
    pub_error_code.publish(bdata);

	bdata.data = tb->Get_AUX_bits();
	pub_aux_bits.publish(bdata);

    motor_driver_temperature = get<0>(tb->Get_temperatures());
    fdata.data = motor_driver_temperature;
    pub_temperature.publish(fdata);

    double current = tb->Get_average_total_current();
    fdata.data = current;
    pub_current.publish(fdata);
}

void publish_imu()
{
	geometry_msgs::Vector3Stamped vec;
	vec.header.stamp = ros::Time::now();
	vec.header.frame_id = "base_link";

	// swap x, y and make z*=-1 to conform to ros standard
	tuple<float, float, float> data = tb->Get_gyro_data();
	vec.vector.x = get<1>(data);
	vec.vector.y = get<0>(data);
	vec.vector.z = -get<2>(data);
	pub_imu_gyr.publish(vec);

	data = tb->Get_acc_data();
	vec.vector.x = get<1>(data);
	vec.vector.y = get<0>(data);
	vec.vector.z = -get<2>(data);
	pub_imu_acc.publish(vec);

	data = tb->Get_mag_data();
	vec.vector.x = get<1>(data);
	vec.vector.y = get<0>(data);
	vec.vector.z = -get<2>(data);
	pub_imu_mag.publish(vec);
}

void publish_odometry_and_tf()
{
	//On first invocation, just remember timestamp and return.
  	if (last_rx_frame_time == ros::Time(0)){
    	last_rx_frame_time = ros::Time::fromBoost(tb->Get_last_frame_timestamp());
    	return;
  	}

  	// @Todo: Sync those two. For now, we use time diff from rx receive time to compute dt
  	// and ros::Time::now() to set the header stamp
  	ros::Time current_rx_time = ros::Time::fromBoost(tb->Get_last_frame_timestamp());
  	ros::Time current_ros_time = ros::Time::now();

  	float dt = (current_rx_time - last_rx_frame_time).toSec();
  	last_rx_frame_time = current_rx_time;

	// calculate odometry (x, y, alpha) from wheel distances and publish
	boost::tuple<float, float> dist = tb->Get_motor_distance();

	double distance_travelled_left_wheel = (get<0>(dist)
                * wheel_diameter * M_PI) / (gear_ratio * 24);
	double distance_travelled_right_wheel = (get<1>(dist)
			* wheel_diameter * M_PI) / (gear_ratio * 24);

	geometry_msgs::Twist drive_vector;
	drive_vector.linear.x = (distance_travelled_left_wheel
			- distance_travelled_right_wheel) / 2;
	drive_vector.linear.y = 0.0;
	drive_vector.linear.z = 0.0;

	drive_vector.angular.x = 0.0;
	drive_vector.angular.y = 0.0;
	drive_vector.angular.z = (double) (distance_travelled_left_wheel
			+ distance_travelled_right_wheel)
			/ (track_width * turning_geometry_factor);

	float dAlpha = drive_vector.angular.z;

  /*

	// calculate "covariance" -- whenever we turn, the error will be high, so set the covariance high
	double cov = odom_low_cov;
	if (dAlpha > odom_turn_high_cov_tolerance)
	{
		cov = odom_high_cov;
	}
  */

    alpha = alpha + dAlpha;
	float dx = drive_vector.linear.x * cos(alpha);
	float dy = drive_vector.linear.x * sin(alpha);

	x = x + dx;
	y = y + dy;

	ROS_DEBUG("translation: x = %g, y = %g rotation: alpha = %g", x, y, alpha);

	// also, calculate velocities via dt
	float vx = dx / dt;
	float vy = dy / dt;
	float valpha = dAlpha / dt;

	// convert distance in drive_vector.linear to speed -> divide by publish rate
	drive_vector.linear.x /= dt;
	drive_vector.linear.y /= dt;

  	if (std::abs(drive_vector.linear.x) > 2.0 ){
    	ROS_ERROR("Implausible linear velocity: %f for dt: %f. Not publishing odometry!", drive_vector.linear.x, dt);
    	return;
  	}

	// publish the odometry message over ROS
	nav_msgs::Odometry odom;
	odom.header.stamp = current_ros_time;
	odom.header.frame_id = tf_to_frame;

	odom.pose.pose.position.x = x;
	odom.pose.pose.position.y = y;
	odom.pose.pose.position.z = 0.0;
	geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(alpha);
	odom.pose.pose.orientation = odom_quat;

  // Set covariances for pose data to -1.0 to signify that it should not be used.
  // Position uncertainty grows unbounded over time, so should not write misleading
  // covariance data here.
  //for (size_t i = 0; i < 36; ++i){
  //  odom.pose.covariance[i] = 100000.0;
  //}
  odom.pose.covariance[0] = 100000.0;
  odom.pose.covariance[7] = 100000.0;
  odom.pose.covariance[14] = 100000.0;
  odom.pose.covariance[21] = 100000.0;
  odom.pose.covariance[28] = 100000.0;
  odom.pose.covariance[35] = 100000.0;

	// set velocity
	odom.child_frame_id = tf_from_frame;
	odom.twist.twist.linear.x = drive_vector.linear.x;
	odom.twist.twist.linear.y = 0.0;
	odom.twist.twist.angular.z = valpha;

	// covariance
  if (std::abs(odom.twist.twist.angular.z) > angular_vel_z_covariance_threshold){
      odom.twist.covariance[0] = high_speed_linear_vel_variance_x;
      odom.twist.covariance[7] = high_speed_linear_vel_variance_yz;
      odom.twist.covariance[14] = high_speed_linear_vel_variance_yz;

      odom.twist.covariance[21] = high_speed_angular_vel_variance_roll_pitch;
      odom.twist.covariance[28] = high_speed_angular_vel_variance_roll_pitch;
      odom.twist.covariance[35] = high_speed_angular_vel_variance_yaw;
  }else{
      odom.twist.covariance[0] = low_speed_linear_vel_variance_x;
      odom.twist.covariance[7] = low_speed_linear_vel_variance_yz;
      odom.twist.covariance[14] = low_speed_linear_vel_variance_yz;

      odom.twist.covariance[21] = low_speed_angular_vel_variance_roll_pitch;
      odom.twist.covariance[28] = low_speed_angular_vel_variance_roll_pitch;
      odom.twist.covariance[35] = low_speed_angular_vel_variance_yaw;
  }

	pub_odom.publish(odom);

	// broadcast tf
	if (publish_tf == true)
	{
		tf::Quaternion rotation = tf::Quaternion(tf::Vector3(0, 0, 1.0), alpha); // rotation from Y axis
		tf::Vector3 translation = tf::Vector3(x, y, 0.0); // in meters
		tf::Transform transform = tf::Transform(rotation, translation);

		tf::StampedTransform stampedTransform = tf::StampedTransform(transform, last_rx_frame_time, tf_to_frame, tf_from_frame);

		tf_broadcaster->sendTransform(stampedTransform);
	}
}

void twist_cmd_callback(const geometry_msgs::Twist::ConstPtr& msg)
{
    //ROS_INFO("received twist");
    if (tb != 0)
    {
		//	ROS_INFO("sending twist");
		tb->Set_drive_command(make_tuple(msg->linear.x, msg->angular.z));
    }
    if (msg->linear.x != 0 || msg->angular.z != 0) driving = true;
    else driving = false;
}

void jointstate_cmd_callback(const sensor_msgs::JointState::ConstPtr& msg)
{
	if (tb != 0)
	{
		// check if the joint state contains a joint called "gripper_front"
		std::vector<std::string> jointnames = msg->name;
		int index = 0;

		for (std::vector<string>::iterator it = jointnames.begin(); it != jointnames.end(); ++it)
		{
			const char* mit = (*it).c_str();
			if (strcmp(mit, "flipper_front") == 0)
			{
				// gripper_front was found -- get the set angle for it (position, in rad -> convert to degrees)
				float setpos = msg->position[index] * (180.0f/M_PI);
				ROS_DEBUG("setting flipper angle to %f", setpos);
				tb->Set_gripper_angle(setpos);

				// since there is nothing but grippers for now, break here
				break;
			}
			index++;
		}
	}
}

void light_callback(const std_msgs::Bool::ConstPtr& msg)
{
	if (tb != 0)
	{
		tb->Set_light(msg->data);
	}
}

void bluelight_callback(const std_msgs::Bool::ConstPtr& msg)
{
	if (tb != 0)
	{
		tb->Set_bluelight(msg->data);
	}
}

void watchdog_feed_callback(const std_msgs::Bool::ConstPtr& msg)
{
	if (tb != 0 && msg->data == true)
	{
		tb->Feed_watchdog();
	}
}

void enable_control_callback(const std_msgs::Bool::ConstPtr& msg)
{
	if (tb != 0)
	{
		ROS_INFO("setting control to %s", (msg->data ? (char*)"true" : (char*)"false"));
		tb->Set_pause_sending(!(msg->data)); 	// if control enabled, pause=false. if control disabled, pause=true.
	}
}

void get_parameters()
{
    ros::NodeHandle local_nh = NodeHandle(ros::this_node::getName());

    local_nh.param<bool>("publish_tf", publish_tf, false);
    local_nh.param<double>("wheel_diameter", wheel_diameter, DEFAULT_WHEEL_DIAMETER);
    local_nh.param<double>("track_width", track_width, DEFAULT_TRACK_WIDTH);
    local_nh.param<double>("gear_ratio", gear_ratio, DEFAULT_GEAR_RATIO);
    local_nh.param<double>("turning_geometry_factor", turning_geometry_factor, DEFAULT_TURNING_GEOMETRY_FACTOR);
  //local_nh.param<double>("odom_low_covariance", odom_low_cov, DEFAULT_ODOM_LOW_COV);
  //local_nh.param<double>("odom_high_covariance", odom_high_cov, DEFAULT_ODOM_HIGH_COV);
  //local_nh.param<double>("odom_turn_high_cov_tolerance", odom_turn_high_cov_tolerance, DEFAULT_ODOM_TURN_HIGH_COV_TOLERANCE);
    local_nh.param<string>("tf_from_frame", tf_from_frame, "/base_link");
    local_nh.param<string>("tf_to_frame", tf_to_frame, "/odom");
    local_nh.param<bool>("watchdog", watchdog, true);
    local_nh.param<bool>("control_enabled_at_startup", control_enabled, false);
    local_nh.param<string>("base_ip", tracker_ip, "10.0.0.2");
    local_nh.param<int>("base_port", tracker_port, 8080);
    local_nh.param<int>("protocol_version", protocol_version, 2); 	// default protocol is 2 (since that is the current one at the time of this writing)

    //covariance_treatment
    double tmp;
    local_nh.param<double>("angular_vel_z_covariance_threshold_deg", tmp, 1.0);
    angular_vel_z_covariance_threshold = tmp * (M_PI/180.0);

    local_nh.param<double>("low_speed_linear_vel_std_dev_x", tmp, 0.025);
    low_speed_linear_vel_variance_x = tmp * tmp;
    local_nh.param<double>("low_speed_linear_vel_std_dev_yz", tmp, 0.01);
    low_speed_linear_vel_variance_yz = tmp * tmp;

    local_nh.param<double>("high_speed_linear_vel_std_dev_x", tmp, 0.05);
    high_speed_linear_vel_variance_x = tmp * tmp;
    local_nh.param<double>("high_speed_linear_vel_std_dev_yz", tmp, 0.05);
    high_speed_linear_vel_variance_yz = tmp * tmp;

    local_nh.param<double>("low_speed_angular_vel_std_dev_yaw_deg", tmp, 3.0);
    low_speed_angular_vel_variance_yaw = ( (tmp * (M_PI/180.0)) * (tmp * (M_PI/180.0)) );
    local_nh.param<double>("low_speed_angular_vel_std_dev_roll_pitch_deg", tmp, 5.0);
    low_speed_angular_vel_variance_roll_pitch = ( (tmp * (M_PI/180.0)) * (tmp * (M_PI/180.0)) );

    local_nh.param<double>("high_speed_angular_vel_std_dev_yaw_deg", tmp, 120.0);
    high_speed_angular_vel_variance_yaw = ( (tmp * (M_PI/180.0)) * (tmp * (M_PI/180.0)) );
    local_nh.param<double>("high_speed_angular_vel_std_dev_roll_pitch_deg", tmp, 30.0);
    high_speed_angular_vel_variance_roll_pitch = ( (tmp * (M_PI/180.0)) * (tmp * (M_PI/180.0)) );

}

void init()
{
    signal(SIGINT, sigint_handler);

    get_parameters();
  	last_rx_frame_time = ros::Time(0);

    sub_twist = nh->subscribe("cmd_vel", 1, &twist_cmd_callback);
    sub_jointstate = nh->subscribe("jointstate_cmd", 1, &jointstate_cmd_callback);
    sub_light = nh->subscribe("light", 1, &light_callback);
    sub_bluelight = nh->subscribe("bluelight", 1, &bluelight_callback);
    sub_watchdog = nh->subscribe("watchdog_feed", 1, &watchdog_feed_callback);
    sub_enabler = nh->subscribe("enable_control", 1, &enable_control_callback);

    // TODO: commented lines would be available, but still need to be published
    //pub_twist = nh->advertise<geometry_msgs::Twist>("twist_status", 1); 		// does this make sense when we have odom?
    pub_odom = nh->advertise<nav_msgs::Odometry>("odom", 1);
    //pub_jointstate = nh->advertise<sensor_msgs::JointState>("jointstate_status", 1);
    //pub_imu = nh->advertise<sensor_msgs::Imu>("imu_data", 1);
    pub_imu_mag = nh->advertise<geometry_msgs::Vector3Stamped>("imu_magnetometer", 1);
    pub_imu_acc = nh->advertise<geometry_msgs::Vector3Stamped>("imu_accelerometer", 1);
    pub_imu_gyr = nh->advertise<geometry_msgs::Vector3Stamped>("imu_gyroscope", 1);
    pub_voltage = nh->advertise<std_msgs::Float32>("supply_voltage", 1);
    pub_temperature = nh->advertise<std_msgs::Float32>("temperatures", 1);
    pub_error_code = nh->advertise<std_msgs::Byte>("error_code", 1);
    pub_aux_bits = nh->advertise<std_msgs::Byte>("aux_bits", 1);
    pub_remaining_optime = nh->advertise<std_msgs::Float32>("remaining_optime", 1);
	pub_docked = nh->advertise<std_msgs::Bool>("robot_docked", 1);
	pub_current = nh->advertise<std_msgs::Float32>("total_current", 1);

    updater->setHardwareID("taurob_base");

    temperature_checker = new diagnostic_updater::FunctionDiagnosticTask
			    (
				    "Motor-driver temperature",
				    boost::bind(&check_temperature, _1)
			    );
    voltage_checker = new diagnostic_updater::FunctionDiagnosticTask
			    (
				    "Supply voltage",
				    boost::bind(&check_voltage, _1)
			    );
    current_checker = new diagnostic_updater::FunctionDiagnosticTask
			    (
				    "Motors overcurrent",
				    boost::bind(&check_current, _1)
			    );
    updater->add(*temperature_checker);
    updater->add(*voltage_checker);
    updater->add(*current_checker);

    ecu_coms_diag = new diagnostic_updater::HeaderlessTopicDiagnostic
			    (
				"Base ECU Communication",
				*updater,
				diagnostic_updater::FrequencyStatusParam(
				    &ecu_com_diag_min_freq,
				    &ecu_com_diag_max_freq,
				    0.1,
				    10)
			    );
    ecu_coms_diag->addTask(temperature_checker);
    ecu_coms_diag->addTask(voltage_checker);
    ecu_coms_diag->addTask(current_checker);

    updater->force_update();
	ROS_INFO("Base node initialization complete");
}

void On_error(int error_code)
{
	if (error_code & 0x40)
	{
		ROS_ERROR("OVERCURRENT DETECTED! Communication with ECU has ceased to prevent hardware damage. Check if the tracks or wheels are obstructed! Afterwards, you need to restart the node to try to continue.");
	}
	if (error_code & 0x20)
	{
		ROS_WARN("The operating voltage is too high!");
	}
	if (error_code & 0x10)
	{
		ROS_WARN("Error communicating with on-board IMU");
	}
	if (error_code & 0x08)
	{
		ROS_ERROR("Motor fault detected! Communication with ECU has ceased to prevent hardware damage. Check if the tracks or wheels are obstructed! Afterwards, you need to restart the robot and the node to try to continue.");
	}
	if (error_code & 0x04)
	{
		ROS_INFO("SPI error 4 reported");
	}
	if (error_code & 0x02)
	{
		ROS_INFO("SPI error 2 reported");
	}
	if ((error_code & 0x01 != 0) ||
	    (error_code & 0x80 != 0))
	{
		ROS_WARN("Received unknown error: code %d", error_code);
	}
}

int main(int argc, char **argv)
{
	ROS_INFO("starting base node");
	ros::init(argc, argv, "taurob_base_node");

	NodeHandle rosnh;
	nh = &rosnh;

	tf::TransformBroadcaster tf_bc;
	tf_broadcaster = &tf_bc;

    diagnostic_updater::Updater base_node_updater;
    updater = &base_node_updater;

    init();
    ROS_INFO("\ntaurob base node is running.\n");

	tb = new Taurob_base(tracker_ip, tracker_port, protocol_version, control_enabled);
	tb->Set_on_received_callback(&On_frame_received);
	tb->Set_on_error_callback(&On_error);
	tb->Set_watchdog_enabled(watchdog);
	tb->Run();

    while (ros::ok())
    {
	    ros::spinOnce();
	    updater->update();
	    ros::Duration(0.02).sleep(); 	// 20ms
    }

    if (ecu_coms_diag != 0) delete ecu_coms_diag;
    if (temperature_checker != 0) delete temperature_checker;
    if (voltage_checker != 0) delete voltage_checker;

    tb->Stop();
    delete tb;
    tb = 0;

    ROS_INFO("Terminating.");
    exit(0);
}

ext: cpp

max_stars_repo_name: taurob/taurobtrackerapi

max_issues_repo_name: SimonSchwaiger/taurobtrackerapi

max_forks_repo_name: SimonSchwaiger/taurobtrackerapi


text:
/*
 * Copyright (c) 2013 Juniper Networks, Inc. All rights reserved.
 */

#include <boost/uuid/uuid_io.hpp>
#include <cmn/agent_cmn.h>

#include <base/logging.h>
#include <oper/interface.h>
#include <oper/nexthop.h>
#include <oper/tunnel_nh.h>
#include <oper/vrf.h>
#include <oper/agent_sandesh.h>
#include <oper/inet4_mcroute.h>
#include <oper/multicast.h>
#include <oper/mirror_table.h>

#include <sandesh/sandesh_types.h>
#include <sandesh/sandesh_constants.h>
#include <sandesh/sandesh.h>
#include <sandesh/sandesh_trace.h>

using namespace std;

MulticastHandler *MulticastHandler::obj_;
SandeshTraceBufferPtr MulticastTraceBuf(SandeshTraceBufferCreate("Multicast",
                                                                     1000));
/*
 * Local funtion to derive subnet broadcast
 */
static Ip4Address GetSubnetBroadcastAddress(const Ip4Address &ip_prefix,
                                            uint32_t plen) {
    Ip4Address broadcast(ip_prefix.to_ulong() |
                         ~(0xFFFFFFFF << (32 - plen)));
    return broadcast;
}

/*
 * Local function to find if its a subnet member
 */
static bool IsSubnetMember(const Ip4Address &ip, const Ip4Address &ip_prefix,
                           uint32_t plen) {
    return ((ip_prefix.to_ulong() | ~(0xFFFFFFFF << (32 - plen))) ==
            (ip.to_ulong() | ~(0xFFFFFFFF << (32 - plen))));
}

/*
 * Helper to send route change for xmpp listener
 */
void NotifyXMPPofRecipientChange(const std::string &vrf_name,
                                 const Ip4Address &dip,
                                 const std::string &vn_name) {
    boost::system::error_code ec;
    if (!IS_BCAST_MCAST(dip)) {
        MCTRACE(Log, "notify subnet route chg", vrf_name, dip.to_string(), 0);
        Inet4UcRouteTable::AddSubnetBroadcastRoute(Agent::GetInstance()->GetLocalVmPeer(),
                                                   vrf_name,
                                                   IpAddress::from_string("0.0.0.0", ec).to_v4(),
                                                   dip, vn_name);
    }
}

/*
 * Registeration for notification
 * VM - Looking for local VM added
 * VN - Looking for subnet information from VN
 * Enable trace print messages
 */
void MulticastHandler::Register() {
    Agent::GetInstance()->GetVnTable()->Register(boost::bind(&MulticastHandler::ModifyVNIpam,
                                              _1, _2));
    Agent::GetInstance()->GetInterfaceTable()->Register(boost::bind(&MulticastHandler::ModifyVmInterface,
                                                     _1, _2));

    MulticastHandler::GetInstance()->GetMulticastObjList().clear();
}

/*
 * Route address 255.255.255.255 addition from first VM in VN add
 */
void MulticastHandler::AddBroadcastRoute(const std::string &vrf_name,
                                         const Ip4Address &addr)
{
    boost::system::error_code ec;
    MCTRACE(Log, "add bcast route ", vrf_name, addr.to_string(), 0);
    Inet4McRouteTable::AddV4MulticastRoute(vrf_name,
                             IpAddress::from_string("0.0.0.0", ec).to_v4(),
                             addr);
}

/*
 * Route address 255.255.255.255 deletion from last VM in VN del
 */
void MulticastHandler::DeleteBroadcastRoute(const std::string &vrf_name,
                                            const Ip4Address &addr)
{
    boost::system::error_code ec;
    MCTRACE(Log, "delete bcast route ", vrf_name, addr.to_string(), 0);
    Inet4McRouteTable::DeleteV4MulticastRoute(vrf_name,
                             IpAddress::from_string("0.0.0.0", ec).to_v4(),
                             addr);
}

/*
 * Called from IPAM addition in VN.
 * Creates empty comp NH and add the route pointing to it.
 * Later VM additions result in modification of comp NH
 */
void MulticastHandler::AddSubnetRoute(const std::string &vrf_name,
                                      const Ip4Address &addr,
                                      const std::string &vn_name)
{
    DBRequest req;
    NextHopKey *key;
    boost::system::error_code ec;
    CompositeNHData *cnh_data;

    MCTRACE(Log, "subnet comp NH creation ", vrf_name, addr.to_string(), 0);
    key = new CompositeNHKey(vrf_name, addr,
                             IpAddress::from_string("0.0.0.0", ec).to_v4(), false);
    req.oper = DBRequest::DB_ENTRY_ADD_CHANGE;
    req.key.reset(key);
    cnh_data = new CompositeNHData();
    req.data.reset(cnh_data);
    Agent::GetInstance()->GetNextHopTable()->Enqueue(&req);

    MCTRACE(Log, "subnet route ", vrf_name, addr.to_string(), 0);
    Inet4UcRouteTable::AddSubnetBroadcastRoute(Agent::GetInstance()->GetLocalVmPeer(),
                                 vrf_name,
                                 IpAddress::from_string("0.0.0.0", ec).to_v4(),
                                 addr, vn_name);

    MulticastGroupObject *subnet_broadcast =
        this->FindGroupObject(vrf_name, addr);
    if (subnet_broadcast != NULL) {
        subnet_broadcast->Deleted(false);
        MCTRACE(Log, "mc obj rt added for subnet route ",
                vrf_name, addr.to_string(), 0);
    }
}

/*
 * Delete the subnet route for which IPAM is gone.
 * Comp NH goes off as referring route goes off and refcount is zero
 */
void MulticastHandler::DeleteSubnetRoute(const std::string &vrf_name,
                                         const Ip4Address &addr)
{
    MCTRACE(Log, "delete subnet route ", vrf_name, addr.to_string(), 0);
    Inet4UcRouteTable::DeleteReq(Agent::GetInstance()->GetLocalVmPeer(),
                                 vrf_name, addr, 32);
    MulticastGroupObject *subnet_broadcast =
        this->FindGroupObject(vrf_name, addr);
    if (subnet_broadcast != NULL) {
        subnet_broadcast->Deleted(true);
        subnet_broadcast->SetSourceMPLSLabel(0);
        MCTRACE(Log, "rt gone mc obj marked for deletion for subnet route ",
                vrf_name, addr.to_string(), 0);
    }
}

/*
 * Delete corresponding subnet route using IPAM of VN and delete ipam/vn
 * Also release vm if any from unresolved vm list dependant on this VN
 */
void MulticastHandler::DeleteVnIPAM(const VnEntry *vn)
{
    std::map<uuid, std::vector<VnIpam> > &vn_ipam_map =
        this->GetIpamMap();
    std::vector<VnIpam> ipam_list = vn_ipam_map[vn->GetUuid()];
    Ip4Address broadcast_addr;

    MCTRACE(Info, "delete vnipam for ", vn->GetName());
    if (ipam_list.size() == 0) {
        MCTRACE(Info, "delete vnipam found no ipam for ", vn->GetName());
        return;
    }

    boost::system::error_code ec;
    std::vector<VnIpam>::iterator it = ipam_list.begin();
    while (it != ipam_list.end()) {
        broadcast_addr = (*it).GetBroadcastAddress();
        //Delete route
        MCTRACE(Log, "vn delete, remove subnet route ",
                GetAssociatedVrfForVn(vn->GetUuid()),
                broadcast_addr.to_string(), 0);
        DeleteSubnetRoute(GetAssociatedVrfForVn(vn->GetUuid()),
                          broadcast_addr);

        /*
        Inet4UcRouteTable::DeleteReq(Agent::GetInstance()->GetLocalVmPeer(),
                                     GetAssociatedVrfForVn(vn->GetUuid()),
                                     broadcast_addr, 32);
                                     */
        //Erase will give the next object
        it = ipam_list.erase(it);
    }

    vn_ipam_map.erase(vn->GetUuid());
    this->RemoveVrfVnAssociation(vn->GetUuid());
}

/*
 * IPAM change handling
 * Delete subnet route of non existing IPAM
 * Add subnet route for new IPAM
 * No changes for retained IPAM
 * As this function is used for creation/modification also visit
 * unresolved VM list
 */
void MulticastHandler::HandleIPAMChange(const VnEntry *vn,
                                        const std::vector<VnIpam> &ipam)
{
    bool evaluate_vmlist = false;
    //std::map<uuid, std::vector<VnIpam> > &vn_ipam_map =
    //    this->GetIpamMap();
    std::vector<VnIpam> &old_ipam = this->GetIpamMapList(vn->GetUuid());
    //vm list is evaluated when vn is late or has been deleted and
    //added again. In both the cases local ipam goes is flushed and that
    //means vm need to be revisited. In rest of the cases where ipam is
    //populated VM already had been seen.
    if (old_ipam.size() == 0) {
        evaluate_vmlist = true;
    }

    //TODO Optimize by not keeping vnipam but subnet bcast address
    std::sort(old_ipam.begin(), old_ipam.end());
    std::vector<VnIpam>::iterator old_it = old_ipam.begin();
    std::vector<VnIpam>::const_iterator new_it = ipam.begin();

    //Diff and delete or add
    while ((old_it != old_ipam.end()) && (new_it != ipam.end())) {
        if (*old_it < *new_it) {
            DeleteSubnetRoute(vn->GetVrf()->GetName(),
                              (*old_it).GetBroadcastAddress());
            old_it = old_ipam.erase(old_it);
        } else if (*new_it < *old_it) {
            AddSubnetRoute(vn->GetVrf()->GetName(),
                           (*new_it).GetBroadcastAddress(),
                           vn->GetName());
            old_it = old_ipam.insert(old_it, (*new_it));
        } else {
            old_it++;
            new_it++;
        }
    }

    //Residual element handling
    while (old_it != old_ipam.end()) {
        DeleteSubnetRoute(vn->GetVrf()->GetName(),
                          (*old_it).GetBroadcastAddress());
        old_it = old_ipam.erase(old_it);
    }
    for (; new_it != ipam.end(); new_it++) {
        AddSubnetRoute(vn->GetVrf()->GetName(),
                       (*new_it).GetBroadcastAddress(),
                       vn->GetName());
        old_ipam.push_back(*new_it);
    }

    //Push back the common elements and new elements
    assert(old_ipam.size() == ipam.size());

    //Visit any unresolved vm list
    if (evaluate_vmlist == true) {
        this->VisitUnresolvedVMList(vn);
    }
    //Store the vrf name for this VN
    this->SetVrfNameForVn(vn->GetUuid(), vn->GetVrf()->GetName());
}

/*
 * VM dependant on VN will be moved to participating stage
 * This can happen when VM comes before VN is active with VRF and IPAM.
 */
void MulticastHandler::VisitUnresolvedVMList(const VnEntry *vn)
{
    const Interface *intf = NULL;
    const VmPortInterface *vm_itf = NULL;
    std::vector<VnIpam> &ipam = this->GetIpamMapList(vn->GetUuid());

    assert(ipam.size() != 0);

    //Now go thru unresolved vm list
    std::list<const VmPortInterface *> vmitf_list =
        this->GetUnresolvedSubnetVMList(vn->GetUuid());

    if (vmitf_list.size() == 0) {
        return;
    }
    for (std::list<const VmPortInterface *>::iterator it_itf = vmitf_list.begin();
         it_itf != vmitf_list.end();) {

        vm_itf = (*it_itf);
        intf = static_cast<const Interface *>(vm_itf);

        if ((vm_itf == NULL) || (intf->GetActiveState() != true)) {
            //Delete vm itf
            it_itf++;
            continue;
        }
        for (std::vector<VnIpam>::const_iterator it = ipam.begin();
            it != ipam.end(); it++) {
            if (IsSubnetMember(vm_itf->GetIpAddr(), (*it).ip_prefix, (*it).plen)) {
                this->AddVmInterface(vm_itf->GetVrf()->GetName(),
                      GetSubnetBroadcastAddress(vm_itf->GetIpAddr(),
                      (*it).plen), vm_itf->GetUuid(),
                      vn->GetName());
            }
        }
        it_itf++;
    }
}

/* Regsitered call for VN */
void MulticastHandler::ModifyVNIpam(DBTablePartBase *partition, DBEntryBase *e)
{
    const VnEntry *vn = static_cast<const VnEntry *>(e);
    const std::vector<VnIpam> &ipam = vn->GetVnIpam();

    MCTRACE(Info, "Modifyvnipam for ", vn->GetName());
    if ((vn->IsDeleted() == true) || (ipam.size() == 0) ||
        (vn->GetVrf() == NULL)) {
        MulticastHandler::GetInstance()->DeleteVnIPAM(vn);
        return;
    }

    //Now store the ipam for this vn and handle related ops
    MCTRACE(Info, "Modifyvnipam for handle ipam change ", vn->GetName());
    MulticastHandler::GetInstance()->HandleIPAMChange(vn, ipam);
}

/* Registered call for VM */
void MulticastHandler::ModifyVmInterface(DBTablePartBase *partition,
                                         DBEntryBase *e)
{
    const Interface *intf = static_cast<const Interface *>(e);
    const VmPortInterface *vm_itf;

    if (intf->GetType() != Interface::VMPORT) {
        return;
    }

    if (intf->IsDeleted() || intf->GetActiveState() == false) {
        MulticastHandler::GetInstance()->DeleteVmInterface(intf);
        return;
    }

    vm_itf = static_cast<const VmPortInterface *>(intf);
    Ip4Address vm_itf_ip = vm_itf->GetIpAddr();
    assert(vm_itf->GetVnEntry() != NULL);

    //PIck up ipam from local ipam list
    std::vector<VnIpam> ipam = MulticastHandler::GetInstance()->
                       GetIpamMapList(vm_itf->GetVnEntry()->GetUuid());
    for(std::vector<VnIpam>::const_iterator it = ipam.begin();
        it != ipam.end(); it++) {
        if (IsSubnetMember(vm_itf_ip, (*it).ip_prefix, (*it).plen)) {
            MCTRACE(Log, "vm interface add being issued for ",
                    vm_itf->GetVrf()->GetName(),
                    vm_itf->GetIpAddr().to_string(), 0);
            MulticastHandler::GetInstance()->AddVmInterface(
                              vm_itf->GetVrf()->GetName(),
                              GetSubnetBroadcastAddress(vm_itf->GetIpAddr(),
                              (*it).plen), vm_itf->GetUuid(),
                              vm_itf->GetVnEntry()->GetName());
            break;
        }
    }

    MCTRACE(Log, "vm ipam not found, add to unresolve ",
            vm_itf->GetVrf()->GetName(),
            vm_itf->GetIpAddr().to_string(), 0);
    MulticastHandler::GetInstance()->AddToUnresolvedSubnetVMList(
                                           vm_itf->GetVnEntry()->GetUuid(),
                                           vm_itf);
    return;
}

/*
 * Delete the old mpls label and add the new mpls label if it is non zero
 * Delete route if group is non subnet broadcast, subnet bcast is deleted
 * via IPAM going off
 */
void MulticastHandler::DeleteRouteandMPLS(MulticastGroupObject *obj)
{
    //delete mcast routes, subnet bcast gets deleted via vn delete
    if (IS_BCAST_MCAST(obj->GetGroupAddress())) {
        Inet4McRouteTable::DeleteV4MulticastRoute(obj->GetVrfName(),
                                                  obj->GetSourceAddress(),
                                                  obj->GetGroupAddress());
    }
    /* delete the MPLS label route */
    obj->SetSourceMPLSLabel(0);
    MCTRACE(Log, "delete route mpls ", obj->GetVrfName(),
            obj->GetGroupAddress().to_string(),
            obj->GetSourceMPLSLabel());
}

/*
 * Delete VM interface
 * Traverse the multicast obj list of which this VM is a member.
 * Delete the VM from them and check if local VM list size is zero.
 * If it is zero then delete the route and mpls.
 */
void MulticastHandler::DeleteVmInterface(const Interface *intf)
{
    const VmPortInterface *vm_itf = static_cast<const VmPortInterface *>(intf);
    std::list<MulticastGroupObject *> &obj_list = this->GetVmToMulticastObjMap(
                                                          vm_itf->GetUuid());
    for (std::list<MulticastGroupObject *>::iterator it = obj_list.begin();
         it != obj_list.end(); it++) {
        // When IPAM/VN goes off first than VM then it marks mc obj
        // for deletion. Cleanup of related data structures like vm-mcobj
        // happens when VM goes off. So dont trigger any notify at same time.
        // However if all local VMs are gone then route will be deleted only
        // when VN/IPAM goes off. At that time notify xmpp to unsubscribe
        //Deletelocalmember removes vm from mc obj
        if (((*it)->DeleteLocalMember(vm_itf->GetUuid()) == true) &&
            ((*it)->IsDeleted() == false) &&
            ((*it)->GetLocalListSize() != 0)) {
            this->TriggerCompositeNHChange(*it);
            MCTRACE(Log, "trigger cnh  ", (*it)->GetVrfName(),
                    (*it)->GetGroupAddress().to_string(),
                    (*it)->GetSourceMPLSLabel());
        }

        if((*it)->GetLocalListSize() == 0) {
            MCTRACE(Info, "Del vm notify ", vm_itf->GetIpAddr().to_string());
            //Empty tunnel list
            (*it)->FlushAllFabricOlist();
            //Update comp nh
            if ((*it)->IsDeleted() == false) {
                this->TriggerCompositeNHChange(*it);
                NotifyXMPPofRecipientChange((*it)->GetVrfName(),
                                            (*it)->GetGroupAddress(),
                                            (*it)->GetVnName());
            }
            //Time to delete route(for mcast address) and mpls
            this->DeleteRouteandMPLS(*it);
            /* delete mcast object */
            this->DeleteMulticastObject((*it)->vrf_name_, (*it)->grp_address_);
        }
    }
    //Remove all mc obj references from reverse map of vm to mc obj and the
    //delete the uuid of vm
    this->vm_to_mcobj_list_[vm_itf->GetUuid()].clear();
    this->DeleteVmToMulticastObjMap(vm_itf->GetUuid());
    this->DeleteVMFromUnResolvedList(vm_itf);
    MCTRACE(Info, "Del vm notify done ", vm_itf->GetIpAddr().to_string());
}

//Delete multicast object for vrf/G
void MulticastHandler::DeleteMulticastObject(const std::string &vrf_name,
                                             const Ip4Address &grp_addr) {
    MCTRACE(Log, "delete obj  vrf/grp/size ", vrf_name, grp_addr.to_string(),
        this->GetMulticastObjList().size());
    for(std::set<MulticastGroupObject *>::iterator it =
        this->GetMulticastObjList().begin();
        it != this->GetMulticastObjList().end(); it++) {
        if (((*it)->GetVrfName() == vrf_name) &&
            ((*it)->GetGroupAddress() == grp_addr)) {
            delete (*it);
            this->GetMulticastObjList().erase(it++);
            break;
        }
    }
}

//Helper to find object for VRF/G
MulticastGroupObject *MulticastHandler::FindGroupObject(const std::string &vrf_name,
                                                        const Ip4Address &dip) {
    for(std::set<MulticastGroupObject *>::iterator it =
        this->GetMulticastObjList().begin();
        it != this->GetMulticastObjList().end(); it++) {
        if (((*it)->GetVrfName() == vrf_name) &&
            ((*it)->GetGroupAddress() == dip)) {
            return (*it);
        }
    }
    MCTRACE(Log, "mcast obj size ", vrf_name, dip.to_string(),
            this->GetMulticastObjList().size());
    return NULL;
}

/*
 * Send the updated list for composite NH
 * Updated list is derived from local and tunnel olist
 */
void MulticastHandler::TriggerCompositeNHChange(MulticastGroupObject *obj)
{
    DBRequest req;
    NextHopKey *key;
    std::vector<ComponentNHData> data;
    CompositeNHData *cnh_data;

    for (TunnelOlist::const_iterator it = obj->GetTunnelOlist().begin();
         it != obj->GetTunnelOlist().end(); it++) {
        ComponentNHData nh_data(it->label_, Agent::GetInstance()->GetDefaultVrf(),
                                Agent::GetInstance()->GetRouterId(), it->daddr_, false,
                                it->tunnel_bmap_);
        data.push_back(nh_data);
    }

    for (std::list<uuid>::const_iterator it = obj->GetLocalOlist().begin();
         it != obj->GetLocalOlist().end(); it++) {
        ComponentNHData nh_data(0, (*it), true);
        data.push_back(nh_data);
    }

    MCTRACE(Log, "enqueue comp ", obj->GetVrfName(),
            obj->GetGroupAddress().to_string(), data.size());
    key = new CompositeNHKey(obj->GetVrfName(), obj->GetGroupAddress(),
                             obj->GetSourceAddress(), false);
    req.oper = DBRequest::DB_ENTRY_ADD_CHANGE;
    req.key.reset(key);
    cnh_data = new CompositeNHData(data, CompositeNHData::REPLACE);
    req.data.reset(cnh_data);
    Agent::GetInstance()->GetNextHopTable()->Enqueue(&req);
}

/*
 * VM addition helper
 * Adds 255.255.255.255 route
 * Adds subnet broadcast if VN Ipam is present
 * If VN IPAM is not present then add to unresolved list
 * so that it can be added later
 */
void MulticastHandler::AddVmInterface(const std::string &vrf_name,
                                      const Ip4Address &dip,
                                      const uuid &intf_uuid,
                                      const std::string &vn_name) {
    MulticastGroupObject *all_broadcast = NULL;
    MulticastGroupObject *subnet_broadcast = NULL;
    boost::system::error_code ec;
    Ip4Address broadcast =  IpAddress::from_string("255.255.255.255",
                                                   ec).to_v4();
    bool add_route = false;

    //All broadcast addition 255.255.255.255
    all_broadcast = this->FindGroupObject(vrf_name, broadcast);
    if (all_broadcast == NULL) {
        all_broadcast = new MulticastGroupObject(vrf_name, broadcast, vn_name);
        this->AddToMulticastObjList(all_broadcast);
        add_route = true;
    }
    if (all_broadcast->AddLocalMember(intf_uuid) == true) {
        this->TriggerCompositeNHChange(all_broadcast);
        //Trigger route add
        if (add_route) {
            this->AddBroadcastRoute(vrf_name, broadcast);
        }
        this->AddVmToMulticastObjMap(intf_uuid, all_broadcast);
    }

    //Subnet broadcast
    add_route = false;
    subnet_broadcast = this->FindGroupObject(vrf_name, dip);
    if (subnet_broadcast == NULL) {
        subnet_broadcast = new MulticastGroupObject(vrf_name, dip, vn_name);
        this->AddToMulticastObjList(subnet_broadcast);
    }
    if (subnet_broadcast->GetLocalListSize() == 0) {
        add_route = true;
    }
    if (subnet_broadcast->AddLocalMember(intf_uuid) == true) {
        this->TriggerCompositeNHChange(subnet_broadcast);
        this->AddVmToMulticastObjMap(intf_uuid, subnet_broadcast);
        //Dummy notification to let xmpp know that cnh is populated
        if (add_route) {
            NotifyXMPPofRecipientChange(vrf_name, dip, vn_name);
        }
    }
}

/*
 * Release all info coming via ctrl node for this multicast object
 */
bool MulticastGroupObject::ModifyFabricMembers(const TunnelOlist &olist)
{
    DBRequest req;
    NextHopKey *key;
    TunnelNHData *tnh_data;

    GetTunnelOlist().clear();

    for (TunnelOlist::const_iterator it = olist.begin();
         it != olist.end(); it++) {
        AddMemberInTunnelOlist(it->label_, it->daddr_, it->tunnel_bmap_);

        key = new TunnelNHKey(Agent::GetInstance()->GetDefaultVrf(), Agent::GetInstance()->GetRouterId(),
                              it->daddr_, false,
                              TunnelType::ComputeType(it->tunnel_bmap_));
        tnh_data = new TunnelNHData();
        req.oper = DBRequest::DB_ENTRY_ADD_CHANGE;
        req.key.reset(key);
        req.data.reset(tnh_data);
        Agent::GetInstance()->GetNextHopTable()->Enqueue(&req);
        MCTRACE(Log, "Enqueue add TUNNEL ", Agent::GetInstance()->GetDefaultVrf(),
                it->daddr_.to_string(), it->label_);
    }
    return true;
}

/*
 * Static funtion to be called to handle XMPP info from ctrl node
 * Key is VRF/G/S
 * Info has label (for source to vrouter) and
 * OLIST of NH (server IP + label for that server)
 */
void MulticastHandler::ModifyFabricMembers(const std::string &vrf_name,
                                           const Ip4Address &grp,
                                           const Ip4Address &src,
                                           uint32_t label,
                                           const TunnelOlist &olist)
{
    MulticastGroupObject *obj =
        MulticastHandler::GetInstance()->FindGroupObject(vrf_name, grp);
    MCTRACE(Log, "XMPP call multicast handler ", vrf_name, grp.to_string(), label);

    if ((obj == NULL) || obj->IsDeleted()) {
        MCTRACE(Log, "Multicast object deleted ", vrf_name,
                grp.to_string(), label);
        return;
    }

    obj->SetSourceMPLSLabel(label);
    if (obj->ModifyFabricMembers(olist) == true) {
        MulticastHandler::GetInstance()->TriggerCompositeNHChange(obj);
    }

    MCTRACE(Log, "Add fabric grp label ", vrf_name, grp.to_string(), label);
}

//Helper to delete fabric nh
void MulticastGroupObject::FlushAllFabricOlist() {
    GetTunnelOlist().clear();
}

/*
 * XMPP peer has gone down, so flush off all the information
 * coming via control node i.e. MPLS label and tunnel info
 */
void MulticastHandler::HandlePeerDown() {
    for (std::set<MulticastGroupObject *>::iterator it =
         MulticastHandler::GetInstance()->GetMulticastObjList().begin();
         it != MulticastHandler::GetInstance()->GetMulticastObjList().end(); it++) {
        //Delete the label
        (*it)->SetSourceMPLSLabel(0);
        //Empty the tunnel OLIST
        (*it)->FlushAllFabricOlist();
        //Update comp NH
        MulticastHandler::GetInstance()->TriggerCompositeNHChange(*it);
    }
}

/*
 * handle mpls label changes/deletion/addition of new label
 */
void MulticastGroupObject::SetSourceMPLSLabel(uint32_t label) {
    MCTRACE(Log, "current src label ", vrf_name_,
            grp_address_.to_string(), src_mpls_label_);
    if (label == src_mpls_label_) {
        return;
    }

    //Add new label
    if ((label != 0) && (label != src_mpls_label_)) {
        MCTRACE(Log, "new src label ", vrf_name_,
                grp_address_.to_string(), label);
        MplsLabel::CreateMcastLabelReq(GetVrfName(), GetGroupAddress(),
                                       GetSourceAddress(), label);
    }
    //Delete old_label
    MplsLabel::DeleteMcastLabelReq(GetVrfName(), GetGroupAddress(),
                                   GetSourceAddress(), src_mpls_label_);
    src_mpls_label_ = label;
}

/*
 * Shutdown for clearing all stuff related to multicast
 */
void MulticastHandler::Shutdown() {
    //Delete all route mpls and trigger cnh change
    for (std::set<MulticastGroupObject *>::iterator it =
         MulticastHandler::GetInstance()->GetMulticastObjList().begin();
         it != MulticastHandler::GetInstance()->GetMulticastObjList().end(); it++) {
        //Empty the tunnel OLIST
        (*it)->FlushAllFabricOlist();
        //Update comp NH
        MulticastHandler::GetInstance()->TriggerCompositeNHChange(*it);
        //Delete the label and route
        MulticastHandler::GetInstance()->DeleteRouteandMPLS(*it);
        if (!IS_BCAST_MCAST((*it)->GetGroupAddress())) {
            MulticastHandler::GetInstance()->DeleteSubnetRoute(
                                                   (*it)->GetVrfName(),
                                                   (*it)->GetGroupAddress());
        }
        //Delete the multicast object
        delete (*it);
    }
    //Release the multicasthandler singleton object
    delete (MulticastHandler::GetInstance()->obj_);
}

ext: cc

max_stars_repo_name: sysbot/contrail-controller

max_issues_repo_name: sysbot/contrail-controller

max_forks_repo_name: sysbot/contrail-controller


text:
#define BOOST_TEST_MODULE MyTest
#include <boost/test/included/unit_test.hpp> //single-header
#include "MathLibrary.h" // project being tested

BOOST_AUTO_TEST_CASE(my_boost_test)
{
    double a = 7.4;
    int b = 99;
    BOOST_CHECK(a + b == MathLibrary::Arithmetic::Add(a,b));
}


text:
/*******************************************************************************
*
*  Filename    : KeysErrPlot.cc
*  Description : Keys PDF constructed from different nuisance parameters
*  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
*
*******************************************************************************/
#include "TstarAnalysis/Common/interface/PlotStyle.hpp"
#include "TstarAnalysis/CompareDataMC/interface/SampleErrHistMgr.hpp"
#include "TstarAnalysis/MassRecoCompare/interface/Common.hpp"
#include "TstarAnalysis/MassRecoCompare/interface/KeysErrMgr.hpp"

#include "ManagerUtils/PlotUtils/interface/Common.hpp"
#include "ManagerUtils/PlotUtils/interface/RooFitUtils.hpp"

#include <boost/format.hpp>
#include <iostream>

using namespace std;

void
MakeKeysErrPlots( KeysErrMgr* mgr )
{

  for( const auto histerr : histerrlist ){
    const string& tag = histerr.tag;

    TCanvas* c   = mgr::NewCanvas();
    TPad* toppad = mgr::NewTopPad();
    TPad* botpad = mgr::NewBottomPad();

    RooPlot* frame    = KeysErrMgr::x().frame();
    const double xmin = KeysErrMgr::x().getMin();
    const double xmax = KeysErrMgr::x().getMax();

    RooAbsPdf* central = mgr->Pdf( "" );
    RooAbsPdf* uppdf   = mgr->Pdf( tag+"Up" );
    RooAbsPdf* downpdf = mgr->Pdf( tag+"Down" );

    /*******************************************************************************
    *   Drawing top pad
    *******************************************************************************/
    toppad->Draw();
    toppad->cd();

    const double centralnorm = mgr->DataSet( "" )->sumEntries();
    const double upnorm      = ( tag == "pdf" || tag == "scale" ) ? centralnorm : mgr->DataSet( tag+"Up" )->sumEntries();
    const double downnorm    = ( tag == "pdf" || tag == "scale" ) ? centralnorm : mgr->DataSet( tag+"Down" )->sumEntries();

    TGraph* cenplot  = mgr::PlotOn( frame, central, RooFit::Normalization( centralnorm, RooAbsReal::NumEvent ) );
    TGraph* upplot   = mgr::PlotOn( frame, uppdf,   RooFit::Normalization( upnorm, RooAbsReal::NumEvent ) );
    TGraph* downplot = mgr::PlotOn( frame, downpdf, RooFit::Normalization( downnorm, RooAbsReal::NumEvent ) );
    frame->Draw();

    const double pltmax = mgr::GetYmax( cenplot, upplot, downplot );

    c->cd();
    /*******************************************************************************
    *   Drawing bottom plot
    *******************************************************************************/
    botpad->Draw();
    botpad->cd();
    TGraph* uprel   = mgr::DividedGraphSimple( upplot, cenplot );
    TGraph* downrel = mgr::DividedGraphSimple( downplot, cenplot );

    uprel->Draw( "AL" );
    downrel->Draw( "L" );

    TLine cenline( xmin, 1, xmax, 1  );
    TLine upline( xmin, 2, xmax, 2 );
    TLine downline( xmin, 0, xmax, 0 );

    cenline.Draw();
    upline.Draw();
    downline.Draw();

    c->cd();

    /*******************************************************************************
    *   Plot objects styling
    *******************************************************************************/
    mgr::SetTopPlotAxis( frame );
    frame->GetYaxis()->SetTitle( "Arbitrary unit" );
    frame->SetTitle( "" );
    frame->SetMinimum( 0.3 );
    frame->SetMaximum( pltmax * 1.8 );

    cenplot->SetLineColor( kBlack );
    upplot->SetLineColor( KBLUE );
    downplot->SetLineColor( KRED );
    uprel->SetLineColor( KBLUE );
    downrel->SetLineColor( KRED );

    mgr::SetBottomPlotAxis( uprel );
    uprel->GetXaxis()->SetTitle( frame->GetXaxis()->GetTitle() );
    uprel->GetXaxis()->SetRangeUser( xmin, xmax );
    uprel->GetYaxis()->SetTitle( "#frac{up,down}{central}" );
    uprel->SetMaximum( 2.2 );
    uprel->SetMinimum( -0.2 );

    upline.SetLineStyle( 3 );
    upline.SetLineColor( kBlack );
    downline.SetLineStyle( 3 );
    downline.SetLineColor( kBlack );

    /*******************************************************************************
    *   Drawing additional objects
    *******************************************************************************/
    mgr::DrawCMSLabel( SIMULATION );

    TLegend* legend    = mgr::NewLegend( 0.5, 0.75 );
    const string label = ( histerr.label != "" ) ? histerr.label : "1#sigma";
    legend->AddEntry( cenplot,  "Central Value",                                   "l" );
    legend->AddEntry( upplot,   ( histerr.rootname + "(+" + label + ")" ).c_str(), "l" );
    legend->AddEntry( downplot, ( histerr.rootname + "(-" + label + ")" ).c_str(), "l" );
    legend->Draw();

    mgr::LatexMgr latex;
    latex.SetOrigin( PLOT_X_TEXT_MAX, 0.75-TEXT_MARGIN, TOP_RIGHT )
    .WriteLine( mgr->LatexName() )
    .WriteLine( boost::str( boost::format( "M_{t*}=%dGeV/c^{2}" )%reconamer.GetInput<int>( "mass" ) ) );

    mgr::SaveToPDF( c, reconamer.PlotFileName( "errcomp", tag ) );

    delete uprel;
    delete downrel;
    delete toppad;
    delete botpad;
    delete frame;
    delete c;
  }
}

max_stars_repo_name: NTUHEP-Tstar/TstarAnalysis

max_issues_repo_name: NTUHEP-Tstar/TstarAnalysis

max_forks_repo_name: NTUHEP-Tstar/TstarAnalysis


text:
/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Mat;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedInPlaceGenMatRowIndColIndGenDouble : public Expected {
    public:
      ExpectedInPlaceGenMatRowIndColIndGenDouble() {
        cout << "Compute ExpectedInPlaceGenMatRowIndColIndGenDouble(): " << endl;

          vector<vector<pair<string, void*>>> inputs = Input::getTestParameters({
            InputClass::GenMat,
            InputClass::RowInd,
            InputClass::ColInd,
            InputClass::GenDouble
          });

          for (vector<pair<string, void*>> input : inputs) {
            _fileSuffix = "";

            int n = 0;
            for (pair<string, void*> value : input) {
              switch (n) {
                case 0:
                  _fileSuffix += value.first;
                  _genMat = *static_cast<Mat<double>*>(value.second);
                  break;
                case 1:
                  _fileSuffix += "," + value.first;
                  _rowInd = *static_cast<int*>(value.second);
                  break;
                case 2:
                  _fileSuffix += "," + value.first;
                  _colInd = *static_cast<int*>(value.second);
                  break;
                case 3:
                  _fileSuffix += "," + value.first;
                  _genDouble = *static_cast<double*>(value.second);
                  break;
              }
              ++n;
            }

            cout << "Using input: " << _fileSuffix << endl;

            _copyOfGenMat = _genMat;
            _copyOfRowInd = _rowInd;
            _copyOfColInd = _colInd;
            _copyOfGenDouble = _genDouble;

            expectedMatAtEqual();

            _genMat = _copyOfGenMat;
            _rowInd = _copyOfRowInd;
            _colInd = _copyOfColInd;
            _genDouble = _copyOfGenDouble;
            expectedMatAtPlus();

            _genMat = _copyOfGenMat;
            _rowInd = _copyOfRowInd;
            _colInd = _copyOfColInd;
            _genDouble = _copyOfGenDouble;
            expectedMatAtMinus();

            _genMat = _copyOfGenMat;
            _rowInd = _copyOfRowInd;
            _colInd = _copyOfColInd;
            _genDouble = _copyOfGenDouble;
            expectedMatAtTimes();

            _genMat = _copyOfGenMat;
            _rowInd = _copyOfRowInd;
            _colInd = _copyOfColInd;
            _genDouble = _copyOfGenDouble;
            expectedMatAtDivide();
          }

          cout << "done." << endl;
        }

    protected:
      Mat<double> _genMat;
      Mat<double> _copyOfGenMat;

      int _rowInd;
      int _copyOfRowInd;

      int _colInd;
      int _copyOfColInd;

      double _genDouble;
      double _copyOfGenDouble;

      void expectedMatAtEqual() {
        if(!_genMat.in_range(_rowInd, _colInd)) {
          return;
        }

        _genMat.at(_rowInd, _colInd) = _genDouble;

        cout << "- Compute expectedMatAtEqual() ... ";
        save<double>("Mat.atEqual", _genMat);
        cout << "done." << endl;
      }

      void expectedMatAtPlus() {
        if(!_genMat.in_range(_rowInd, _colInd)) {
          return;
        }

        _genMat.at(_rowInd, _colInd) += _genDouble;

        cout << "- Compute expectedMatAtPlus() ... ";
        save<double>("Mat.atPlus", _genMat);
        cout << "done." << endl;
      }

      void expectedMatAtMinus() {
        if(!_genMat.in_range(_rowInd, _colInd)) {
          return;
        }

        _genMat.at(_rowInd, _colInd) -= _genDouble;

        cout << "- Compute expectedMatAtMinus() ... ";
        save<double>("Mat.atMinus", _genMat);
        cout << "done." << endl;
      }

      void expectedMatAtTimes() {
        if(!_genMat.in_range(_rowInd, _colInd)) {
          return;
        }

        _genMat.at(_rowInd, _colInd) *= _genDouble;

        cout << "- Compute expectedMatAtTimes() ... ";
        save<double>("Mat.atTimes", _genMat);
        cout << "done." << endl;
      }

      void expectedMatAtDivide() {
        if(!_genMat.in_range(_rowInd, _colInd)) {
          return;
        }

        _genMat.at(_rowInd, _colInd) /= _genDouble;

        cout << "- Compute expectedMatAtDivide() ... ";
        save<double>("Mat.atDivide", _genMat);
        cout << "done." << endl;
      }
  };
}

max_stars_repo_name: SebastianNiemann/ArmadilloJava

max_issues_repo_name: sebiniemann/ArmadilloJava

max_forks_repo_name: sebiniemann/ArmadilloJava


text:
/*
 * Author: David Soto
 * Year:   2016
 */

// this node uses a stereo camera rig to generate a point cloud of the environment.
// It uses the exFAST detector to generate a sparse point cloud.
// See this for more info: http://www.ra.cs.uni-tuebingen.de/software/sparsestereo/welcome_e.html

#include <navigator_vision_lib/cv_tools.hpp>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/thread.hpp>
#include <boost/bind.hpp>
#include <boost/filesystem.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <vector>
#include <iostream>
#include <iomanip>
#include <exception>
#include <sparsestereo/exception.h>
#include <sparsestereo/extendedfast.h>
#include <sparsestereo/stereorectification.h>
#include <sparsestereo/sparsestereo-inl.h>
#include <sparsestereo/census-inl.h>
#include <sparsestereo/imageconversion.h>
#include <sparsestereo/censuswindow.h>
#include <ros/ros.h>
#include <ros/package.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <image_geometry/pinhole_camera_model.h>
#include <tf/transform_listener.h>
#include <std_srvs/SetBool.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>

void left_image_callback(
    const sensor_msgs::ImageConstPtr &image_msg_ptr,
    const sensor_msgs::CameraInfoConstPtr &info_msg_ptr);
void right_image_callback(
    const sensor_msgs::ImageConstPtr &image_msg_ptr,
    const sensor_msgs::CameraInfoConstPtr &info_msg_ptr);
void cloud_callback (const sensor_msgs::PointCloud2ConstPtr& cloud_msg);
bool detection_activation_switch(
    std_srvs::SetBool::Request &req,
    std_srvs::SetBool::Response &resp);

using namespace std;
using namespace cv;
using namespace sparsestereo;
using namespace boost;
using namespace boost::posix_time;
using ros::param::param;

struct XYZRGB{

  float x, y, z;
  uint8_t r, g, b;

  bool operator==(const XYZRGB &other) const{
    if(this->x == other.x && this->y == other.y && this->z == other.z){
      return true;
    }
    else return false;
  }

  bool operator<(const XYZRGB &other) const{
    if(this->x > other.x) return false;
    else if(this->y > other.y) return false;
    else if(this->z >= other.z) return false;
    else return true;
  }
};

// Image storage
sensor_msgs::ImageConstPtr left_most_recent,  right_most_recent;
sensor_msgs::CameraInfoConstPtr left_most_recent_info, right_most_recent_info;

// Stereo subscribing
image_transport::CameraSubscriber left_image_sub, right_image_sub;
image_geometry::PinholeCameraModel left_cam_model, right_cam_model;

// Velodyne subscribing
ros::Subscriber pcd_sub;

// Camera projection matrices
Matx34d left_P, right_P;

// TF
// tf::TransformListener tf_listener;
string tf_frame_stereo_l {"/stereo_left_cam"};
string tf_frame_stereo_r {"/stereo_right_cam"};
string tf_frame_velodyne {"/velodyne"};
tf::StampedTransform stereoL_tf_velodyne;

// Point clouds
vector<XYZRGB> stereo_point_cloud;
// sensor_msgs::PointCloud2 pt_cloud_msg;
std_msgs::Header header;
int pt_cloud_seq {0};

// Point cloud publisher
ros::Publisher pcl_pub;

// Thread safety
boost::mutex left_mtx, right_mtx;

// Control
bool active = false;
ros::ServiceServer detection_switch;

int main(int argc, char** argv) {
  namespace fs = boost::filesystem;

  try {

    stringstream log_msg;
    log_msg << "\nInitializing Sparse Stereo Point Cloud Driver:\n";
    int tab_sz = 4;

    // globals
    ros::init(argc, argv, "stereo_point_cloud_driver");
    ros::NodeHandle nh;
    image_transport::ImageTransport img_trans {nh};
    image_geometry::PinholeCameraModel left_cam_model, right_cam_model;
    cv_bridge::CvImagePtr input_bridge;
    string activation_srv_name {"stereo/activation_srv"};

    // Default parameters
    string img_topic_left_default = "/stereo/left/image_raw/";
    string img_topic_right_default = "/stereo/right/image_raw/";
    string activation_default = "/stereo/activation_switch";
    string dbg_topic_default = "/stereo/dbg_imgs";

    // Advertise activation switch
    detection_switch = nh.advertiseService(
      activation_srv_name, detection_activation_switch);

    // Advertise point cloud topic
    pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("stereo_front_point_cloud", 1);

    // Subscribe to Cameras (image + camera_info)
    cout << "subscribing to cameras" << endl;
    string left = param<string>("/stereo/input_left", img_topic_left_default);
    string right = param<string>("/stereo/input_right", img_topic_right_default);
    cout << "Got topic name params!" << endl;
    left_image_sub = img_trans.subscribeCamera(left, 10, left_image_callback);
    right_image_sub = img_trans.subscribeCamera(right, 10, right_image_callback);
    log_msg << setw(1 * tab_sz) << "" << "Camera Subscriptions:\x1b[37m\n"
      << setw(2 * tab_sz) << "" << "left  = " << left << endl
      << setw(2 * tab_sz) << "" << "right = " << right << "\x1b[0m\n";
    cout << left << ' ' << right << endl;
    ROS_INFO(log_msg.str().c_str());

    // Subscribe to Velodyne scan
    pcd_sub = nh.subscribe ("/velodyne_points", 1, cloud_callback);

    // Wait for transform between velodyne and stereo frames
    tf::TransformListener tf_listener;
    ROS_INFO("Waiting 10 seconds for tf between velodyne and stereo frames to become available...");
    // try{
    //   ros::Time now = ros::Time(0);
    //   if (tf_listener.waitForTransform(tf_frame_velodyne, tf_frame_stereo_l,
    //                  now, ros::Duration(10.0))){
    //     cout << "TF is available." << endl;
    //   }
    //   else cout << "Timed out waiting for TF." << endl;

    //   tf_listener.lookupTransform(tf_frame_velodyne, tf_frame_stereo_l, now, stereoL_tf_velodyne);
    //   // cout << "TF:" << stereoL_tf_velodyne << endl;
    // }
    // catch (const std::exception& e) {
    //   cerr << "Fatal exception while acquiring TF's: " << e.what();
    //   return -1;
    // }

    // Size adjustment ROI
    /*
      exFAST is currently heavily optimized for VGA size images, extracting a
      properly sized region of interest is much more efficient than resizing the image
    */

    // Stereo matching parameters
    double uniqueness = 0.8;
    int maxDisp = 70;
    int leftRightStep = 2;

    // Feature detection parameters
    double adaptivity = 1.0;
    int minThreshold = 10;

    // Load rectification data
    fs::path calibration_file_path {ros::package::getPath("navigator_perception") + "/calibration.xml"};
    cout << calibration_file_path << endl;
    if ( !boost::filesystem::exists( calibration_file_path ) ) // Check file exists
    {
      cout << "Can't find calibration file!" << std::endl;
      throw "Can't find calibration file!";
    }
    StereoRectification* rectification = NULL;
    rectification = new StereoRectification(CalibrationResult(calibration_file_path.c_str()));

    // The stereo matcher. SSE Optimized implementation is only available for a 5x5 window
    SparseStereo<CensusWindow<5>, short> stereo(maxDisp, 1, uniqueness, rectification,
                                                false, false, leftRightStep);

    // Feature detectors for left and right image
    FeatureDetector* leftFeatureDetector = new ExtendedFAST(true, minThreshold, adaptivity, true, 10);
    FeatureDetector* rightFeatureDetector = new ExtendedFAST(false, minThreshold, adaptivity, true, 10);

    // Sparse Correspondences
    vector<SparseMatch> correspondences;

    // Main stereo processing loop
    ros::Rate loop_rate(10);  // process images 10 times per second
    cout << "\x1b[1;31mMain Loop!\x1b[0m" << endl;
    while (ros::ok()) {
      cout << "clock: " << microsec_clock::local_time() << endl;
      cout << " The left Camera Subscriber is connected to " << left_image_sub.getNumPublishers() << " publishers." << endl;
      cout << " The right Camera Subscriber is connected to " << right_image_sub.getNumPublishers() << " publishers." << endl;
      cout << "active=" << (active? "true" : "false") << endl;
      cout  << "left_most_recent=" << left_most_recent << " right_most_recent=" << right_most_recent << endl;
      // Idle if active is not set or valid image pointer pairs have not been retrieved
      if (!active || (left_most_recent == nullptr) || (right_most_recent == nullptr)){
        ros::spinOnce();  // still handle ROS callbacks if skipping an iteration
        loop_rate.sleep();
        cout << "Skipping this stereo pair for processing." << endl;
        continue;
      }
      try {
        // Containers
        Mat raw_left, raw_right, draw_left, draw_right;
        Mat_<unsigned char> leftImg, rightImg;

        // Thread Safety
        cout << "Getting frame data" << endl;
        left_mtx.lock();
        right_mtx.lock();
        {
          // Get Frame Message data
          namespace cvb = cv_bridge;
          cout << "left_most_recent: " << left_most_recent << endl;
          cout << "right_most_recent: " << right_most_recent << endl;
          input_bridge = cvb::toCvCopy(left_most_recent, sensor_msgs::image_encodings::BGR8);
          raw_left = input_bridge->image;
          draw_left = raw_left.clone();
          // imshow("raw_left", raw_left); waitKey(0);
          cout << "raw_left_size:"  << raw_left.size() << endl;
          input_bridge = cvb::toCvCopy(right_most_recent, sensor_msgs::image_encodings::BGR8);
          raw_right = input_bridge->image;
          draw_right = raw_right.clone();
        }
        cout << "Got frame data" << endl;
        left_mtx.unlock();
        right_mtx.unlock();

        // Check images for data and adjust size
        if(raw_left.data == NULL || raw_right.data == NULL)
          throw sparsestereo::Exception("Unable to open input images!");
        cvtColor(raw_left, leftImg, CV_BGR2GRAY);
        cvtColor(raw_right, rightImg, CV_BGR2GRAY);

        // Enforce approximate image synchronization
        double left_stamp = left_most_recent_info->header.stamp.toSec();
        double right_stamp = right_most_recent_info->header.stamp.toSec();
        double sync_error = fabs(left_stamp - right_stamp);
        stringstream sync_msg;
        sync_msg << "Left and right images were not sufficiently synchronized"
                 << "\nsync error: " << sync_error << "s";
        if (sync_error > 0.3) {
          ROS_WARN(sync_msg.str().c_str());
          cout << "skipping" << endl;
          continue;
        }

        // Objects for storing final and intermediate results
        int rows = leftImg.rows;
        int cols = leftImg.cols;
        Mat_<char> charLeft(rows, cols);
        Mat_<char> charRight(rows, cols);
        Mat_<unsigned int> censusLeft(rows, cols);
        Mat_<unsigned int> censusRight(rows, cols);
        vector<KeyPoint> keypointsLeft, keypointsRight;

        ptime lastTime = microsec_clock::local_time(); // Time algorithm start time

        // Calculate census transforms for both images and detet features
        // for both images in parallel
        #pragma omp parallel sections default(shared) num_threads(2)
        {
          #pragma omp section
          {
            ImageConversion::unsignedToSigned(leftImg, &charLeft);
            Census::transform5x5(charLeft, &censusLeft);
            keypointsLeft.clear();
            leftFeatureDetector->detect(leftImg, keypointsLeft);
          }
          #pragma omp section
          {
            ImageConversion::unsignedToSigned(rightImg, &charRight);
            Census::transform5x5(charRight, &censusRight);
            keypointsRight.clear();
            rightFeatureDetector->detect(rightImg, keypointsRight);
          }
        }

        // Stereo matching and reconstruction
        Point2f pt_L;
        Point2f pt_R;
        Matx31d pt_L_hom;
        Matx31d pt_R_hom;
        Mat X, X_hom;
        Vec3b color;

        // Calculate correspondences in Blue channel
        correspondences.clear();
        stereo.match(censusLeft, censusRight, keypointsLeft, keypointsRight, &correspondences);

        for(int i=0; i<(int)correspondences.size(); i++) {

          // Visualize feature points
          Scalar rand_color = Scalar(rand() % 256, rand() % 256, rand() % 256);
          rectangle(raw_left, pt_L - Point2f(2,2), pt_L + Point2f(2, 2), rand_color, CV_FILLED);
          rectangle(raw_right, pt_R - Point2f(2,2), pt_R + Point2f(2, 2), rand_color, CV_FILLED);

          // Triangulate matches to reconstruct 3D point
          pt_L = correspondences[i].imgLeft->pt;
          pt_R = correspondences[i].imgRight->pt;
          pt_L_hom = Matx31d(pt_L.x, pt_L.y, 1);
          pt_R_hom = Matx31d(pt_R.x, pt_R.y, 1);
          X_hom = nav::triangulate_Linear_LS(Mat(left_P), Mat(right_P), Mat(pt_L_hom), Mat(pt_R_hom));
          X_hom = X_hom / X_hom.at<double>(3, 0);

          // Get Color and fill point_cloud element precursor
          XYZRGB point;
          point.x = X_hom.at<double>(0, 0);
          point.y = X_hom.at<double>(1, 0);
          point.z = X_hom.at<double>(2, 0);
          Vec3b pix_color = raw_left.at<Vec3b>(pt_L);
          cout << i << "ptL: " << pt_L << " ptR: " << pt_R << " Color: " << pix_color
            << " " << X_hom.t() << endl;
          point.b = pix_color[0];
          point.g = pix_color[1];
          point.r = pix_color[2];
          stereo_point_cloud.push_back(point);
        }

        // Display image
        Mat screen = Mat::zeros(Size(leftImg.cols*2, leftImg.rows), CV_8UC3);
        raw_left.copyTo(screen(Rect(Point(0,0), leftImg.size())));
        raw_right.copyTo(screen(Rect(Point(leftImg.cols,0), leftImg.size())));
        namedWindow("Stereo");
        imshow("Stereo", screen);
        waitKey(0);

        // Print statistics
        time_duration elapsed = (microsec_clock::local_time() - lastTime);
        cout << "Time for stereo matching: " << elapsed.total_microseconds()/1.0e6 << "s" << endl
          << "Features detected in left image: " << keypointsLeft.size() << endl
          << "Features detected in right image: " << keypointsRight.size() << endl
          << "Percentage of matched features: " << (100.0 * correspondences.size() / keypointsLeft.size()) << "%" << endl;

        // Populate point cloud message header
        sensor_msgs::PointCloud2 pt_cloud_msg;
        header.seq = pt_cloud_seq++;
        ros::Time frame_time_l = left_most_recent_info->header.stamp;
        ros::Time frame_time_r = right_most_recent_info->header.stamp;
        ros::Duration offset((frame_time_r - frame_time_l).toSec() / 2);
        header.stamp = left_most_recent_info->header.stamp + offset;
        header.frame_id = tf_frame_stereo_l;
        pt_cloud_msg.header = header;
        cout << "pcd header: " <<pt_cloud_msg.header << endl;

        // Specify point cloud msg meta-data
        pt_cloud_msg.height = 1;
        pt_cloud_msg.width = stereo_point_cloud.size();
        pt_cloud_msg.is_bigendian = false;
        pt_cloud_msg.is_dense = false;

        // Fill point cloud msg
        sensor_msgs::PointCloud2Modifier pcd_modifier(pt_cloud_msg);
        pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
        sensor_msgs::PointCloud2Iterator<float> iter_x(pt_cloud_msg, "x");
        sensor_msgs::PointCloud2Iterator<float> iter_y(pt_cloud_msg, "y");
        sensor_msgs::PointCloud2Iterator<float> iter_z(pt_cloud_msg, "z");
        sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(pt_cloud_msg, "b");
        sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(pt_cloud_msg, "g");
        sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(pt_cloud_msg, "r");

        cout << "filling pcd2 msg " << endl;
        for(size_t i = 0; i < stereo_point_cloud.size(); ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_b, ++iter_g, ++iter_r)
        {
          *iter_x = stereo_point_cloud[i].x;
          *iter_y = stereo_point_cloud[i].y;
          *iter_z = stereo_point_cloud[i].z;
          *iter_b = stereo_point_cloud[i].b;
          *iter_g = stereo_point_cloud[i].g;
          *iter_r = stereo_point_cloud[i].r;
          cout << i << "XYZBGR: " << *iter_x << " " << *iter_y << " " << *iter_z << " " << int(*iter_b) << " " << int(*iter_g) << " " <<
           int(*iter_r) << endl;
         }

        // Publish point cloud
        pcl_pub.publish(pt_cloud_msg);

        // Empty point cloud container for upcoming iteration
        cout << "PCD size: " << pt_cloud_msg.height * pt_cloud_msg.row_step
          << ", " << stereo_point_cloud.size() << endl;
        stereo_point_cloud.clear();

        // Handle ROS callbacks
        ros::spinOnce();
      }
      catch (const std::exception& e) {
        cerr << "Fatal exception during main loop: " << e.what();
        return -1;
      }
      loop_rate.sleep();
    }  // end main while loop
    return 0;
  } // end large try block
  catch (const std::exception& e) {
    cerr << "Fatal exception during initialization: " << e.what();
    return -1;
  }
}

bool detection_activation_switch(
    std_srvs::SetBool::Request &req,
    std_srvs::SetBool::Response &resp) {
  stringstream ros_log;
  ros_log << "\x1b[1;31mSetting sereo point cloud generation to: \x1b[1;37m"
          << (req.data ? "on" : "off") << "\x1b[0m";
  ROS_INFO(ros_log.str().c_str());
  active = req.data;
  resp.success = true;
  return true;
}

void left_image_callback(
    const sensor_msgs::ImageConstPtr &image_msg_ptr,
    const sensor_msgs::CameraInfoConstPtr &info_msg_ptr) {

  // Get ptrs to most recent frame and info (thread safe)
  left_mtx.lock();
  cout << "\x1b[1;31mleft callback!\x1b[0m" << endl;
  left_most_recent = image_msg_ptr;
  left_most_recent_info = info_msg_ptr;
  left_mtx.unlock();

  // Initialize camera parameters
  static bool first_call = true;
  if(first_call){
    left_cam_model.fromCameraInfo(left_most_recent_info);
    left_P = left_cam_model.fullProjectionMatrix();
    tf_frame_stereo_l = left_cam_model.tfFrame();
    first_call = false;
  }
}

void right_image_callback(
    const sensor_msgs::ImageConstPtr &image_msg_ptr,
    const sensor_msgs::CameraInfoConstPtr &info_msg_ptr) {

  // Get ptrs to most recent frame and info (thread safe)
  right_mtx.lock();
  cout << "\x1b[1;31mright callback!\x1b[0m" << endl;
  right_most_recent = image_msg_ptr;
  right_most_recent_info = info_msg_ptr;
  right_mtx.unlock();

  // Initialize camera parameters
  static bool first_call = true;
  if(first_call){
    right_cam_model.fromCameraInfo(right_most_recent_info);
    right_P = right_cam_model.fullProjectionMatrix();
    right_P(0, 3) = -140;
    cout << "right projection_matrix: " << right_P << endl;
    tf_frame_stereo_r = right_cam_model.tfFrame();
    first_call = false;
  }
}

void cloud_callback (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
     ROS_INFO("velodyne_points callback");
     sensor_msgs::PointCloud2 colored_velodyne_pcd;

}

max_stars_repo_name: jaxnb/NaviGator

max_issues_repo_name: jaxnb/NaviGator

max_forks_repo_name: ericgorday/NaviGator