<commit_before>/*
 * mpu6050_node.cpp
 * Copyright (c) 2017, Robot Mitya.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * 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.
 *     * Neither the name of the Robot Mitya 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 OWNER 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.
 *
 *  Created on: Dec 3, 2017
 *      Author: Dmitry Dzakhov
 */

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include "consts.h"

//#define MPU6050_INCLUDE_DMP_MOTIONAPPS20

#include "MPU6050_6Axis_MotionApps20.h"

class Mpu6050Node
{
public:
  Mpu6050Node();

  void prepareImuMessage(const sensor_msgs::ImuPtr& msg);
  void publishImuMessage(const sensor_msgs::ImuPtr& msg);

  void setup();
  void loop(const sensor_msgs::ImuPtr& msg);
private:
  MPU6050 *mpu_;

  int update_rate_;
  int i2cAddress_;

  // Topic RM_IMU_TOPIC_NAME ('imu') publisher:
  ros::Publisher imuPublisher_;

  // MPU control/status variants
  bool dmpReady_;  // set true if DMP initialization was successful
  uint8_t mpuIntStatus_;   // holds actual interrupt status byte from MPU
  uint8_t devStatus_;      // return status after each device operation (0 = success, !0 = error)
  uint16_t packetSize_;    // expected DMP packet size (default is 42 bytes)
  uint16_t fifoCount_;     // count of all bytes currently in FIFO
  uint8_t fifoBuffer_[64]; // FIFO storage buffer

  // orientation/motion variants
  Quaternion q_;           // [w, x, y, z]         quaternion container
  VectorInt16 aa_;         // [x, y, z]            accelerator sensor measurements
  VectorInt16 aaReal_;     // [x, y, z]            gravity-free accelerator sensor measurements
  VectorInt16 aaWorld_;    // [x, y, z]            world-frame accelerator sensor measurements
  VectorFloat gravity_;    // [x, y, z]            gravity vector
  float ypr_[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
  float last_ypr_[3];      // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
  ros::Publisher imu_pub_;
  boost::array<double, 9> linear_acceleration_cov_;
  boost::array<double, 9> angular_velocity_cov_;
  boost::array<double, 9> orientation_cov_;
  boost::array<double, 9> unk_orientation_cov_;
  boost::array<double, 9> magnetic_cov_;
  ros::Time last_update_;

  void setupCovariance(boost::array<double, 9> &cov, double stdev);
};

Mpu6050Node::Mpu6050Node()
{
  dmpReady_ = false;

  ros::NodeHandle privateNodeHandle("~");
  int i2cAddress = 0x68;
  privateNodeHandle.param("i2c_address", i2cAddress_, 0x68);

  double linear_stdev, angular_stdev, orientation_stdev;
  privateNodeHandle.param("linear_acceleration_stdev", linear_stdev, 0.0003);
  privateNodeHandle.param("angular_velocity_stdev", angular_stdev, 0.02 * (M_PI / 180.0));
  privateNodeHandle.param("orientation_stdev", orientation_stdev, 1.0);
  privateNodeHandle.param("update_rate", update_rate_, 100);

  setupCovariance(linear_acceleration_cov_, linear_stdev);
  setupCovariance(angular_velocity_cov_, angular_stdev);
  setupCovariance(orientation_cov_, orientation_stdev);

  ros::NodeHandle nodeHandle(RM_NAMESPACE);
  imuPublisher_ = nodeHandle.advertise<sensor_msgs::Imu>(RM_IMU_TOPIC_NAME, 100);
}

void Mpu6050Node::setupCovariance(boost::array<double, 9> &cov, double stdev)
{
    std::fill(cov.begin(), cov.end(), 0.0);
    if (stdev == 0.0)
      cov[0] = -1.0;
    else
      cov[0 + 0] = cov[3 + 1] = cov[6 + 2] = std::pow(stdev, 2);
}

void Mpu6050Node::prepareImuMessage(const sensor_msgs::ImuPtr& msg)
{
  msg->header.frame_id = "imu";
  msg->orientation_covariance = orientation_cov_;
  msg->angular_velocity_covariance = angular_velocity_cov_;
  msg->linear_acceleration_covariance = linear_acceleration_cov_;
}

void Mpu6050Node::publishImuMessage(const sensor_msgs::ImuPtr& msg)
{
  imuPublisher_.publish(msg);
}

void Mpu6050Node::setup()
{
  mpu_ = new MPU6050(i2cAddress_);

  // initialize device
  ROS_INFO("Initializing I2C devices...\n");
  mpu_->initialize();

  // verify connection
  ROS_INFO("Testing device connections...\n");
  ROS_INFO(mpu_->testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n");

  // load and configure the DMP
  ROS_INFO("Initializing DMP...\n");
  devStatus_ = mpu_->dmpInitialize();

  // make sure it worked (returns 0 if so)
  if (devStatus_ == 0)
  {
    // turn on the DMP, now that it's ready
    ROS_INFO("Enabling DMP...\n");
    mpu_->setDMPEnabled(true);

    // enable Arduino interrupt detection
    //Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    //attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus_ = mpu_->getIntStatus();

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    ROS_INFO("DMP ready!\n");
    dmpReady_ = true;

    // get expected DMP packet size for later comparison
    packetSize_ = mpu_->dmpGetFIFOPacketSize();
  }
  else
  {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    ROS_ERROR_STREAM("DMP Initialization failed (code " << devStatus_ <<")\n");
  }
}

void Mpu6050Node::loop(const sensor_msgs::ImuPtr& msg)
{
  // if programming failed, don't try to do anything
  if (!dmpReady_) return;
  // get current FIFO count
  fifoCount_ = mpu_->getFIFOCount();

  if (fifoCount_ == 1024)
  {
    // reset so we can continue cleanly
    mpu_->resetFIFO();
    ROS_WARN("FIFO overflow!\n");

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
  }
  else if (fifoCount_ >= 42)
  {
    // read a packet from FIFO
    mpu_->getFIFOBytes(fifoBuffer_, packetSize_);

    ros::Time now = ros::Time::now();
    msg->header.stamp = now;
    ros::Duration dt_r = now - last_update_;
    last_update_ = now;

    // display quaternion values in easy matrix form: w x y z
    mpu_->dmpGetQuaternion(&q_, fifoBuffer_);
    msg->orientation.x = q_.x;
    msg->orientation.y = q_.y;
    msg->orientation.z = q_.z;
    msg->orientation.w = q_.w;

    mpu_->dmpGetGravity(&gravity_, &q_);
    mpu_->dmpGetYawPitchRoll(ypr_, &q_, &gravity_);
    float yaw_ang_vel = ypr_[0] - last_ypr_[0] / dt_r.toSec();
    float pitch_ang_vel = ypr_[1] - last_ypr_[1] / dt_r.toSec();
    float roll_ang_vel = ypr_[2] - last_ypr_[2] / dt_r.toSec();

    msg->angular_velocity.x = roll_ang_vel;
    msg->angular_velocity.y = pitch_ang_vel;
    msg->angular_velocity.z = yaw_ang_vel;

    // display initial world-frame acceleration, adjusted to remove gravity
    // and rotated based on known orientation from quaternion
    mpu_->dmpGetAccel(&aa_, fifoBuffer_);
    mpu_->dmpGetLinearAccelInWorld(&aaWorld_, &aaReal_, &q_);
    //printf("aworld %6d %6d %6d    ", aaWorld.x, aaWorld.y, aaWorld.z);
    msg->linear_acceleration.x = aaWorld_.x;
    msg->linear_acceleration.y = aaWorld_.y;
    msg->linear_acceleration.z = aaWorld_.z;
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, RM_MPU6050_NODE_NAME);

  Mpu6050Node mpu6050Node;

  sensor_msgs::ImuPtr imuPrt(new sensor_msgs::Imu());
  mpu6050Node.prepareImuMessage(imuPrt);

  mpu6050Node.setup();

  ros::Rate rate(100);  // Hz
  while(ros::ok())
  {
    mpu6050Node.loop(imuPrt);
    mpu6050Node.publishImuMessage(imuPrt);

    ros::spinOnce();
    rate.sleep();
  }
  return 0;
}

/*
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>
#include "consts.h"

class Mpu6050Node
{
public:
  Mpu6050Node();
  void fillImuMessage(const sensor_msgs::ImuPtr& msg);
  void publishImuMessage(const sensor_msgs::ImuPtr& msg);
private:
  int i2cAddress_;
  int fileDescriptor_;
  float readWord2c(int addr);

  // Topic RM_IMU_TOPIC_NAME ('imu') publisher:
  ros::Publisher imuPublisher_;
};

const int PWR_MGMT_1 = 0x6B;

Mpu6050Node::Mpu6050Node()
{
  ros::NodeHandle privateNodeHandle("~");
  privateNodeHandle.param("i2c_address", i2cAddress_, 0x68);

  // Connect to device.
  fileDescriptor_ = wiringPiI2CSetup(i2cAddress_);
  if (fileDescriptor_ == -1)
  {
    ROS_ERROR("No i2c device found?");
    return;
  }
  // Device starts in sleep mode so wake it up.
  wiringPiI2CWriteReg16(fileDescriptor_, PWR_MGMT_1, 0);

  ros::NodeHandle nodeHandle(RM_NAMESPACE);
  imuPublisher_ = nodeHandle.advertise<sensor_msgs::Imu>(RM_IMU_TOPIC_NAME, 100);
}

float Mpu6050Node::readWord2c(int addr)
{
  int high = wiringPiI2CReadReg8(fileDescriptor_, addr);
  int low = wiringPiI2CReadReg8(fileDescriptor_, addr + 1);
  int val = (high << 8) + low;
  return float((val >= 0x8000) ? -((65535 - val) + 1) : val);
}

void Mpu6050Node::fillImuMessage(const sensor_msgs::ImuPtr& msg)
{
  msg->header.stamp = ros::Time::now();
  msg->header.frame_id = '0';  // no frame

  // Read gyroscope values.
  // At default sensitivity of 250deg/s we need to scale by 131.
  msg->angular_velocity.x = readWord2c(0x43) / 131;
  msg->angular_velocity.y = readWord2c(0x45) / 131;
  msg->angular_velocity.z = readWord2c(0x47) / 131;

  // Read accelerometer values.
  // At default sensitivity of 2g we need to scale by 16384.
  // Note: at "level" x = y = 0 but z = 1 (i.e. gravity)
  // But! Imu message documentations say acceleration should be in m/2 so need to *9.807
  const float la_rescale = 16384.0 / 9.807;
  msg->linear_acceleration.x = readWord2c(0x3b) / la_rescale;
  msg->linear_acceleration.y = readWord2c(0x3d) / la_rescale;
  msg->linear_acceleration.z = readWord2c(0x3f) / la_rescale;
}

void Mpu6050Node::publishImuMessage(const sensor_msgs::ImuPtr& msg)
{
  imuPublisher_.publish(msg);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, RM_MPU6050_NODE_NAME);

  Mpu6050Node mpu6050Node;
  sensor_msgs::ImuPtr imuPrt(new sensor_msgs::Imu());

  ros::Rate rate(10);  // hz
  while(ros::ok())
  {
    mpu6050Node.fillImuMessage(imuPrt);
    mpu6050Node.publishImuMessage(imuPrt);

    ros::spinOnce();
    rate.sleep();
  }
  return 0;
}
*/
<commit_msg>#12 Changes in mpu6050_node<commit_after>/*
 * mpu6050_node.cpp
 * Copyright (c) 2017, Robot Mitya.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * 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.
 *     * Neither the name of the Robot Mitya 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 OWNER 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.
 *
 *  Created on: Dec 3, 2017
 *      Author: Dmitry Dzakhov
 */

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include "consts.h"

//#define MPU6050_INCLUDE_DMP_MOTIONAPPS20

#include "MPU6050_6Axis_MotionApps20.h"

class Mpu6050Node
{
public:
  Mpu6050Node();

  void prepareImuMessage(const sensor_msgs::ImuPtr& msg);
  void publishImuMessage(const sensor_msgs::ImuPtr& msg);

  void setup();
  void loop(const sensor_msgs::ImuPtr& msg);
private:
  MPU6050 *mpu_;

  int update_rate_;
  int i2cAddress_;

  // Topic RM_IMU_TOPIC_NAME ('imu') publisher:
  ros::Publisher imuPublisher_;

  // MPU control/status variants
  bool dmpReady_;  // set true if DMP initialization was successful
  uint8_t mpuIntStatus_;   // holds actual interrupt status byte from MPU
  uint8_t devStatus_;      // return status after each device operation (0 = success, !0 = error)
  uint16_t packetSize_;    // expected DMP packet size (default is 42 bytes)
  uint16_t fifoCount_;     // count of all bytes currently in FIFO
  uint8_t fifoBuffer_[64]; // FIFO storage buffer

  // orientation/motion variants
  Quaternion q_;           // [w, x, y, z]         quaternion container
  VectorInt16 aa_;         // [x, y, z]            accelerator sensor measurements
  VectorInt16 aaReal_;     // [x, y, z]            gravity-free accelerator sensor measurements
  VectorInt16 aaWorld_;    // [x, y, z]            world-frame accelerator sensor measurements
  VectorFloat gravity_;    // [x, y, z]            gravity vector
  float ypr_[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
  float last_ypr_[3];      // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
  ros::Publisher imu_pub_;
  boost::array<double, 9> linear_acceleration_cov_;
  boost::array<double, 9> angular_velocity_cov_;
  boost::array<double, 9> orientation_cov_;
  boost::array<double, 9> unk_orientation_cov_;
  boost::array<double, 9> magnetic_cov_;
  ros::Time last_update_;

  void setupCovariance(boost::array<double, 9> &cov, double stdev);
};

Mpu6050Node::Mpu6050Node()
{
  dmpReady_ = false;

  ros::NodeHandle privateNodeHandle("~");
  int i2cAddress = 0x68;
  privateNodeHandle.param("i2c_address", i2cAddress_, 0x68);

  double linear_stdev, angular_stdev, orientation_stdev;
  privateNodeHandle.param("linear_acceleration_stdev", linear_stdev, 0.0003);
  privateNodeHandle.param("angular_velocity_stdev", angular_stdev, 0.02 * (M_PI / 180.0));
  privateNodeHandle.param("orientation_stdev", orientation_stdev, 1.0);
  privateNodeHandle.param("update_rate", update_rate_, 100);

  setupCovariance(linear_acceleration_cov_, linear_stdev);
  setupCovariance(angular_velocity_cov_, angular_stdev);
  setupCovariance(orientation_cov_, orientation_stdev);

  ros::NodeHandle nodeHandle(RM_NAMESPACE);
  imuPublisher_ = nodeHandle.advertise<sensor_msgs::Imu>(RM_IMU_TOPIC_NAME, 100);
}

void Mpu6050Node::setupCovariance(boost::array<double, 9> &cov, double stdev)
{
    std::fill(cov.begin(), cov.end(), 0.0);
    if (stdev == 0.0)
      cov[0] = -1.0;
    else
      cov[0 + 0] = cov[3 + 1] = cov[6 + 2] = std::pow(stdev, 2);
}

void Mpu6050Node::prepareImuMessage(const sensor_msgs::ImuPtr& msg)
{
  msg->header.frame_id = "imu";
  msg->orientation_covariance = orientation_cov_;
  msg->angular_velocity_covariance = angular_velocity_cov_;
  msg->linear_acceleration_covariance = linear_acceleration_cov_;
}

void Mpu6050Node::publishImuMessage(const sensor_msgs::ImuPtr& msg)
{
  imuPublisher_.publish(msg);
}

void Mpu6050Node::setup()
{
  mpu_ = new MPU6050(i2cAddress_);

  // initialize device
  ROS_INFO("Initializing I2C devices...\n");
  mpu_->initialize();

  // verify connection
  ROS_INFO("Testing device connections...\n");
  ROS_INFO(mpu_->testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n");

  // load and configure the DMP
  ROS_INFO("Initializing DMP...\n");
  devStatus_ = mpu_->dmpInitialize();

  // make sure it worked (returns 0 if so)
  if (devStatus_ == 0)
  {
    // turn on the DMP, now that it's ready
    ROS_INFO("Enabling DMP...\n");
    mpu_->setDMPEnabled(true);

    // enable Arduino interrupt detection
    //Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    //attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus_ = mpu_->getIntStatus();

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    ROS_INFO("DMP ready!\n");
    dmpReady_ = true;

    // get expected DMP packet size for later comparison
    packetSize_ = mpu_->dmpGetFIFOPacketSize();
  }
  else
  {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    ROS_ERROR_STREAM("DMP Initialization failed (code " << devStatus_ <<")\n");
  }
}

void Mpu6050Node::loop(const sensor_msgs::ImuPtr& msg)
{
  // if programming failed, don't try to do anything
  if (!dmpReady_) return;
  // get current FIFO count
  fifoCount_ = mpu_->getFIFOCount();

  if (fifoCount_ == 1024)
  {
    // reset so we can continue cleanly
    mpu_->resetFIFO();
    ROS_WARN("FIFO overflow!\n");

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
  }
  else if (fifoCount_ >= 42)
  {
    // read a packet from FIFO
    mpu_->getFIFOBytes(fifoBuffer_, packetSize_);

    ros::Time now = ros::Time::now();
    msg->header.stamp = now;
    ros::Duration dt_r = now - last_update_;
    last_update_ = now;

    // display quaternion values in easy matrix form: w x y z
    if (mpu_->dmpGetQuaternion(&q_, fifoBuffer_) == 0)
    {
      msg->orientation.x = q_.x;
      msg->orientation.y = q_.y;
      msg->orientation.z = q_.z;
      msg->orientation.w = q_.w;
    }
    else
    {
      msg->orientation.x = 1;
      msg->orientation.y = 0;
      msg->orientation.z = 0;
      msg->orientation.w = 0;
    }

    mpu_->dmpGetGravity(&gravity_, &q_);
    mpu_->dmpGetYawPitchRoll(ypr_, &q_, &gravity_);
    float yaw_ang_vel = ypr_[0] - last_ypr_[0] / dt_r.toSec();
    float pitch_ang_vel = ypr_[1] - last_ypr_[1] / dt_r.toSec();
    float roll_ang_vel = ypr_[2] - last_ypr_[2] / dt_r.toSec();

    msg->angular_velocity.x = roll_ang_vel;
    msg->angular_velocity.y = pitch_ang_vel;
    msg->angular_velocity.z = yaw_ang_vel;

    // display initial world-frame acceleration, adjusted to remove gravity
    // and rotated based on known orientation from quaternion
    mpu_->dmpGetAccel(&aa_, fifoBuffer_);
    mpu_->dmpGetLinearAccelInWorld(&aaWorld_, &aaReal_, &q_);
    //printf("aworld %6d %6d %6d    ", aaWorld.x, aaWorld.y, aaWorld.z);
    msg->linear_acceleration.x = aaWorld_.x;
    msg->linear_acceleration.y = aaWorld_.y;
    msg->linear_acceleration.z = aaWorld_.z;
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, RM_MPU6050_NODE_NAME);

  Mpu6050Node mpu6050Node;

  sensor_msgs::ImuPtr imuPrt(new sensor_msgs::Imu());
  mpu6050Node.prepareImuMessage(imuPrt);

  mpu6050Node.setup();

  ros::Rate rate(100);  // Hz
  while(ros::ok())
  {
    mpu6050Node.loop(imuPrt);
    mpu6050Node.publishImuMessage(imuPrt);

    ros::spinOnce();
    rate.sleep();
  }
  return 0;
}

/*
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>
#include "consts.h"

class Mpu6050Node
{
public:
  Mpu6050Node();
  void fillImuMessage(const sensor_msgs::ImuPtr& msg);
  void publishImuMessage(const sensor_msgs::ImuPtr& msg);
private:
  int i2cAddress_;
  int fileDescriptor_;
  float readWord2c(int addr);

  // Topic RM_IMU_TOPIC_NAME ('imu') publisher:
  ros::Publisher imuPublisher_;
};

const int PWR_MGMT_1 = 0x6B;

Mpu6050Node::Mpu6050Node()
{
  ros::NodeHandle privateNodeHandle("~");
  privateNodeHandle.param("i2c_address", i2cAddress_, 0x68);

  // Connect to device.
  fileDescriptor_ = wiringPiI2CSetup(i2cAddress_);
  if (fileDescriptor_ == -1)
  {
    ROS_ERROR("No i2c device found?");
    return;
  }
  // Device starts in sleep mode so wake it up.
  wiringPiI2CWriteReg16(fileDescriptor_, PWR_MGMT_1, 0);

  ros::NodeHandle nodeHandle(RM_NAMESPACE);
  imuPublisher_ = nodeHandle.advertise<sensor_msgs::Imu>(RM_IMU_TOPIC_NAME, 100);
}

float Mpu6050Node::readWord2c(int addr)
{
  int high = wiringPiI2CReadReg8(fileDescriptor_, addr);
  int low = wiringPiI2CReadReg8(fileDescriptor_, addr + 1);
  int val = (high << 8) + low;
  return float((val >= 0x8000) ? -((65535 - val) + 1) : val);
}

void Mpu6050Node::fillImuMessage(const sensor_msgs::ImuPtr& msg)
{
  msg->header.stamp = ros::Time::now();
  msg->header.frame_id = '0';  // no frame

  // Read gyroscope values.
  // At default sensitivity of 250deg/s we need to scale by 131.
  msg->angular_velocity.x = readWord2c(0x43) / 131;
  msg->angular_velocity.y = readWord2c(0x45) / 131;
  msg->angular_velocity.z = readWord2c(0x47) / 131;

  // Read accelerometer values.
  // At default sensitivity of 2g we need to scale by 16384.
  // Note: at "level" x = y = 0 but z = 1 (i.e. gravity)
  // But! Imu message documentations say acceleration should be in m/2 so need to *9.807
  const float la_rescale = 16384.0 / 9.807;
  msg->linear_acceleration.x = readWord2c(0x3b) / la_rescale;
  msg->linear_acceleration.y = readWord2c(0x3d) / la_rescale;
  msg->linear_acceleration.z = readWord2c(0x3f) / la_rescale;
}

void Mpu6050Node::publishImuMessage(const sensor_msgs::ImuPtr& msg)
{
  imuPublisher_.publish(msg);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, RM_MPU6050_NODE_NAME);

  Mpu6050Node mpu6050Node;
  sensor_msgs::ImuPtr imuPrt(new sensor_msgs::Imu());

  ros::Rate rate(10);  // hz
  while(ros::ok())
  {
    mpu6050Node.fillImuMessage(imuPrt);
    mpu6050Node.publishImuMessage(imuPrt);

    ros::spinOnce();
    rate.sleep();
  }
  return 0;
}
*/
<|endoftext|>


<commit_before>/*
 * mpu6050_node.cpp
 * Copyright (c) 2017, Robot Mitya.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * 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.
 *     * Neither the name of the Robot Mitya 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 OWNER 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.
 *
 *  Created on: Dec 3, 2017
 *      Author: Dmitry Dzakhov
 */

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include "std_msgs/String.h"
#include <wiringPi.h>
#include <wiringPiI2C.h>
#include "consts.h"
#include "mpu6050_helper.h"
#include "madgwick.h"

#define VALUES_TO_CALIBRATE 1000

class Mpu6050Node
{
public:
  Mpu6050Node();
  void readImuData(float *vX, float *vY, float *vZ, float *aX, float *aY, float *aZ);
  bool performingCalibration(float vX, float vY, float vZ);

  void fillImuMessage(const sensor_msgs::ImuPtr& msg, float vX, float vY, float vZ, float aX, float aY, float aZ);
  void publishImuMessage(const sensor_msgs::ImuPtr& msg);
  void afterCalibration();
private:
  int i2cAddress_;
  int fileDescriptor_;
  uint8_t buffer_[14];
  float gyroFactor_;

  float readWord2c(int addr);

  // Topic RM_IMU_TOPIC_NAME ('imu') publisher:
  ros::Publisher imuPublisher_;

  ros::Subscriber imuInputSubscriber_;
  void imuInputCallback(const std_msgs::StringConstPtr& msg);

  MpuHelper mpuHelper_;

  ros::Time prevStamp_;
  uint32_t prevSeq_;
  MadgwickImu madgwick_;

  uint32_t seq_;
};

const int PWR_MGMT_1 = 0x6B;
const int GYRO_CONFIG_ADDR = 27;
const int ACC_CONFIG_ADDR = 28;

enum GyroScale
{
  GYRO_FS_250DPS = 0,
  GYRO_FS_500DPS,
  GYRO_FS_1000DPS,
  GYRO_FS_2000DPS
};

Mpu6050Node::Mpu6050Node()
{
  ros::NodeHandle privateNodeHandle("~");
  privateNodeHandle.param("i2c_address", i2cAddress_, 0x68);
//  privateNodeHandle.

  // Connect to device.
  fileDescriptor_ = wiringPiI2CSetup(i2cAddress_);
  if (fileDescriptor_ == -1)
  {
    ROS_ERROR("No i2c device found?");
    return;
  }
  // Device starts in sleep mode so wake it up.
  wiringPiI2CWriteReg16(fileDescriptor_, PWR_MGMT_1, 0);

  // Setting gyroscope full scale range:
  GyroScale gyroScale = GYRO_FS_500DPS;
  // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
  uint8_t gyroConfig = wiringPiI2CReadReg8(fileDescriptor_, GYRO_CONFIG_ADDR);
  gyroConfig &= ~0xE0; // Clear self-test bits [7:5]
  gyroConfig &= ~0x18; // Clear FS bits [4:3]
  gyroConfig |= gyroScale << 3; // Set full scale range for the gyroscope
  wiringPiI2CWriteReg8(fileDescriptor_, GYRO_CONFIG_ADDR, gyroConfig);
  // Setting gyroscope factor:
  gyroFactor_ = 3.141592654f / 180.0f; // (convert to radians)
  switch (gyroScale)
  {
    case GYRO_FS_500DPS:
      gyroFactor_ *= 500.0f / 32768.0f;
      break;
    case GYRO_FS_1000DPS:
      gyroFactor_ *= 1000.0f / 32768.0f;
      break;
    case GYRO_FS_2000DPS:
      gyroFactor_ *= 2000.0f / 32768.0f;
      break;
    default:
      gyroFactor_ *= 250.0f / 32768.0f;
      break;
  }

  ros::NodeHandle nodeHandle(RM_NAMESPACE);
  imuPublisher_ = nodeHandle.advertise<sensor_msgs::Imu>(RM_HEAD_IMU_OUTPUT_TOPIC_NAME, 100);

  imuInputSubscriber_ = nodeHandle.subscribe(RM_HEAD_IMU_INPUT_TOPIC_NAME, 10, &Mpu6050Node::imuInputCallback, this);

  seq_ = 0;
  prevSeq_ = 0;
  prevStamp_ = ros::Time::now();
  madgwick_.center();
}

float Mpu6050Node::readWord2c(int addr)
{
  uint16_t val = wiringPiI2CReadReg16(fileDescriptor_, addr);
  uint16_t swappedVal = (val >> 8) | (val << 8);
  return float((swappedVal >= 0x8000) ? -((65535 - swappedVal) + 1) : swappedVal);
}

void Mpu6050Node::readImuData(float *vX, float *vY, float *vZ, float *aX, float *aY, float *aZ)
{
  // Read gyroscope values.
  // At default sensitivity of 250deg/s we need to scale by 131.
  *vX = -readWord2c(0x47) * gyroFactor_; // -z (caused by chip orientation in robot)
  *vY = readWord2c(0x45) * gyroFactor_;  // y
  *vZ = readWord2c(0x43) * gyroFactor_;  // x (caused by chip orientation in robot)

  // Read accelerometer values.
  // At default sensitivity of 2g we need to scale by 16384.
  // Note: at "level" x = y = 0 but z = 1 (i.e. gravity)
  // But! Imu message documentations say acceleration should be in m/2 so need to *9.807
  const float la_rescale = 16384.0 / 9.807;
  *aX = -readWord2c(0x3f) / la_rescale; // -z (caused by chip orientation in robot)
  *aY = readWord2c(0x3d) / la_rescale; // y
  *aZ = readWord2c(0x3b) / la_rescale; // x (caused by chip orientation in robot)

  mpuHelper_.correctMpuData(vX, vY, vZ);
}

void Mpu6050Node::fillImuMessage(const sensor_msgs::ImuPtr& msg, float vX, float vY, float vZ, float aX, float aY, float aZ)
{
  msg->header.stamp = ros::Time::now();
  msg->header.seq = seq_++;
  msg->header.frame_id = '0';  // no frame

  msg->angular_velocity.x = vX;
  msg->angular_velocity.y = vY;
  msg->angular_velocity.z = vZ;

  msg->linear_acceleration.x = aX;
  msg->linear_acceleration.y = aY;
  msg->linear_acceleration.z = aZ;

  ros::Duration deltaTime = msg->header.stamp - prevStamp_;
  prevStamp_ = msg->header.stamp;
  madgwick_.update(deltaTime.toSec(), vX, vY, vZ, aX, aY, aZ);

  tf2Scalar qX, qY, qZ, qW;
  madgwick_.getQuaternion(qX, qY, qZ, qW);
  msg->orientation.x = qX;
  msg->orientation.y = qY;
  msg->orientation.z = qZ;
  msg->orientation.w = qW;
}

void Mpu6050Node::publishImuMessage(const sensor_msgs::ImuPtr& msg)
{
  imuPublisher_.publish(msg);
}

void Mpu6050Node::imuInputCallback(const std_msgs::StringConstPtr& msg)
{
  if (msg->data.compare("calibrate") == 0)
  {
    ROS_INFO("Starting to calibrate head IMU...");
    mpuHelper_.startCalibration();
  }
  else if (msg->data.compare("center") == 0)
  {
    ROS_INFO("Center head IMU...");
    madgwick_.center();
  }
  else if (msg->data.compare("configuration") == 0)
  {
    uint8_t gyroConfig = wiringPiI2CReadReg8(fileDescriptor_, GYRO_CONFIG_ADDR);
    uint8_t accConfig = wiringPiI2CReadReg8(fileDescriptor_, ACC_CONFIG_ADDR);
    ROS_INFO("GYRO_CONFIG (register %d) = %d    ACCEL_CONFIG (register %d) = %d", GYRO_CONFIG_ADDR, gyroConfig, ACC_CONFIG_ADDR, accConfig);
  }
  else
  {
    ROS_ERROR("%s.%s: Unknown command \"%s\"", RM_MPU6050_NODE_NAME, RM_HEAD_IMU_INPUT_TOPIC_NAME, msg->data.c_str());
  }
}

bool Mpu6050Node::performingCalibration(float vX, float vY, float vZ)
{
  return mpuHelper_.processCalibration(vX, vY, vZ);
}

void Mpu6050Node::afterCalibration()
{
  prevStamp_ = ros::Time::now();
  madgwick_.center();
  ROS_INFO("Stopping to calibrate head IMU");
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, RM_MPU6050_NODE_NAME);

  Mpu6050Node mpu6050Node;
  sensor_msgs::ImuPtr imuPrt(new sensor_msgs::Imu());

  float vX, vY, vZ, aX, aY, aZ;
  ros::Rate rate(50);  // hz
  bool prevPerformingCalibration = false;
  bool performingCalibration;
  while(ros::ok())
  {
    mpu6050Node.readImuData(&vX, &vY, &vZ, &aX, &aY, &aZ);

    performingCalibration = mpu6050Node.performingCalibration(vX, vY, vZ);
    if (!performingCalibration)
    {
      if (prevPerformingCalibration)
      {
        // Calibration is over:
        mpu6050Node.afterCalibration();
      }
      mpu6050Node.fillImuMessage(imuPrt, vX, vY, vZ, aX, aY, aZ);
      mpu6050Node.publishImuMessage(imuPrt);
    }
    prevPerformingCalibration = performingCalibration;

    ros::spinOnce();
    rate.sleep();
  }
  return 0;
}
<commit_msg>#16 Fixing roll, pitch, yaw axis orientation<commit_after>/*
 * mpu6050_node.cpp
 * Copyright (c) 2017, Robot Mitya.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * 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.
 *     * Neither the name of the Robot Mitya 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 OWNER 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.
 *
 *  Created on: Dec 3, 2017
 *      Author: Dmitry Dzakhov
 */

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include "std_msgs/String.h"
#include <wiringPi.h>
#include <wiringPiI2C.h>
#include "consts.h"
#include "mpu6050_helper.h"
#include "madgwick.h"

#define VALUES_TO_CALIBRATE 1000

class Mpu6050Node
{
public:
  Mpu6050Node();
  void readImuData(float *vX, float *vY, float *vZ, float *aX, float *aY, float *aZ);
  bool performingCalibration(float vX, float vY, float vZ);

  void fillImuMessage(const sensor_msgs::ImuPtr& msg, float vX, float vY, float vZ, float aX, float aY, float aZ);
  void publishImuMessage(const sensor_msgs::ImuPtr& msg);
  void afterCalibration();
private:
  int i2cAddress_;
  int fileDescriptor_;
  uint8_t buffer_[14];
  float gyroFactor_;

  float readWord2c(int addr);

  // Topic RM_IMU_TOPIC_NAME ('imu') publisher:
  ros::Publisher imuPublisher_;

  ros::Subscriber imuInputSubscriber_;
  void imuInputCallback(const std_msgs::StringConstPtr& msg);

  MpuHelper mpuHelper_;

  ros::Time prevStamp_;
  uint32_t prevSeq_;
  MadgwickImu madgwick_;

  uint32_t seq_;
};

const int PWR_MGMT_1 = 0x6B;
const int GYRO_CONFIG_ADDR = 27;
const int ACC_CONFIG_ADDR = 28;

enum GyroScale
{
  GYRO_FS_250DPS = 0,
  GYRO_FS_500DPS,
  GYRO_FS_1000DPS,
  GYRO_FS_2000DPS
};

Mpu6050Node::Mpu6050Node()
{
  ros::NodeHandle privateNodeHandle("~");
  privateNodeHandle.param("i2c_address", i2cAddress_, 0x68);
//  privateNodeHandle.

  // Connect to device.
  fileDescriptor_ = wiringPiI2CSetup(i2cAddress_);
  if (fileDescriptor_ == -1)
  {
    ROS_ERROR("No i2c device found?");
    return;
  }
  // Device starts in sleep mode so wake it up.
  wiringPiI2CWriteReg16(fileDescriptor_, PWR_MGMT_1, 0);

  // Setting gyroscope full scale range:
  GyroScale gyroScale = GYRO_FS_500DPS;
  // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
  uint8_t gyroConfig = wiringPiI2CReadReg8(fileDescriptor_, GYRO_CONFIG_ADDR);
  gyroConfig &= ~0xE0; // Clear self-test bits [7:5]
  gyroConfig &= ~0x18; // Clear FS bits [4:3]
  gyroConfig |= gyroScale << 3; // Set full scale range for the gyroscope
  wiringPiI2CWriteReg8(fileDescriptor_, GYRO_CONFIG_ADDR, gyroConfig);
  // Setting gyroscope factor:
  gyroFactor_ = 3.141592654f / 180.0f; // (convert to radians)
  switch (gyroScale)
  {
    case GYRO_FS_500DPS:
      gyroFactor_ *= 500.0f / 32768.0f;
      break;
    case GYRO_FS_1000DPS:
      gyroFactor_ *= 1000.0f / 32768.0f;
      break;
    case GYRO_FS_2000DPS:
      gyroFactor_ *= 2000.0f / 32768.0f;
      break;
    default:
      gyroFactor_ *= 250.0f / 32768.0f;
      break;
  }

  ros::NodeHandle nodeHandle(RM_NAMESPACE);
  imuPublisher_ = nodeHandle.advertise<sensor_msgs::Imu>(RM_HEAD_IMU_OUTPUT_TOPIC_NAME, 100);

  imuInputSubscriber_ = nodeHandle.subscribe(RM_HEAD_IMU_INPUT_TOPIC_NAME, 10, &Mpu6050Node::imuInputCallback, this);

  seq_ = 0;
  prevSeq_ = 0;
  prevStamp_ = ros::Time::now();
  madgwick_.center();
}

float Mpu6050Node::readWord2c(int addr)
{
  uint16_t val = wiringPiI2CReadReg16(fileDescriptor_, addr);
  uint16_t swappedVal = (val >> 8) | (val << 8);
  return float((swappedVal >= 0x8000) ? -((65535 - swappedVal) + 1) : swappedVal);
}

void Mpu6050Node::readImuData(float *vX, float *vY, float *vZ, float *aX, float *aY, float *aZ)
{
  // Read gyroscope values.
  *vX = readWord2c(0x45) * gyroFactor_; // y (caused by chip orientation in robot)
  *vY = readWord2c(0x47) * gyroFactor_; // z (caused by chip orientation in robot)
  *vZ = readWord2c(0x43) * gyroFactor_; // x (caused by chip orientation in robot)

  // Read accelerometer values.
  // At default sensitivity of 2g we need to scale by 16384.
  const float la_rescale = 16384.0f;
  *aX = readWord2c(0x3d) / la_rescale; // y (caused by chip orientation in robot)
  *aY = readWord2c(0x3f) / la_rescale; // z (caused by chip orientation in robot)
  *aZ = readWord2c(0x3b) / la_rescale; // x (caused by chip orientation in robot)

  mpuHelper_.correctMpuData(vX, vY, vZ);
}

void Mpu6050Node::fillImuMessage(const sensor_msgs::ImuPtr& msg, float vX, float vY, float vZ, float aX, float aY, float aZ)
{
  msg->header.stamp = ros::Time::now();
  msg->header.seq = seq_++;
  msg->header.frame_id = '0';  // no frame

  msg->angular_velocity.x = vX;
  msg->angular_velocity.y = vY;
  msg->angular_velocity.z = vZ;

  msg->linear_acceleration.x = aX;
  msg->linear_acceleration.y = aY;
  msg->linear_acceleration.z = aZ;

  ros::Duration deltaTime = msg->header.stamp - prevStamp_;
  prevStamp_ = msg->header.stamp;
  madgwick_.update(deltaTime.toSec(), vX, vY, vZ, aX, aY, aZ);

  tf2Scalar qX, qY, qZ, qW;
  madgwick_.getQuaternion(qX, qY, qZ, qW);
  msg->orientation.x = qX;
  msg->orientation.y = qY;
  msg->orientation.z = qZ;
  msg->orientation.w = qW;
}

void Mpu6050Node::publishImuMessage(const sensor_msgs::ImuPtr& msg)
{
  imuPublisher_.publish(msg);
}

void Mpu6050Node::imuInputCallback(const std_msgs::StringConstPtr& msg)
{
  if (msg->data.compare("calibrate") == 0)
  {
    ROS_INFO("Starting to calibrate head IMU...");
    mpuHelper_.startCalibration();
  }
  else if (msg->data.compare("center") == 0)
  {
    ROS_INFO("Center head IMU...");
    madgwick_.center();
  }
  else if (msg->data.compare("configuration") == 0)
  {
    uint8_t gyroConfig = wiringPiI2CReadReg8(fileDescriptor_, GYRO_CONFIG_ADDR);
    uint8_t accConfig = wiringPiI2CReadReg8(fileDescriptor_, ACC_CONFIG_ADDR);
    ROS_INFO("GYRO_CONFIG (register %d) = %d    ACCEL_CONFIG (register %d) = %d", GYRO_CONFIG_ADDR, gyroConfig, ACC_CONFIG_ADDR, accConfig);
  }
  else
  {
    ROS_ERROR("%s.%s: Unknown command \"%s\"", RM_MPU6050_NODE_NAME, RM_HEAD_IMU_INPUT_TOPIC_NAME, msg->data.c_str());
  }
}

bool Mpu6050Node::performingCalibration(float vX, float vY, float vZ)
{
  return mpuHelper_.processCalibration(vX, vY, vZ);
}

void Mpu6050Node::afterCalibration()
{
  prevStamp_ = ros::Time::now();
  madgwick_.center();
  ROS_INFO("Stopping to calibrate head IMU");
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, RM_MPU6050_NODE_NAME);

  Mpu6050Node mpu6050Node;
  sensor_msgs::ImuPtr imuPrt(new sensor_msgs::Imu());

  float vX, vY, vZ, aX, aY, aZ;
  ros::Rate rate(50);  // hz
  bool prevPerformingCalibration = false;
  bool performingCalibration;
  while(ros::ok())
  {
    mpu6050Node.readImuData(&vX, &vY, &vZ, &aX, &aY, &aZ);

    performingCalibration = mpu6050Node.performingCalibration(vX, vY, vZ);
    if (!performingCalibration)
    {
      if (prevPerformingCalibration)
      {
        // Calibration is over:
        mpu6050Node.afterCalibration();
      }
      mpu6050Node.fillImuMessage(imuPrt, vX, vY, vZ, aX, aY, aZ);
      mpu6050Node.publishImuMessage(imuPrt);
    }
    prevPerformingCalibration = performingCalibration;

    ros::spinOnce();
    rate.sleep();
  }
  return 0;
}
<|endoftext|>


<commit_before>#include "tool_picker.h"

using namespace std;
using namespace baxter_core_msgs;

ToolPicker::ToolPicker(std::string _name, std::string _limb, bool _no_robot) :
                       HoldCtrl(_name,_limb, _no_robot), CartesianEstimatorClient(_name, _limb),
                       elap_time(0)
{
    setHomeConfiguration();

    setState(START);

    insertAction(ACTION_GET,       static_cast<f_action>(&ToolPicker::getObject));
    insertAction(ACTION_PASS,      static_cast<f_action>(&ToolPicker::passObject));
    insertAction(ACTION_GET_PASS,  static_cast<f_action>(&ToolPicker::getPassObject));
    insertAction(ACTION_CLEANUP,   static_cast<f_action>(&ToolPicker::cleanUpObject));

    printActionDB();

    if (_no_robot) return;

    reduceSquish();

    if (!callAction(ACTION_HOME)) setState(ERROR);
}

void ToolPicker::reduceSquish()
{
    XmlRpc::XmlRpcValue squish_params;
    // store the initial squish thresholds from the parameter server
    _n.getParam("collision/right/baxter/squish_thresholds", squish_params);
    ROS_ASSERT(squish_params.getType()==XmlRpc::XmlRpcValue::TypeArray);

    for (int i = 0; i < squish_params.size(); ++i)
    {
        // store the initial squish thresholds for reset later
        squish_thresholds.push_back(squish_params[i]);
    }

    // adjust the squish thresholds for better tool picking
    squish_params[3] = 0.5 * static_cast<double>(squish_params[3]);
    squish_params[5] = 0.5 * static_cast<double>(squish_params[5]);

    // set the squish thresholds in the parameter server to the new values
    _n.setParam("collision/right/baxter/squish_thresholds", squish_params);
}

void ToolPicker::resetSquish()
{
    XmlRpc::XmlRpcValue squish_params;
    for (std::vector<int>::size_type i = 0; i != squish_thresholds.size(); i++)
    {
        // rewrite the squish parameters from the initial squish thresholds stored in reduceSquish()
        squish_params[i] = squish_thresholds[i];
    }

    // reset squish thresholds in the parameter server to the new values
    _n.setParam("collision/right/baxter/squish_thresholds", squish_params);
}

bool ToolPicker::pickUpObject()
{
    ROS_INFO("[%s] Start Picking up object %s..", getLimb().c_str(),
                  CartesianEstimatorClient::getObjectName().c_str());

    if (!is_ir_ok())
    {
        ROS_ERROR("No callback from the IR sensor! Stopping.");
        setSubState(NO_IR_SENSOR);
        return false;
    }

    if (!waitForCartEstData())
    {
        setSubState(NO_OBJ);
        return false;
    }

    double offs_x = 0.0;
    double offs_y = 0.0;

    if (!computeOffsets(offs_x, offs_y))    return false;

    double x = getObjectPos().x + offs_x;
    double y = getObjectPos().y + offs_y;
    double z =       getPos().z;

    geometry_msgs::Quaternion q;
    if (!computeOrientation(q))             return false;

    ROS_INFO("Going to: %g %g %g", x, y, z);
    if (!goToPose(x, y, z, q.x, q.y, q.z, q.w, "loose"))
    {
        return false;
    }

    if (!waitForCartEstData())
    {
        setSubState(NO_OBJ);
        return false;
    }

    ros::Time start_time = ros::Time::now();
    double z_start       =       getPos().z;
    int cnt_ik_fail = 0;

    ros::Rate r(THREAD_FREQ);

    while(RobotInterface::ok())
    {
        double new_elap_time = (ros::Time::now() - start_time).toSec();

        x = getObjectPos().x + offs_x;
        y = getObjectPos().y + offs_y;
        z = z_start - ARM_SPEED * new_elap_time / 1.3;

        ROS_DEBUG("Time %g Going to: %g %g %g Position: %g %g %g", new_elap_time, x, y, z,
                                                       getPos().x, getPos().y, getPos().z);

        if (goToPoseNoCheck(x,y,z,q.x, q.y, q.z, q.w))
        {
            cnt_ik_fail = 0;
            if (new_elap_time - elap_time > 0.02)
            {
                ROS_WARN("\t\t\t\t\tTime elapsed: %g", new_elap_time - elap_time);
            }
            elap_time = new_elap_time;

            if(hasCollidedIR("strict") || hasCollidedCD())
            {
                if (hasCollidedCD())
                {
                    moveArm("up", 0.002);
                }
                ROS_INFO("Collision!");
                return true;
            }

            r.sleep();
        }
        else    cnt_ik_fail++;

        if (cnt_ik_fail == 10)  return false;
    }

    return false;
}

bool ToolPicker::computeOffsets(double &_x_offs, double &_y_offs)
{
    if      (getAction() == ACTION_GET || getAction() == ACTION_GET_PASS)
    {
        if (CartesianEstimatorClient::getObjectName() == "screwdriver")
        {
            _x_offs = +0.010;
            _y_offs = +0.017;
        }
        else if (CartesianEstimatorClient::getObjectName() == "screws_box"  ||
                 CartesianEstimatorClient::getObjectName() == "brackets_box")
        {
            _x_offs = +0.06;
        }
    }
    else if (getAction() == ACTION_CLEANUP)
    {
        if (CartesianEstimatorClient::getObjectName() == "screwdriver")
        {
            _x_offs = -0.020;
            _y_offs = -0.010;
        }
        else if (CartesianEstimatorClient::getObjectName() == "screws_box"  ||
                 CartesianEstimatorClient::getObjectName() == "brackets_box")
        {
            _x_offs =  0.020;
            _y_offs = -0.058;
        }
    }
    else
    {
        ROS_ERROR("State is neither ACTION_GET, ACTION_GET_PASS or ACTION_CLEANUP!");
        return false;
    }

    return true;
}

bool ToolPicker::computeOrientation(geometry_msgs::Quaternion &_q)
{
    if      (getAction() == ACTION_GET || getAction() == ACTION_GET_PASS)
    {
        quaternionFromDoubles(_q, POOL_ORI_R);
    }
    else if (getAction() == ACTION_CLEANUP)
    {
        quaternionFromDoubles(_q, VERTICAL_ORI_R);
    }
    else
    {
        ROS_ERROR("State is neither ACTION_GET, ACTION_GET_PASS or ACTION_CLEANUP!");
        return false;
    }

    return true;
}

int ToolPicker::chooseObjectID(std::vector<int> _objs)
{
    if (getSubState() != CHECK_OBJ_IDS)
    {
        return ArmCtrl::chooseObjectID(_objs);
    }

    ROS_DEBUG("[%s] Choosing object IDs", getLimb().c_str());
    int res = -1;

    if (!waitForCartEstOK())
    {
        setSubState(NO_OBJ);
        return false;
    }

    if (!waitForCartEstObjsFound())
    {
        setSubState(NO_OBJ);
        return false;
    }

    std::vector<string> objs_str;
    for (size_t i = 0; i < _objs.size(); ++i)
    {
        objs_str.push_back(getObjectNameFromDB(_objs[i]));
    }

    std::vector<string> av_objects = getAvailableObjects(objs_str);

    if (av_objects.size() == 0)     return res;

    std::srand(std::time(0)); //use current time as seed
    string res_str = av_objects[rand() % av_objects.size()];

    res = getObjectIDFromDB(res_str);

    return res;
}

bool ToolPicker::getObject()
{
    if (!homePoseStrict())          return false;
    ros::Duration(0.05).sleep();
    if (!pickUpObject())            return false;
    if (!gripObject())              return false;
    if (!moveArm("up", 0.3))        return false;
    // if (!hoverAboveTable(Z_LOW))    return false;

    return true;
}

bool ToolPicker::passObject()
{
    if (getPrevAction() != ACTION_GET)  return false;

    if (CartesianEstimatorClient::getObjectName() == "screwdriver")
    {
        if (!goToPose(0.85, -0.26, 0.27, HORIZONTAL_ORI_R)) return false;
        ros::Duration(1.0).sleep();
        if (!waitForForceInteraction())                     return false;
        if (!releaseObject())                               return false;
    }
    else if (CartesianEstimatorClient::getObjectName() == "screws_box"  ||
             CartesianEstimatorClient::getObjectName() == "brackets_box")
    {
        if (!hoverAboveTable(Z_LOW))    return false;

        if (CartesianEstimatorClient::getObjectName() == "brackets_box")
        {
            if (!goToPose(0.65, -0.10, -0.12, VERTICAL_ORI_R)) return false;
        }
        else
        {
            if (!goToPose(0.65, -0.30, -0.12, VERTICAL_ORI_R)) return false;
        }

        ros::Duration(0.5).sleep();
        if (!releaseObject())           return false;
        if (!hoverAboveTable(Z_LOW))    return false;
    }

    if (!homePoseStrict())              return false;

    return true;
}

bool ToolPicker::getPassObject()
{
    if (!getObject())      return false;
    setPrevAction(ACTION_GET);
    if (!passObject())     return false;

    return true;
}

bool ToolPicker::cleanUpObject()
{
    if (!goToPose(0.65, -0.25, 0.25, VERTICAL_ORI_R)) return false;
    ros::Duration(0.05).sleep();

    if (getObjectIDs().size() >  1)
    {
        setSubState(CHECK_OBJ_IDS);
        setObjectID(chooseObjectID(getObjectIDs()));
        ROS_INFO("[%s] Chosen object with ID %i", getLimb().c_str(),
                                                     getObjectID());
    }

    if (!waitForCartEstObjFound())
    {
        setSubState(NO_OBJ);
        return false;
    }

    if (!pickUpObject())            return false;
    if (!gripObject())              return false;
    if (!moveArm("up", 0.3))        return false;
    if (!homePoseStrict())          return false;

    if (CartesianEstimatorClient::getObjectName() == "screwdriver")
    {
        if (!goToPose(0.20, -0.85, -0.27, POOL_ORI_R)) return false;
    }
    else if (CartesianEstimatorClient::getObjectName() == "brackets_box")
    {
        if (!goToPose(0.00, -0.85, -0.25, POOL_ORI_R)) return false;
    }
    else if (CartesianEstimatorClient::getObjectName() == "screws_box")
    {
        if (!goToPose(-0.15, -0.85, -0.25, POOL_ORI_R)) return false;
    }

    ros::Duration(0.5).sleep();
    if (!releaseObject())           return false;
    if (!homePoseStrict())          return false;

    return true;
}

void ToolPicker::setHomeConfiguration()
{
    ArmCtrl::setHomeConfiguration("pool");
}

void ToolPicker::setObjectID(int _obj)
{
    ArmCtrl::setObjectID(_obj);
    CartesianEstimatorClient::setObjectName(ArmCtrl::getObjectNameFromDB(_obj));
}

ToolPicker::~ToolPicker()
{
    resetSquish();
}
<commit_msg>will work for left and right arm now<commit_after>#include "tool_picker.h"

using namespace std;
using namespace baxter_core_msgs;

ToolPicker::ToolPicker(std::string _name, std::string _limb, bool _no_robot) :
                       HoldCtrl(_name,_limb, _no_robot), CartesianEstimatorClient(_name, _limb),
                       elap_time(0)
{
    setHomeConfiguration();

    setState(START);

    insertAction(ACTION_GET,       static_cast<f_action>(&ToolPicker::getObject));
    insertAction(ACTION_PASS,      static_cast<f_action>(&ToolPicker::passObject));
    insertAction(ACTION_GET_PASS,  static_cast<f_action>(&ToolPicker::getPassObject));
    insertAction(ACTION_CLEANUP,   static_cast<f_action>(&ToolPicker::cleanUpObject));

    printActionDB();

    if (_no_robot) return;

    reduceSquish();

    if (!callAction(ACTION_HOME)) setState(ERROR);
}

void ToolPicker::reduceSquish()
{
    XmlRpc::XmlRpcValue squish_params;
    // store the initial squish thresholds from the parameter server
    _n.getParam("collision/right/baxter/squish_thresholds", squish_params);
    ROS_ASSERT(squish_params.getType()==XmlRpc::XmlRpcValue::TypeArray);

    for (int i = 0; i < squish_params.size(); ++i)
    {
        // store the initial squish thresholds for reset later
        squish_thresholds.push_back(squish_params[i]);
    }

    // adjust the squish thresholds for better tool picking
    squish_params[3] = 0.5 * static_cast<double>(squish_params[3]);
    squish_params[5] = 0.5 * static_cast<double>(squish_params[5]);

    // set the squish thresholds in the parameter server to the new values
    _n.setParam("collision/"+getLimb()"/baxter/squish_thresholds", squish_params);
}

void ToolPicker::resetSquish()
{
    XmlRpc::XmlRpcValue squish_params;
    for (std::vector<int>::size_type i = 0; i != squish_thresholds.size(); i++)
    {
        // rewrite the squish parameters from the initial squish thresholds stored in reduceSquish()
        squish_params[i] = squish_thresholds[i];
    }

    // reset squish thresholds in the parameter server to the new values
    _n.setParam("collision/right/baxter/squish_thresholds", squish_params);
}

bool ToolPicker::pickUpObject()
{
    ROS_INFO("[%s] Start Picking up object %s..", getLimb().c_str(),
                  CartesianEstimatorClient::getObjectName().c_str());

    if (!is_ir_ok())
    {
        ROS_ERROR("No callback from the IR sensor! Stopping.");
        setSubState(NO_IR_SENSOR);
        return false;
    }

    if (!waitForCartEstData())
    {
        setSubState(NO_OBJ);
        return false;
    }

    double offs_x = 0.0;
    double offs_y = 0.0;

    if (!computeOffsets(offs_x, offs_y))    return false;

    double x = getObjectPos().x + offs_x;
    double y = getObjectPos().y + offs_y;
    double z =       getPos().z;

    geometry_msgs::Quaternion q;
    if (!computeOrientation(q))             return false;

    ROS_INFO("Going to: %g %g %g", x, y, z);
    if (!goToPose(x, y, z, q.x, q.y, q.z, q.w, "loose"))
    {
        return false;
    }

    if (!waitForCartEstData())
    {
        setSubState(NO_OBJ);
        return false;
    }

    ros::Time start_time = ros::Time::now();
    double z_start       =       getPos().z;
    int cnt_ik_fail = 0;

    ros::Rate r(THREAD_FREQ);

    while(RobotInterface::ok())
    {
        double new_elap_time = (ros::Time::now() - start_time).toSec();

        x = getObjectPos().x + offs_x;
        y = getObjectPos().y + offs_y;
        z = z_start - ARM_SPEED * new_elap_time / 1.3;

        ROS_DEBUG("Time %g Going to: %g %g %g Position: %g %g %g", new_elap_time, x, y, z,
                                                       getPos().x, getPos().y, getPos().z);

        if (goToPoseNoCheck(x,y,z,q.x, q.y, q.z, q.w))
        {
            cnt_ik_fail = 0;
            if (new_elap_time - elap_time > 0.02)
            {
                ROS_WARN("\t\t\t\t\tTime elapsed: %g", new_elap_time - elap_time);
            }
            elap_time = new_elap_time;

            if(hasCollidedIR("strict") || hasCollidedCD())
            {
                if (hasCollidedCD())
                {
                    moveArm("up", 0.002);
                }
                ROS_INFO("Collision!");
                return true;
            }

            r.sleep();
        }
        else    cnt_ik_fail++;

        if (cnt_ik_fail == 10)  return false;
    }

    return false;
}

bool ToolPicker::computeOffsets(double &_x_offs, double &_y_offs)
{
    if      (getAction() == ACTION_GET || getAction() == ACTION_GET_PASS)
    {
        if (CartesianEstimatorClient::getObjectName() == "screwdriver")
        {
            _x_offs = +0.010;
            _y_offs = +0.017;
        }
        else if (CartesianEstimatorClient::getObjectName() == "screws_box"  ||
                 CartesianEstimatorClient::getObjectName() == "brackets_box")
        {
            _x_offs = +0.06;
        }
    }
    else if (getAction() == ACTION_CLEANUP)
    {
        if (CartesianEstimatorClient::getObjectName() == "screwdriver")
        {
            _x_offs = -0.020;
            _y_offs = -0.010;
        }
        else if (CartesianEstimatorClient::getObjectName() == "screws_box"  ||
                 CartesianEstimatorClient::getObjectName() == "brackets_box")
        {
            _x_offs =  0.020;
            _y_offs = -0.058;
        }
    }
    else
    {
        ROS_ERROR("State is neither ACTION_GET, ACTION_GET_PASS or ACTION_CLEANUP!");
        return false;
    }

    return true;
}

bool ToolPicker::computeOrientation(geometry_msgs::Quaternion &_q)
{
    if      (getAction() == ACTION_GET || getAction() == ACTION_GET_PASS)
    {
        quaternionFromDoubles(_q, POOL_ORI_R);
    }
    else if (getAction() == ACTION_CLEANUP)
    {
        quaternionFromDoubles(_q, VERTICAL_ORI_R);
    }
    else
    {
        ROS_ERROR("State is neither ACTION_GET, ACTION_GET_PASS or ACTION_CLEANUP!");
        return false;
    }

    return true;
}

int ToolPicker::chooseObjectID(std::vector<int> _objs)
{
    if (getSubState() != CHECK_OBJ_IDS)
    {
        return ArmCtrl::chooseObjectID(_objs);
    }

    ROS_DEBUG("[%s] Choosing object IDs", getLimb().c_str());
    int res = -1;

    if (!waitForCartEstOK())
    {
        setSubState(NO_OBJ);
        return false;
    }

    if (!waitForCartEstObjsFound())
    {
        setSubState(NO_OBJ);
        return false;
    }

    std::vector<string> objs_str;
    for (size_t i = 0; i < _objs.size(); ++i)
    {
        objs_str.push_back(getObjectNameFromDB(_objs[i]));
    }

    std::vector<string> av_objects = getAvailableObjects(objs_str);

    if (av_objects.size() == 0)     return res;

    std::srand(std::time(0)); //use current time as seed
    string res_str = av_objects[rand() % av_objects.size()];

    res = getObjectIDFromDB(res_str);

    return res;
}

bool ToolPicker::getObject()
{
    if (!homePoseStrict())          return false;
    ros::Duration(0.05).sleep();
    if (!pickUpObject())            return false;
    if (!gripObject())              return false;
    if (!moveArm("up", 0.3))        return false;
    // if (!hoverAboveTable(Z_LOW))    return false;

    return true;
}

bool ToolPicker::passObject()
{
    if (getPrevAction() != ACTION_GET)  return false;

    if (CartesianEstimatorClient::getObjectName() == "screwdriver")
    {
        if (!goToPose(0.85, -0.26, 0.27, HORIZONTAL_ORI_R)) return false;
        ros::Duration(1.0).sleep();
        if (!waitForForceInteraction())                     return false;
        if (!releaseObject())                               return false;
    }
    else if (CartesianEstimatorClient::getObjectName() == "screws_box"  ||
             CartesianEstimatorClient::getObjectName() == "brackets_box")
    {
        if (!hoverAboveTable(Z_LOW))    return false;

        if (CartesianEstimatorClient::getObjectName() == "brackets_box")
        {
            if (!goToPose(0.65, -0.10, -0.12, VERTICAL_ORI_R)) return false;
        }
        else
        {
            if (!goToPose(0.65, -0.30, -0.12, VERTICAL_ORI_R)) return false;
        }

        ros::Duration(0.5).sleep();
        if (!releaseObject())           return false;
        if (!hoverAboveTable(Z_LOW))    return false;
    }

    if (!homePoseStrict())              return false;

    return true;
}

bool ToolPicker::getPassObject()
{
    if (!getObject())      return false;
    setPrevAction(ACTION_GET);
    if (!passObject())     return false;

    return true;
}

bool ToolPicker::cleanUpObject()
{
    if (!goToPose(0.65, -0.25, 0.25, VERTICAL_ORI_R)) return false;
    ros::Duration(0.05).sleep();

    if (getObjectIDs().size() >  1)
    {
        setSubState(CHECK_OBJ_IDS);
        setObjectID(chooseObjectID(getObjectIDs()));
        ROS_INFO("[%s] Chosen object with ID %i", getLimb().c_str(),
                                                     getObjectID());
    }

    if (!waitForCartEstObjFound())
    {
        setSubState(NO_OBJ);
        return false;
    }

    if (!pickUpObject())            return false;
    if (!gripObject())              return false;
    if (!moveArm("up", 0.3))        return false;
    if (!homePoseStrict())          return false;

    if (CartesianEstimatorClient::getObjectName() == "screwdriver")
    {
        if (!goToPose(0.20, -0.85, -0.27, POOL_ORI_R)) return false;
    }
    else if (CartesianEstimatorClient::getObjectName() == "brackets_box")
    {
        if (!goToPose(0.00, -0.85, -0.25, POOL_ORI_R)) return false;
    }
    else if (CartesianEstimatorClient::getObjectName() == "screws_box")
    {
        if (!goToPose(-0.15, -0.85, -0.25, POOL_ORI_R)) return false;
    }

    ros::Duration(0.5).sleep();
    if (!releaseObject())           return false;
    if (!homePoseStrict())          return false;

    return true;
}

void ToolPicker::setHomeConfiguration()
{
    ArmCtrl::setHomeConfiguration("pool");
}

void ToolPicker::setObjectID(int _obj)
{
    ArmCtrl::setObjectID(_obj);
    CartesianEstimatorClient::setObjectName(ArmCtrl::getObjectNameFromDB(_obj));
}

ToolPicker::~ToolPicker()
{
    resetSquish();
}
<|endoftext|>


<commit_before>/**
 * Copyright (c) 2011-2014 libbitcoin developers (see AUTHORS)
 *
 * This file is part of libbitcoin-explorer.
 *
 * libbitcoin-explorer is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Affero General Public License with
 * additional permissions to the one published by the Free Software
 * Foundation, either version 3 of the License, or (at your option)
 * any later version. For more information see LICENSE.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */

#include <bitcoin/explorer/commands/fetch-tx-index.hpp>

#include <iostream>
#include <boost/format.hpp>
#include <bitcoin/bitcoin.hpp>
#include <bitcoin/explorer/callback_state.hpp>
#include <bitcoin/explorer/define.hpp>
#include <bitcoin/explorer/obelisk_client.hpp>
#include <bitcoin/explorer/primitives/base16.hpp>
#include <bitcoin/explorer/prop_tree.hpp>
#include <bitcoin/explorer/utility/utility.hpp>

using namespace bc;
using namespace bc::client;
using namespace bc::explorer;
using namespace bc::explorer::commands;
using namespace bc::explorer::primitives;

static void handle_error(callback_state& state, const std::error_code& error)
{
    state.handle_error(error);
}

static void handle_callback(callback_state& state, const hash_digest& hash,
    size_t height, size_t index)
{
    // native is info.
    state.output(prop_tree(hash, height, index));
}

static void fetch_tx_index_from_hash(obelisk_client& client,
    callback_state& state, const primitives::btc256& hash)
{
    // Do not pass the hash by reference here.
    auto on_done = [&state, hash](size_t block_height, size_t index)
    {
        handle_callback(state, hash, block_height, index);
    };

    auto on_error = [&state](const std::error_code& error)
    {
        handle_error(state, error);
    };

    client.get_codec()->fetch_transaction_index(on_error, on_done, hash);
}

console_result fetch_tx_index::invoke(std::ostream& output, std::ostream& error)
{
    // Bound parameters.
    const auto& encoding = get_format_option();
    const auto& hashes = get_hashs_argument();
    const auto retries = get_general_retries_setting();
    const auto timeout = get_general_wait_setting();
    const auto& server = get_server_address_setting();

    czmqpp::context context;
    obelisk_client client(context, period_ms(timeout), retries);

    if (client.connect(server) < 0)
        return console_result::failure;

    callback_state state(error, output, encoding);

    for (auto hash: hashes)
        fetch_tx_index_from_hash(client, state, hash);

    client.resolve_callbacks();

    return state.get_result();
}

<commit_msg>Deprecation comment.<commit_after>/**
 * Copyright (c) 2011-2014 libbitcoin developers (see AUTHORS)
 *
 * This file is part of libbitcoin-explorer.
 *
 * libbitcoin-explorer is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Affero General Public License with
 * additional permissions to the one published by the Free Software
 * Foundation, either version 3 of the License, or (at your option)
 * any later version. For more information see LICENSE.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */

#include <bitcoin/explorer/commands/fetch-tx-index.hpp>

#include <iostream>
#include <boost/format.hpp>
#include <bitcoin/bitcoin.hpp>
#include <bitcoin/explorer/callback_state.hpp>
#include <bitcoin/explorer/define.hpp>
#include <bitcoin/explorer/obelisk_client.hpp>
#include <bitcoin/explorer/primitives/base16.hpp>
#include <bitcoin/explorer/prop_tree.hpp>
#include <bitcoin/explorer/utility/utility.hpp>

using namespace bc;
using namespace bc::client;
using namespace bc::explorer;
using namespace bc::explorer::commands;
using namespace bc::explorer::primitives;

static void handle_error(callback_state& state, const std::error_code& error)
{
    state.handle_error(error);
}

static void handle_callback(callback_state& state, const hash_digest& hash,
    size_t height, size_t index)
{
    // native is info.
    state.output(prop_tree(hash, height, index));
}

static void fetch_tx_index_from_hash(obelisk_client& client,
    callback_state& state, const primitives::btc256& hash)
{
    // Do not pass the hash by reference here.
    auto on_done = [&state, hash](size_t block_height, size_t index)
    {
        handle_callback(state, hash, block_height, index);
    };

    auto on_error = [&state](const std::error_code& error)
    {
        handle_error(state, error);
    };

    client.get_codec()->fetch_transaction_index(on_error, on_done, hash);
}

// This call is deprecated at the server.
console_result fetch_tx_index::invoke(std::ostream& output, std::ostream& error)
{
    // Bound parameters.
    const auto& encoding = get_format_option();
    const auto& hashes = get_hashs_argument();
    const auto retries = get_general_retries_setting();
    const auto timeout = get_general_wait_setting();
    const auto& server = get_server_address_setting();

    czmqpp::context context;
    obelisk_client client(context, period_ms(timeout), retries);

    if (client.connect(server) < 0)
        return console_result::failure;

    callback_state state(error, output, encoding);

    for (auto hash: hashes)
        fetch_tx_index_from_hash(client, state, hash);

    client.resolve_callbacks();

    return state.get_result();
}

<|endoftext|>


<commit_before>/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
 *
 * This library is open source and may be redistributed and/or modified under
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * OpenSceneGraph Public License for more details.
*/
#include <stdlib.h>
#include <string.h>

#include <osg/CullSettings>
#include <osg/ArgumentParser>
#include <osg/ApplicationUsage>
#include <osg/os_utils>

#include <osg/Notify>

using namespace osg;

CullSettings::CullSettings(const CullSettings& cs)
{
    setCullSettings(cs);
}

void CullSettings::setDefaults()
{
    _inheritanceMask = ALL_VARIABLES;
    _inheritanceMaskActionOnAttributeSetting = DISABLE_ASSOCIATED_INHERITANCE_MASK_BIT;
    _cullingMode = DEFAULT_CULLING;
    _LODScale = 1.0f;
    _smallFeatureCullingPixelSize = 2.0f;

    _computeNearFar = COMPUTE_NEAR_FAR_USING_BOUNDING_VOLUMES;
    _nearFarRatio = 0.0005;
    _impostorActive = true;
    _depthSortImpostorSprites = false;
    _impostorPixelErrorThreshold = 4.0f;
    _numFramesToKeepImpostorSprites = 10;
    _cullMask = 0xffffffff;
    _cullMaskLeft = 0xffffffff;
    _cullMaskRight = 0xffffffff;

    // override during testing
    //_computeNearFar = COMPUTE_NEAR_FAR_USING_PRIMITIVES;
    //_nearFarRatio = 0.00005f;
}

void CullSettings::setCullSettings(const CullSettings& rhs)
{
    _inheritanceMask = rhs._inheritanceMask;
    _inheritanceMaskActionOnAttributeSetting = rhs._inheritanceMaskActionOnAttributeSetting;

    _computeNearFar = rhs._computeNearFar;
    _cullingMode = rhs._cullingMode;
    _LODScale = rhs._LODScale;
    _smallFeatureCullingPixelSize = rhs._smallFeatureCullingPixelSize;

    _clampProjectionMatrixCallback = rhs._clampProjectionMatrixCallback;
    _nearFarRatio = rhs._nearFarRatio;
    _impostorActive = rhs._impostorActive;
    _depthSortImpostorSprites = rhs._depthSortImpostorSprites;
    _impostorPixelErrorThreshold = rhs._impostorPixelErrorThreshold;
    _numFramesToKeepImpostorSprites = rhs._numFramesToKeepImpostorSprites;

    _cullMask = rhs._cullMask;
    _cullMaskLeft = rhs._cullMaskLeft;
    _cullMaskRight =  rhs._cullMaskRight;
}

void CullSettings::inheritCullSettings(const CullSettings& settings, unsigned int inheritanceMask)
{
    if (inheritanceMask & COMPUTE_NEAR_FAR_MODE) _computeNearFar = settings._computeNearFar;
    if (inheritanceMask & NEAR_FAR_RATIO) _nearFarRatio = settings._nearFarRatio;
    if (inheritanceMask & IMPOSTOR_ACTIVE) _impostorActive = settings._impostorActive;
    if (inheritanceMask & DEPTH_SORT_IMPOSTOR_SPRITES) _depthSortImpostorSprites = settings._depthSortImpostorSprites;
    if (inheritanceMask & IMPOSTOR_PIXEL_ERROR_THRESHOLD) _impostorPixelErrorThreshold = settings._impostorPixelErrorThreshold;
    if (inheritanceMask & NUM_FRAMES_TO_KEEP_IMPOSTORS_SPRITES) _numFramesToKeepImpostorSprites = settings._numFramesToKeepImpostorSprites;
    if (inheritanceMask & CULL_MASK) _cullMask = settings._cullMask;
    if (inheritanceMask & CULL_MASK_LEFT) _cullMaskLeft = settings._cullMaskLeft;
    if (inheritanceMask & CULL_MASK_RIGHT) _cullMaskRight = settings._cullMaskRight;
    if (inheritanceMask & CULLING_MODE) _cullingMode = settings._cullingMode;
    if (inheritanceMask & LOD_SCALE) _LODScale = settings._LODScale;
    if (inheritanceMask & SMALL_FEATURE_CULLING_PIXEL_SIZE) _smallFeatureCullingPixelSize = settings._smallFeatureCullingPixelSize;
    if (inheritanceMask & CLAMP_PROJECTION_MATRIX_CALLBACK) _clampProjectionMatrixCallback = settings._clampProjectionMatrixCallback;
}

static ApplicationUsageProxy ApplicationUsageProxyCullSettings_e0(ApplicationUsage::ENVIRONMENTAL_VARIABLE,"OSG_COMPUTE_NEAR_FAR_MODE <mode>","DO_NOT_COMPUTE_NEAR_FAR | COMPUTE_NEAR_FAR_USING_BOUNDING_VOLUMES | COMPUTE_NEAR_FAR_USING_PRIMITIVES");
static ApplicationUsageProxy ApplicationUsageProxyCullSettings_e1(ApplicationUsage::ENVIRONMENTAL_VARIABLE,"OSG_NEAR_FAR_RATIO <float>","Set the ratio between near and far planes - must greater than 0.0 but less than 1.0.");

void CullSettings::readEnvironmentalVariables()
{
    OSG_INFO<<"CullSettings::readEnvironmentalVariables()"<<std::endl;

    std::string value;
    if (getEnvVar("OSG_COMPUTE_NEAR_FAR_MODE", value))
    {
        if (value=="DO_NOT_COMPUTE_NEAR_FAR") _computeNearFar = DO_NOT_COMPUTE_NEAR_FAR;
        else if (value=="COMPUTE_NEAR_FAR_USING_BOUNDING_VOLUMES") _computeNearFar = COMPUTE_NEAR_FAR_USING_BOUNDING_VOLUMES;
        else if (value=="COMPUTE_NEAR_FAR_USING_PRIMITIVES") _computeNearFar = COMPUTE_NEAR_FAR_USING_PRIMITIVES;

        OSG_INFO<<"Set compute near far mode to "<<_computeNearFar<<std::endl;

    }

    if (getEnvVar("OSG_NEAR_FAR_RATIO", _nearFarRatio))
    {
        OSG_INFO<<"Set near/far ratio to "<<_nearFarRatio<<std::endl;
    }
}

void CullSettings::readCommandLine(ArgumentParser& arguments)
{
    // report the usage options.
    if (arguments.getApplicationUsage())
    {
        arguments.getApplicationUsage()->addCommandLineOption("--COMPUTE_NEAR_FAR_MODE <mode>","DO_NOT_COMPUTE_NEAR_FAR | COMPUTE_NEAR_FAR_USING_BOUNDING_VOLUMES | COMPUTE_NEAR_FAR_USING_PRIMITIVES");
        arguments.getApplicationUsage()->addCommandLineOption("--NEAR_FAR_RATIO <float>","Set the ratio between near and far planes - must greater than 0.0 but less than 1.0.");
    }

    std::string str;
    while(arguments.read("--COMPUTE_NEAR_FAR_MODE",str))
    {
        if (str=="DO_NOT_COMPUTE_NEAR_FAR") _computeNearFar = DO_NOT_COMPUTE_NEAR_FAR;
        else if (str=="COMPUTE_NEAR_FAR_USING_BOUNDING_VOLUMES") _computeNearFar = COMPUTE_NEAR_FAR_USING_BOUNDING_VOLUMES;
        else if (str=="COMPUTE_NEAR_FAR_USING_PRIMITIVES") _computeNearFar = COMPUTE_NEAR_FAR_USING_PRIMITIVES;

        OSG_INFO<<"Set compute near far mode to "<<_computeNearFar<<std::endl;
    }

    double value;
    while(arguments.read("--NEAR_FAR_RATIO",value))
    {
        _nearFarRatio = value;

        OSG_INFO<<"Set near/far ratio to "<<_nearFarRatio<<std::endl;
    }

}

void CullSettings::write(std::ostream& out)
{
    out<<"CullSettings: "<<this<<" {"<<std::endl;

    out<<"    _inheritanceMask = "<<_inheritanceMask<<std::endl;
    out<<"    _inheritanceMaskActionOnAttributeSetting = "<<_inheritanceMaskActionOnAttributeSetting<<std::endl;
    out<<"    _computeNearFar = "<<_computeNearFar<<std::endl;
    out<<"    _cullingMode = "<<_cullingMode<<std::endl;
    out<<"    _LODScale = "<<_LODScale<<std::endl;
    out<<"    _smallFeatureCullingPixelSize = "<<_smallFeatureCullingPixelSize<<std::endl;
    out<<"    _clampProjectionMatrixCallback = "<<_clampProjectionMatrixCallback.get()<<std::endl;
    out<<"    _nearFarRatio = "<<_nearFarRatio<<std::endl;
    out<<"    _impostorActive = "<<_impostorActive<<std::endl;
    out<<"    _depthSortImpostorSprites = "<<_depthSortImpostorSprites<<std::endl;
    out<<"    _impostorPixelErrorThreshold = "<<_impostorPixelErrorThreshold<<std::endl;
    out<<"    _numFramesToKeepImpostorSprites = "<<_numFramesToKeepImpostorSprites<<std::endl;
    out<<"    _cullMask = "<<_cullMask<<std::endl;
    out<<"    _cullMaskLeft = "<<_cullMaskLeft<<std::endl;
    out<<"    _cullMaskRight = "<<_cullMaskRight<<std::endl;

    out<<"{"<<std::endl;
}

<commit_msg>Added --NO_CULLING, --VIEW_FRSUTUM and --VIEW_FRUSTUM_SIDES/--vfs command line options for setting the Camera CullingMode value<commit_after>/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
 *
 * This library is open source and may be redistributed and/or modified under
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * OpenSceneGraph Public License for more details.
*/
#include <stdlib.h>
#include <string.h>

#include <osg/CullSettings>
#include <osg/ArgumentParser>
#include <osg/ApplicationUsage>
#include <osg/os_utils>

#include <osg/Notify>

using namespace osg;

CullSettings::CullSettings(const CullSettings& cs)
{
    setCullSettings(cs);
}

void CullSettings::setDefaults()
{
    _inheritanceMask = ALL_VARIABLES;
    _inheritanceMaskActionOnAttributeSetting = DISABLE_ASSOCIATED_INHERITANCE_MASK_BIT;
    _cullingMode = DEFAULT_CULLING;
    _LODScale = 1.0f;
    _smallFeatureCullingPixelSize = 2.0f;

    _computeNearFar = COMPUTE_NEAR_FAR_USING_BOUNDING_VOLUMES;
    _nearFarRatio = 0.0005;
    _impostorActive = true;
    _depthSortImpostorSprites = false;
    _impostorPixelErrorThreshold = 4.0f;
    _numFramesToKeepImpostorSprites = 10;
    _cullMask = 0xffffffff;
    _cullMaskLeft = 0xffffffff;
    _cullMaskRight = 0xffffffff;

    // override during testing
    //_computeNearFar = COMPUTE_NEAR_FAR_USING_PRIMITIVES;
    //_nearFarRatio = 0.00005f;
}

void CullSettings::setCullSettings(const CullSettings& rhs)
{
    _inheritanceMask = rhs._inheritanceMask;
    _inheritanceMaskActionOnAttributeSetting = rhs._inheritanceMaskActionOnAttributeSetting;

    _computeNearFar = rhs._computeNearFar;
    _cullingMode = rhs._cullingMode;
    _LODScale = rhs._LODScale;
    _smallFeatureCullingPixelSize = rhs._smallFeatureCullingPixelSize;

    _clampProjectionMatrixCallback = rhs._clampProjectionMatrixCallback;
    _nearFarRatio = rhs._nearFarRatio;
    _impostorActive = rhs._impostorActive;
    _depthSortImpostorSprites = rhs._depthSortImpostorSprites;
    _impostorPixelErrorThreshold = rhs._impostorPixelErrorThreshold;
    _numFramesToKeepImpostorSprites = rhs._numFramesToKeepImpostorSprites;

    _cullMask = rhs._cullMask;
    _cullMaskLeft = rhs._cullMaskLeft;
    _cullMaskRight =  rhs._cullMaskRight;
}

void CullSettings::inheritCullSettings(const CullSettings& settings, unsigned int inheritanceMask)
{
    if (inheritanceMask & COMPUTE_NEAR_FAR_MODE) _computeNearFar = settings._computeNearFar;
    if (inheritanceMask & NEAR_FAR_RATIO) _nearFarRatio = settings._nearFarRatio;
    if (inheritanceMask & IMPOSTOR_ACTIVE) _impostorActive = settings._impostorActive;
    if (inheritanceMask & DEPTH_SORT_IMPOSTOR_SPRITES) _depthSortImpostorSprites = settings._depthSortImpostorSprites;
    if (inheritanceMask & IMPOSTOR_PIXEL_ERROR_THRESHOLD) _impostorPixelErrorThreshold = settings._impostorPixelErrorThreshold;
    if (inheritanceMask & NUM_FRAMES_TO_KEEP_IMPOSTORS_SPRITES) _numFramesToKeepImpostorSprites = settings._numFramesToKeepImpostorSprites;
    if (inheritanceMask & CULL_MASK) _cullMask = settings._cullMask;
    if (inheritanceMask & CULL_MASK_LEFT) _cullMaskLeft = settings._cullMaskLeft;
    if (inheritanceMask & CULL_MASK_RIGHT) _cullMaskRight = settings._cullMaskRight;
    if (inheritanceMask & CULLING_MODE) _cullingMode = settings._cullingMode;
    if (inheritanceMask & LOD_SCALE) _LODScale = settings._LODScale;
    if (inheritanceMask & SMALL_FEATURE_CULLING_PIXEL_SIZE) _smallFeatureCullingPixelSize = settings._smallFeatureCullingPixelSize;
    if (inheritanceMask & CLAMP_PROJECTION_MATRIX_CALLBACK) _clampProjectionMatrixCallback = settings._clampProjectionMatrixCallback;
}

static ApplicationUsageProxy ApplicationUsageProxyCullSettings_e0(ApplicationUsage::ENVIRONMENTAL_VARIABLE,"OSG_COMPUTE_NEAR_FAR_MODE <mode>","DO_NOT_COMPUTE_NEAR_FAR | COMPUTE_NEAR_FAR_USING_BOUNDING_VOLUMES | COMPUTE_NEAR_FAR_USING_PRIMITIVES");
static ApplicationUsageProxy ApplicationUsageProxyCullSettings_e1(ApplicationUsage::ENVIRONMENTAL_VARIABLE,"OSG_NEAR_FAR_RATIO <float>","Set the ratio between near and far planes - must greater than 0.0 but less than 1.0.");

void CullSettings::readEnvironmentalVariables()
{
    OSG_INFO<<"CullSettings::readEnvironmentalVariables()"<<std::endl;

    std::string value;
    if (getEnvVar("OSG_COMPUTE_NEAR_FAR_MODE", value))
    {
        if (value=="DO_NOT_COMPUTE_NEAR_FAR") _computeNearFar = DO_NOT_COMPUTE_NEAR_FAR;
        else if (value=="COMPUTE_NEAR_FAR_USING_BOUNDING_VOLUMES") _computeNearFar = COMPUTE_NEAR_FAR_USING_BOUNDING_VOLUMES;
        else if (value=="COMPUTE_NEAR_FAR_USING_PRIMITIVES") _computeNearFar = COMPUTE_NEAR_FAR_USING_PRIMITIVES;

        OSG_INFO<<"Set compute near far mode to "<<_computeNearFar<<std::endl;

    }

    if (getEnvVar("OSG_NEAR_FAR_RATIO", _nearFarRatio))
    {
        OSG_INFO<<"Set near/far ratio to "<<_nearFarRatio<<std::endl;
    }
}

void CullSettings::readCommandLine(ArgumentParser& arguments)
{
    OSG_INFO<<"CullSettings::readCommandLine(ArgumentParser& arguments)"<<std::endl;

    // report the usage options.
    if (arguments.getApplicationUsage())
    {
        arguments.getApplicationUsage()->addCommandLineOption("--COMPUTE_NEAR_FAR_MODE <mode>","DO_NOT_COMPUTE_NEAR_FAR | COMPUTE_NEAR_FAR_USING_BOUNDING_VOLUMES | COMPUTE_NEAR_FAR_USING_PRIMITIVES");
        arguments.getApplicationUsage()->addCommandLineOption("--NEAR_FAR_RATIO <float>","Set the ratio between near and far planes - must greater than 0.0 but less than 1.0.");
    }

    while(arguments.read("--NO_CULLING")) setCullingMode(NO_CULLING);
    while(arguments.read("--VIEW_FRUSTUM")) setCullingMode(VIEW_FRUSTUM_CULLING);
    while(arguments.read("--VIEW_FRUSTUM_SIDES") || arguments.read("--vfs") ) setCullingMode(VIEW_FRUSTUM_SIDES_CULLING);

    std::string str;
    while(arguments.read("--COMPUTE_NEAR_FAR_MODE",str))
    {
        if (str=="DO_NOT_COMPUTE_NEAR_FAR") _computeNearFar = DO_NOT_COMPUTE_NEAR_FAR;
        else if (str=="COMPUTE_NEAR_FAR_USING_BOUNDING_VOLUMES") _computeNearFar = COMPUTE_NEAR_FAR_USING_BOUNDING_VOLUMES;
        else if (str=="COMPUTE_NEAR_FAR_USING_PRIMITIVES") _computeNearFar = COMPUTE_NEAR_FAR_USING_PRIMITIVES;

        OSG_INFO<<"Set compute near far mode to "<<_computeNearFar<<std::endl;
    }

    double value;
    while(arguments.read("--NEAR_FAR_RATIO",value))
    {
        _nearFarRatio = value;

        OSG_INFO<<"Set near/far ratio to "<<_nearFarRatio<<std::endl;
    }

}

void CullSettings::write(std::ostream& out)
{
    out<<"CullSettings: "<<this<<" {"<<std::endl;

    out<<"    _inheritanceMask = "<<_inheritanceMask<<std::endl;
    out<<"    _inheritanceMaskActionOnAttributeSetting = "<<_inheritanceMaskActionOnAttributeSetting<<std::endl;
    out<<"    _computeNearFar = "<<_computeNearFar<<std::endl;
    out<<"    _cullingMode = "<<_cullingMode<<std::endl;
    out<<"    _LODScale = "<<_LODScale<<std::endl;
    out<<"    _smallFeatureCullingPixelSize = "<<_smallFeatureCullingPixelSize<<std::endl;
    out<<"    _clampProjectionMatrixCallback = "<<_clampProjectionMatrixCallback.get()<<std::endl;
    out<<"    _nearFarRatio = "<<_nearFarRatio<<std::endl;
    out<<"    _impostorActive = "<<_impostorActive<<std::endl;
    out<<"    _depthSortImpostorSprites = "<<_depthSortImpostorSprites<<std::endl;
    out<<"    _impostorPixelErrorThreshold = "<<_impostorPixelErrorThreshold<<std::endl;
    out<<"    _numFramesToKeepImpostorSprites = "<<_numFramesToKeepImpostorSprites<<std::endl;
    out<<"    _cullMask = "<<_cullMask<<std::endl;
    out<<"    _cullMaskLeft = "<<_cullMaskLeft<<std::endl;
    out<<"    _cullMaskRight = "<<_cullMaskRight<<std::endl;

    out<<"{"<<std::endl;
}

<|endoftext|>