<commit_before>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Willow Garage, Inc.
 *  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 Willow Garage 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.
 *********************************************************************/

/* Author: Ioan Sucan, Dave Coleman */

#include <moveit/ompl_interface/ompl_interface.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/profiler/profiler.h>
#include <class_loader/class_loader.h>

#include <dynamic_reconfigure/server.h>
#include "moveit_planners_ompl/OMPLDynamicReconfigureConfig.h"

#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>

namespace ompl_interface
{
using namespace moveit_planners_ompl;

class OMPLPlannerManager : public planning_interface::PlannerManager
{
public:

  OMPLPlannerManager() : planning_interface::PlannerManager(),
                         nh_("~"),
                         display_random_valid_states_(false)
  {
  }

  virtual bool initialize(const robot_model::RobotModelConstPtr& model, const std::string &ns)
  {
    if (!ns.empty())
      nh_ = ros::NodeHandle(ns);
    ompl_interface_.reset(new OMPLInterface(model, nh_));
    std::string ompl_ns = ns.empty() ? "ompl" : ns + "/ompl";
    dynamic_reconfigure_server_.reset(new dynamic_reconfigure::Server<OMPLDynamicReconfigureConfig>(ros::NodeHandle(nh_, ompl_ns)));
    dynamic_reconfigure_server_->setCallback(boost::bind(&OMPLPlannerManager::dynamicReconfigureCallback, this, _1, _2));
    return true;
  }

  virtual bool canServiceRequest(const moveit_msgs::MotionPlanRequest &req) const
  {
    return req.trajectory_constraints.constraints.empty();
  }

  virtual std::string getDescription() const
  {
    return "OMPL";
  }

  virtual void getPlanningAlgorithms(std::vector<std::string> &algs) const
  {
    const planning_interface::PlannerConfigurationMap &pconfig = ompl_interface_->getPlannerConfigurations();
    algs.clear();
    algs.reserve(pconfig.size());
    for (planning_interface::PlannerConfigurationMap::const_iterator it = pconfig.begin() ; it != pconfig.end() ; ++it)
      algs.push_back(it->first);
  }

  virtual void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap &pconfig)
  {
    // this call can add a few more configs than we pass in (adds defaults)
    ompl_interface_->setPlannerConfigurations(pconfig);
    // so we read the configs instead of just setting pconfig
    PlannerManager::setPlannerConfigurations(ompl_interface_->getPlannerConfigurations());
  }

  virtual planning_interface::PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                                    const planning_interface::MotionPlanRequest &req,
                                                                    moveit_msgs::MoveItErrorCodes &error_code) const
  {
    return ompl_interface_->getPlanningContext(planning_scene, req, error_code);
  }

private:

    /*
    bool r = ompl_interface_->solve(planning_scene, req, res);
    if (!planner_data_link_name_.empty())
      displayPlannerData(planning_scene, planner_data_link_name_);
    return r;
    */

    /*
    bool r = ompl_interface_->solve(planning_scene, req, res);
    if (!planner_data_link_name_.empty())
      displayPlannerData(planning_scene, planner_data_link_name_);
    return r;
    */

  /*
  void displayRandomValidStates()
  {
    ompl_interface::ModelBasedPlanningContextPtr pc = ompl_interface_->getLastPlanningContext();
    if (!pc || !pc->getPlanningScene())
    {
      ROS_ERROR("No planning context to sample states for");
      return;
    }
    ROS_INFO_STREAM("Displaying states for context " << pc->getName());
    const og::SimpleSetup &ss = pc->getOMPLSimpleSetup();
    ob::ValidStateSamplerPtr vss = ss.getSpaceInformation()->allocValidStateSampler();
    robot_state::RobotState kstate = pc->getPlanningScene()->getCurrentState();
    ob::ScopedState<> rstate1(ss.getStateSpace());
    ob::ScopedState<> rstate2(ss.getStateSpace());
    ros::WallDuration wait(2);
    unsigned int n = 0;
    std::vector<ob::State*> sts;
    if (vss->sample(rstate2.get()))
      while (display_random_valid_states_)
      {
        if (!vss->sampleNear(rstate1.get(), rstate2.get(), 10000000))
          continue;
        pc->getOMPLStateSpace()->copyToRobotState(kstate, rstate1.get());
        kstate.getJointStateGroup(pc->getJointModelGroupName())->updateLinkTransforms();
        moveit_msgs::DisplayRobotState state_msg;
        robot_state::robotStateToRobotStateMsg(kstate, state_msg.state);
        pub_valid_states_.publish(state_msg);
        n = (n + 1) % 2;
        if (n == 0)
        {
          robot_trajectory::RobotTrajectory traj(pc->getRobotModel(), pc->getJointModelGroupName());
          unsigned int g = ss.getSpaceInformation()->getMotionStates(rstate1.get(), rstate2.get(), sts, 10, true, true);
          ROS_INFO("Generated a motion with %u states", g);
          for (std::size_t i = 0 ; i < g ; ++i)
          {
            pc->getOMPLStateSpace()->copyToRobotState(kstate, sts[i]);
            traj.addSuffixWayPoint(kstate, 0.0);
          }
          moveit_msgs::DisplayTrajectory msg;
          msg.model_id = pc->getRobotModel()->getName();
          msg.trajectory.resize(1);
          traj.getRobotTrajectoryMsg(msg.trajectory[0]);
          robot_state::robotStateToRobotStateMsg(traj.getFirstWayPoint(), msg.trajectory_start);
          pub_valid_traj_.publish(msg);
        }
        rstate2 = rstate1;
        wait.sleep();
      }
  }

  void displayPlannerData(const planning_scene::PlanningSceneConstPtr& planning_scene,
                          const std::string &link_name) const
  {
    ompl_interface::ModelBasedPlanningContextPtr pc = ompl_interface_->getLastPlanningContext();
    if (pc)
    {
      ompl::base::PlannerData pd(pc->getOMPLSimpleSetup()->getSpaceInformation());
      pc->getOMPLSimpleSetup()->getPlannerData(pd);
      robot_state::RobotState kstate = planning_scene->getCurrentState();
      visualization_msgs::MarkerArray arr;
      std_msgs::ColorRGBA color;
      color.r = 1.0f;
      color.g = 0.25f;
      color.b = 1.0f;
      color.a = 1.0f;
      unsigned int nv = pd.numVertices();
      for (unsigned int i = 0 ; i < nv ; ++i)
      {
        pc->getOMPLStateSpace()->copyToRobotState(kstate, pd.getVertex(i).getState());
        kstate.getJointStateGroup(pc->getJointModelGroupName())->updateLinkTransforms();
        const Eigen::Vector3d &pos = kstate.getLinkState(link_name)->getGlobalLinkTransform().translation();

        visualization_msgs::Marker mk;
        mk.header.stamp = ros::Time::now();
        mk.header.frame_id = planning_scene->getPlanningFrame();
        mk.ns = "planner_data";
        mk.id = i;
        mk.type = visualization_msgs::Marker::SPHERE;
        mk.action = visualization_msgs::Marker::ADD;
        mk.pose.position.x = pos.x();
        mk.pose.position.y = pos.y();
        mk.pose.position.z = pos.z();
        mk.pose.orientation.w = 1.0;
        mk.scale.x = mk.scale.y = mk.scale.z = 0.025;
        mk.color = color;
        mk.lifetime = ros::Duration(30.0);
        arr.markers.push_back(mk);
      }
      pub_markers_.publish(arr);
    }
  }
  */

  void dynamicReconfigureCallback(OMPLDynamicReconfigureConfig &config, uint32_t level)
  {
    if (config.link_for_exploration_tree.empty() && !planner_data_link_name_.empty())
    {
      pub_markers_.shutdown();
      planner_data_link_name_.clear();
      ROS_INFO("Not displaying OMPL exploration data structures.");
    }
    else
      if (!config.link_for_exploration_tree.empty() && planner_data_link_name_.empty())
      {
        pub_markers_ = nh_.advertise<visualization_msgs::MarkerArray>("ompl_planner_data_marker_array", 5);
        planner_data_link_name_ = config.link_for_exploration_tree;
        ROS_INFO("Displaying OMPL exploration data structures for %s", planner_data_link_name_.c_str());
      }

    ompl_interface_->simplifySolutions(config.simplify_solutions);
    ompl_interface_->getPlanningContextManager().setMaximumSolutionSegmentLength(config.maximum_waypoint_distance);
    ompl_interface_->getPlanningContextManager().setMinimumWaypointCount(config.minimum_waypoint_count);
    if (display_random_valid_states_ && !config.display_random_valid_states)
    {
      display_random_valid_states_ = false;
      if (pub_valid_states_thread_)
      {
        pub_valid_states_thread_->join();
        pub_valid_states_thread_.reset();
      }
      pub_valid_states_.shutdown();
      pub_valid_traj_.shutdown();
    }
    else
      if (!display_random_valid_states_ && config.display_random_valid_states)
      {
        pub_valid_states_ = nh_.advertise<moveit_msgs::DisplayRobotState>("ompl_planner_valid_states", 5);
        pub_valid_traj_ = nh_.advertise<moveit_msgs::DisplayTrajectory>("ompl_planner_valid_trajectories", 5);
    display_random_valid_states_ = true;
        //    pub_valid_states_thread_.reset(new boost::thread(boost::bind(&OMPLPlannerManager::displayRandomValidStates, this)));
      }
  }

  ros::NodeHandle nh_;
  boost::scoped_ptr<dynamic_reconfigure::Server<OMPLDynamicReconfigureConfig> > dynamic_reconfigure_server_;
  boost::scoped_ptr<OMPLInterface> ompl_interface_;
  boost::scoped_ptr<boost::thread> pub_valid_states_thread_;
  bool display_random_valid_states_;
  ros::Publisher pub_markers_;
  ros::Publisher pub_valid_states_;
  ros::Publisher pub_valid_traj_;
  std::string planner_data_link_name_;
};

} // ompl_interface

CLASS_LOADER_REGISTER_CLASS(ompl_interface::OMPLPlannerManager, planning_interface::PlannerManager);
<commit_msg>fixing internal storing of config settings<commit_after>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Willow Garage, Inc.
 *  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 Willow Garage 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.
 *********************************************************************/

/* Author: Ioan Sucan, Dave Coleman */

#include <moveit/ompl_interface/ompl_interface.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/profiler/profiler.h>
#include <class_loader/class_loader.h>

#include <dynamic_reconfigure/server.h>
#include "moveit_planners_ompl/OMPLDynamicReconfigureConfig.h"

#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>

namespace ompl_interface
{
using namespace moveit_planners_ompl;

class OMPLPlannerManager : public planning_interface::PlannerManager
{
public:

  OMPLPlannerManager() : planning_interface::PlannerManager(),
                         nh_("~"),
                         display_random_valid_states_(false)
  {
  }

  virtual bool initialize(const robot_model::RobotModelConstPtr& model, const std::string &ns)
  {
    if (!ns.empty())
      nh_ = ros::NodeHandle(ns);
    ompl_interface_.reset(new OMPLInterface(model, nh_));
    std::string ompl_ns = ns.empty() ? "ompl" : ns + "/ompl";
    dynamic_reconfigure_server_.reset(new dynamic_reconfigure::Server<OMPLDynamicReconfigureConfig>(ros::NodeHandle(nh_, ompl_ns)));
    dynamic_reconfigure_server_->setCallback(boost::bind(&OMPLPlannerManager::dynamicReconfigureCallback, this, _1, _2));
    config_settings_ = ompl_interface_->getPlannerConfigurations();
    return true;
  }

  virtual bool canServiceRequest(const moveit_msgs::MotionPlanRequest &req) const
  {
    return req.trajectory_constraints.constraints.empty();
  }

  virtual std::string getDescription() const
  {
    return "OMPL";
  }

  virtual void getPlanningAlgorithms(std::vector<std::string> &algs) const
  {
    const planning_interface::PlannerConfigurationMap &pconfig = ompl_interface_->getPlannerConfigurations();
    algs.clear();
    algs.reserve(pconfig.size());
    for (planning_interface::PlannerConfigurationMap::const_iterator it = pconfig.begin() ; it != pconfig.end() ; ++it)
      algs.push_back(it->first);
  }

  virtual void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap &pconfig)
  {
    // this call can add a few more configs than we pass in (adds defaults)
    ompl_interface_->setPlannerConfigurations(pconfig);
    // so we read the configs instead of just setting pconfig
    PlannerManager::setPlannerConfigurations(ompl_interface_->getPlannerConfigurations());
  }

  virtual planning_interface::PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                                    const planning_interface::MotionPlanRequest &req,
                                                                    moveit_msgs::MoveItErrorCodes &error_code) const
  {
    return ompl_interface_->getPlanningContext(planning_scene, req, error_code);
  }

private:

    /*
    bool r = ompl_interface_->solve(planning_scene, req, res);
    if (!planner_data_link_name_.empty())
      displayPlannerData(planning_scene, planner_data_link_name_);
    return r;
    */

    /*
    bool r = ompl_interface_->solve(planning_scene, req, res);
    if (!planner_data_link_name_.empty())
      displayPlannerData(planning_scene, planner_data_link_name_);
    return r;
    */

  /*
  void displayRandomValidStates()
  {
    ompl_interface::ModelBasedPlanningContextPtr pc = ompl_interface_->getLastPlanningContext();
    if (!pc || !pc->getPlanningScene())
    {
      ROS_ERROR("No planning context to sample states for");
      return;
    }
    ROS_INFO_STREAM("Displaying states for context " << pc->getName());
    const og::SimpleSetup &ss = pc->getOMPLSimpleSetup();
    ob::ValidStateSamplerPtr vss = ss.getSpaceInformation()->allocValidStateSampler();
    robot_state::RobotState kstate = pc->getPlanningScene()->getCurrentState();
    ob::ScopedState<> rstate1(ss.getStateSpace());
    ob::ScopedState<> rstate2(ss.getStateSpace());
    ros::WallDuration wait(2);
    unsigned int n = 0;
    std::vector<ob::State*> sts;
    if (vss->sample(rstate2.get()))
      while (display_random_valid_states_)
      {
        if (!vss->sampleNear(rstate1.get(), rstate2.get(), 10000000))
          continue;
        pc->getOMPLStateSpace()->copyToRobotState(kstate, rstate1.get());
        kstate.getJointStateGroup(pc->getJointModelGroupName())->updateLinkTransforms();
        moveit_msgs::DisplayRobotState state_msg;
        robot_state::robotStateToRobotStateMsg(kstate, state_msg.state);
        pub_valid_states_.publish(state_msg);
        n = (n + 1) % 2;
        if (n == 0)
        {
          robot_trajectory::RobotTrajectory traj(pc->getRobotModel(), pc->getJointModelGroupName());
          unsigned int g = ss.getSpaceInformation()->getMotionStates(rstate1.get(), rstate2.get(), sts, 10, true, true);
          ROS_INFO("Generated a motion with %u states", g);
          for (std::size_t i = 0 ; i < g ; ++i)
          {
            pc->getOMPLStateSpace()->copyToRobotState(kstate, sts[i]);
            traj.addSuffixWayPoint(kstate, 0.0);
          }
          moveit_msgs::DisplayTrajectory msg;
          msg.model_id = pc->getRobotModel()->getName();
          msg.trajectory.resize(1);
          traj.getRobotTrajectoryMsg(msg.trajectory[0]);
          robot_state::robotStateToRobotStateMsg(traj.getFirstWayPoint(), msg.trajectory_start);
          pub_valid_traj_.publish(msg);
        }
        rstate2 = rstate1;
        wait.sleep();
      }
  }

  void displayPlannerData(const planning_scene::PlanningSceneConstPtr& planning_scene,
                          const std::string &link_name) const
  {
    ompl_interface::ModelBasedPlanningContextPtr pc = ompl_interface_->getLastPlanningContext();
    if (pc)
    {
      ompl::base::PlannerData pd(pc->getOMPLSimpleSetup()->getSpaceInformation());
      pc->getOMPLSimpleSetup()->getPlannerData(pd);
      robot_state::RobotState kstate = planning_scene->getCurrentState();
      visualization_msgs::MarkerArray arr;
      std_msgs::ColorRGBA color;
      color.r = 1.0f;
      color.g = 0.25f;
      color.b = 1.0f;
      color.a = 1.0f;
      unsigned int nv = pd.numVertices();
      for (unsigned int i = 0 ; i < nv ; ++i)
      {
        pc->getOMPLStateSpace()->copyToRobotState(kstate, pd.getVertex(i).getState());
        kstate.getJointStateGroup(pc->getJointModelGroupName())->updateLinkTransforms();
        const Eigen::Vector3d &pos = kstate.getLinkState(link_name)->getGlobalLinkTransform().translation();

        visualization_msgs::Marker mk;
        mk.header.stamp = ros::Time::now();
        mk.header.frame_id = planning_scene->getPlanningFrame();
        mk.ns = "planner_data";
        mk.id = i;
        mk.type = visualization_msgs::Marker::SPHERE;
        mk.action = visualization_msgs::Marker::ADD;
        mk.pose.position.x = pos.x();
        mk.pose.position.y = pos.y();
        mk.pose.position.z = pos.z();
        mk.pose.orientation.w = 1.0;
        mk.scale.x = mk.scale.y = mk.scale.z = 0.025;
        mk.color = color;
        mk.lifetime = ros::Duration(30.0);
        arr.markers.push_back(mk);
      }
      pub_markers_.publish(arr);
    }
  }
  */

  void dynamicReconfigureCallback(OMPLDynamicReconfigureConfig &config, uint32_t level)
  {
    if (config.link_for_exploration_tree.empty() && !planner_data_link_name_.empty())
    {
      pub_markers_.shutdown();
      planner_data_link_name_.clear();
      ROS_INFO("Not displaying OMPL exploration data structures.");
    }
    else
      if (!config.link_for_exploration_tree.empty() && planner_data_link_name_.empty())
      {
        pub_markers_ = nh_.advertise<visualization_msgs::MarkerArray>("ompl_planner_data_marker_array", 5);
        planner_data_link_name_ = config.link_for_exploration_tree;
        ROS_INFO("Displaying OMPL exploration data structures for %s", planner_data_link_name_.c_str());
      }

    ompl_interface_->simplifySolutions(config.simplify_solutions);
    ompl_interface_->getPlanningContextManager().setMaximumSolutionSegmentLength(config.maximum_waypoint_distance);
    ompl_interface_->getPlanningContextManager().setMinimumWaypointCount(config.minimum_waypoint_count);
    if (display_random_valid_states_ && !config.display_random_valid_states)
    {
      display_random_valid_states_ = false;
      if (pub_valid_states_thread_)
      {
        pub_valid_states_thread_->join();
        pub_valid_states_thread_.reset();
      }
      pub_valid_states_.shutdown();
      pub_valid_traj_.shutdown();
    }
    else
      if (!display_random_valid_states_ && config.display_random_valid_states)
      {
        pub_valid_states_ = nh_.advertise<moveit_msgs::DisplayRobotState>("ompl_planner_valid_states", 5);
        pub_valid_traj_ = nh_.advertise<moveit_msgs::DisplayTrajectory>("ompl_planner_valid_trajectories", 5);
    display_random_valid_states_ = true;
        //    pub_valid_states_thread_.reset(new boost::thread(boost::bind(&OMPLPlannerManager::displayRandomValidStates, this)));
      }
  }

  ros::NodeHandle nh_;
  boost::scoped_ptr<dynamic_reconfigure::Server<OMPLDynamicReconfigureConfig> > dynamic_reconfigure_server_;
  boost::scoped_ptr<OMPLInterface> ompl_interface_;
  boost::scoped_ptr<boost::thread> pub_valid_states_thread_;
  bool display_random_valid_states_;
  ros::Publisher pub_markers_;
  ros::Publisher pub_valid_states_;
  ros::Publisher pub_valid_traj_;
  std::string planner_data_link_name_;
};

} // ompl_interface

CLASS_LOADER_REGISTER_CLASS(ompl_interface::OMPLPlannerManager, planning_interface::PlannerManager);
<|endoftext|>


<commit_before>#include <ros/ros.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <fstream>
#include <igvc/StringUtils.hpp>
#include <gps_common/conversions.h>
#include <string>
#include <mutex>

using namespace std;
using namespace geometry_msgs;
using namespace ros;

ros::Publisher waypoint_pub;
vector<PointStamped> waypoints;
PointStamped current_waypoint;
Point map_origin;

mutex current_mutex;

void loadWaypointsFile(string path, vector<PointStamped>& waypoints)
{
    if(path.empty())
    {
        ROS_ERROR_STREAM("Could not load empty file path.");
        return;
    }

    ifstream file;
    file.open(path.c_str());

    if(!file.is_open())
    {
        ROS_INFO_STREAM("Could not open file: " << path);
        return;
    }

    string line = "";
    auto lineIndex = 1;
    while(!file.eof())
    {
        getline(file, line);

        if(!line.empty())
        {
            vector<string> tokens = split(line, ',');

            if(tokens.size() != 2)
            {
                ROS_ERROR_STREAM(path << ":" << lineIndex << " - " << tokens.size() << " tokens instead of 2.");
                return;
            }

            auto lat = stof(tokens[0]);
            auto lon = stof(tokens[1]);

            PointStamped p;

            gps_common::UTM(lat, lon, &(p.point.x), &(p.point.y));

            waypoints.push_back(p);
        }

        lineIndex++;
    }
}

double distanceBetweenPoints(const Point &p1, const Point &p2)
{
    return sqrt((p2.x-p1.x)*(p2.x-p1.x) + (p2.y-p1.y)*(p2.y-p1.y));
}

void positionCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
{
    lock_guard<mutex> lock(current_mutex);
    if(distanceBetweenPoints(msg->pose.pose.position, current_waypoint.point) < 1.0)
    {
        // advance to next waypoint.
        current_waypoint = waypoints.front();
        current_waypoint.point.x -= map_origin.x;
        current_waypoint.point.y -= map_origin.y;
        if(waypoints.size() > 1)
            waypoints.erase(waypoints.begin());
    }
}

void originCallback(const geometry_msgs::PointStampedConstPtr& msg)
{
    lock_guard<mutex> lock(current_mutex);
    map_origin = msg->point;
}

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

    ros::NodeHandle nh;
    ros::NodeHandle nhp("~");

    ROS_INFO_STREAM("Has param: " << nhp.hasParam("file"));

    string path;
    nhp.getParam("file", path);

    ROS_INFO_STREAM("Loading waypoints from " << path);

    waypoint_pub = nh.advertise<PointStamped>("/waypoint", 1);

    ros::Subscriber odom_sub = nh.subscribe("/robot_pose_ekf/odom_combined", 1, positionCallback);

    ros::Subscriber origin_sub = nh.subscribe("/map_origin", 1, originCallback);

    loadWaypointsFile(path, waypoints);

    if(!waypoints.empty())
    {
        ROS_INFO_STREAM(waypoints.size() << " waypoints found.");
        current_waypoint = waypoints.front();
        Rate rate(1); // 1 Hz
        while(ros::ok())
        {
            ROS_INFO("publishing current waypoint...");
            lock_guard<mutex> lock(current_mutex);
            current_waypoint.header.stamp = ros::Time::now();
            current_waypoint.header.seq++;
            current_waypoint.header.frame_id = "map";
            waypoint_pub.publish(current_waypoint);

            ros::spinOnce();
            rate.sleep();
        }
    } else {
        ROS_ERROR("No valid waypoint entries found.");
    }

    return 0;
}
<commit_msg>Adds support for DMS notation in waypoint files.<commit_after>#include <ros/ros.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <fstream>
#include <igvc/StringUtils.hpp>
#include <gps_common/conversions.h>
#include <string>
#include <mutex>

using namespace std;
using namespace geometry_msgs;
using namespace ros;

ros::Publisher waypoint_pub;
vector<PointStamped> waypoints;
PointStamped current_waypoint;
Point map_origin;

mutex current_mutex;

double dmsToDec(string dms)
{
    auto qMarkIter = dms.find('?');
    auto aposIter = dms.find('\'');
    auto qouteIter = dms.find('\"');
    auto degrees = stod(dms.substr(0, qMarkIter));
    auto minutes = stod(dms.substr(qmarkIter+1, aposIter));
    auto seconds = stod(dms.substr(aposIter+1, qouteIter));
    auto dirChar = dms[dms.size() - 1];

    degrees += minutes * 60.0;
    degrees += seconds * 3600.0;

    if(dirChar == 'W' || dirChar == 'S')
        degrees *= -1;

    return degrees;
}

void loadWaypointsFile(string path, vector<PointStamped>& waypoints)
{
    if(path.empty())
    {
        ROS_ERROR_STREAM("Could not load empty file path.");
        return;
    }

    ifstream file;
    file.open(path.c_str());

    if(!file.is_open())
    {
        ROS_INFO_STREAM("Could not open file: " << path);
        return;
    }

    string line = "";
    auto lineIndex = 1;
    while(!file.eof())
    {
        getline(file, line);

        if(!line.empty())
        {
            vector<string> tokens = split(line, ',');

            if(tokens.size() != 2)
            {
                ROS_ERROR_STREAM(path << ":" << lineIndex << " - " << tokens.size() << " tokens instead of 2.");
                return;
            }

            double lat, lon;
            if(tokens[0].find('?') != string::npos)
                lat = dmsToDec(tokens[0]);
            else
                lat = stod(tokens[0]);
            if(tokens[1].find('?') != string::npos)
                lon  = dmsToDec(tokens[1]);
            else
                lon = stod(tokens[1]);

            PointStamped p;

            gps_common::UTM(lat, lon, &(p.point.x), &(p.point.y));

            waypoints.push_back(p);
        }

        lineIndex++;
    }
}

double distanceBetweenPoints(const Point &p1, const Point &p2)
{
    return sqrt((p2.x-p1.x)*(p2.x-p1.x) + (p2.y-p1.y)*(p2.y-p1.y));
}

void positionCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
{
    lock_guard<mutex> lock(current_mutex);
    if(distanceBetweenPoints(msg->pose.pose.position, current_waypoint.point) < 1.0)
    {
        // advance to next waypoint.
        current_waypoint = waypoints.front();
        current_waypoint.point.x -= map_origin.x;
        current_waypoint.point.y -= map_origin.y;
        if(waypoints.size() > 1)
            waypoints.erase(waypoints.begin());
    }
}

void originCallback(const geometry_msgs::PointStampedConstPtr& msg)
{
    lock_guard<mutex> lock(current_mutex);
    map_origin = msg->point;
}

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

    ros::NodeHandle nh;
    ros::NodeHandle nhp("~");

    ROS_INFO_STREAM("Has param: " << nhp.hasParam("file"));

    string path;
    nhp.getParam("file", path);

    ROS_INFO_STREAM("Loading waypoints from " << path);

    waypoint_pub = nh.advertise<PointStamped>("/waypoint", 1);

    ros::Subscriber odom_sub = nh.subscribe("/robot_pose_ekf/odom_combined", 1, positionCallback);

    ros::Subscriber origin_sub = nh.subscribe("/map_origin", 1, originCallback);

    loadWaypointsFile(path, waypoints);

    if(!waypoints.empty())
    {
        ROS_INFO_STREAM(waypoints.size() << " waypoints found.");
        current_waypoint = waypoints.front();
        Rate rate(1); // 1 Hz
        while(ros::ok())
        {
            ROS_INFO("publishing current waypoint...");
            lock_guard<mutex> lock(current_mutex);
            current_waypoint.header.stamp = ros::Time::now();
            current_waypoint.header.seq++;
            current_waypoint.header.frame_id = "map";
            waypoint_pub.publish(current_waypoint);

            ros::spinOnce();
            rate.sleep();
        }
    } else {
        ROS_ERROR("No valid waypoint entries found.");
    }

    return 0;
}
<|endoftext|>


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

#include "umc_defs.h"
#ifdef MFX_ENABLE_AV1_VIDEO_DECODE

#include "umc_structures.h"
#include "umc_va_base.h"

#include "umc_av1_decoder_va.h"
#include "umc_av1_utils.h"
#include "umc_av1_frame.h"
#include "umc_av1_bitstream.h"
#include "umc_av1_va_packer.h"

#include "umc_frame_data.h"

#include <algorithm>

namespace UMC_AV1_DECODER
{
    AV1DecoderVA::AV1DecoderVA()
        : va(nullptr)
    {}

    UMC::Status AV1DecoderVA::SetParams(UMC::BaseCodecParams* info)
    {
        if (!info)
            return UMC::UMC_ERR_NULL_PTR;

        AV1DecoderParams* dp =
            DynamicCast<AV1DecoderParams, UMC::BaseCodecParams>(info);
        if (!dp)
            return UMC::UMC_ERR_INVALID_PARAMS;

        if (!dp->pVideoAccelerator)
            return UMC::UMC_ERR_NULL_PTR;

        va = dp->pVideoAccelerator;
        packer.reset(Packer::CreatePacker(va));

        uint32_t const dpb_size =
            params.async_depth + TOTAL_REFS;
        SetDPBSize(dpb_size);
        SetRefSize(TOTAL_REFS);
        return UMC::UMC_OK;
    }

    UMC::Status AV1DecoderVA::SubmitTiles(AV1DecoderFrame& frame, bool firstSubmission)
    {
        VM_ASSERT(va);
        UMC::Status sts = UMC::UMC_OK;

        if (firstSubmission)
        {
            // it's first submission for current frame - need to call BeginFrame
            sts = va->BeginFrame(frame.GetMemID()); // TODO: [Rev0.85] Clarify with Ce which surface index to send: recon of display
            if (sts != UMC::UMC_OK)
                return sts;

            VM_ASSERT(packer);
            packer->BeginFrame();

            frame.StartDecoding();
        }

        auto &tileSets = frame.GetTileSets();
        packer->PackAU(tileSets, frame, firstSubmission);

        const bool lastSubmission = (GetNumMissingTiles(frame) == 0);
        if (lastSubmission)
            packer->EndFrame();

        sts = va->Execute();

        if (lastSubmission) // it's last submission for current frame - need to call EndFrame
            sts = va->EndFrame();

        return sts;
    }

    void AV1DecoderVA::AllocateFrameData(UMC::VideoDataInfo const& info, UMC::FrameMemID id, AV1DecoderFrame& frame)
    {
        VM_ASSERT(id != UMC::FRAME_MID_INVALID);

        UMC::FrameData fd;
        fd.Init(&info, id, allocator);

        frame.AllocateAndLock(&fd);
    }

    inline bool InProgress(AV1DecoderFrame const& frame)
    {
        return frame.DecodingStarted() && !frame.DecodingCompleted();
    }

    bool AV1DecoderVA::QueryFrames()
    {
        std::unique_lock<std::mutex> auto_guard(guard);

        // form frame queue in decoded order
        DPBType decode_queue;
        for (DPBType::iterator frm = dpb.begin(); frm != dpb.end(); frm++)
            if (InProgress(**frm))
                decode_queue.push_back(*frm);

        std::sort(decode_queue.begin(), decode_queue.end(),
            [](AV1DecoderFrame const* f1, AV1DecoderFrame const* f2) {return f1->UID < f2->UID; });

        // below logic around "wasCompleted" was adopted from AVC/HEVC decoders
        bool wasCompleted = false;

        // iterate through frames submitted to the driver in decoded order
        for (DPBType::iterator frm = decode_queue.begin(); frm != decode_queue.end(); frm++)
        {
            AV1DecoderFrame& frame = **frm;
            uint32_t index = 0;

            VAStatus surfErr = VA_STATUS_SUCCESS;
            index = frame.GetMemID();
            auto_guard.unlock();
            UMC::Status sts =  packer->SyncTask(index, &surfErr);
            auto_guard.lock();

            frame.CompleteDecoding();
            wasCompleted = true;

            if (sts < UMC::UMC_OK)
            {
                // TODO: [Global] Add GPU hang reporting
            }
            else if (sts == UMC::UMC_OK)
            {
                switch (surfErr)
                {
                    case MFX_CORRUPTION_MAJOR:
                        frame.AddError(UMC::ERROR_FRAME_MAJOR);
                        break;

                    case MFX_CORRUPTION_MINOR:
                        frame.AddError(UMC::ERROR_FRAME_MINOR);
                        break;
                }
            }
        }

        return wasCompleted;
    }
}

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

#include "umc_defs.h"
#ifdef MFX_ENABLE_AV1_VIDEO_DECODE

#include "umc_structures.h"
#include "umc_va_base.h"

#include "umc_av1_decoder_va.h"
#include "umc_av1_utils.h"
#include "umc_av1_frame.h"
#include "umc_av1_bitstream.h"
#include "umc_av1_va_packer.h"

#include "umc_frame_data.h"

#include <algorithm>

namespace UMC_AV1_DECODER
{
    AV1DecoderVA::AV1DecoderVA()
        : va(nullptr)
    {}

    UMC::Status AV1DecoderVA::SetParams(UMC::BaseCodecParams* info)
    {
        if (!info)
            return UMC::UMC_ERR_NULL_PTR;

        AV1DecoderParams* dp =
            DynamicCast<AV1DecoderParams, UMC::BaseCodecParams>(info);
        if (!dp)
            return UMC::UMC_ERR_INVALID_PARAMS;

        if (!dp->pVideoAccelerator)
            return UMC::UMC_ERR_NULL_PTR;

        va = dp->pVideoAccelerator;
        packer.reset(Packer::CreatePacker(va));

        uint32_t const dpb_size =
            params.async_depth + TOTAL_REFS;
        SetDPBSize(dpb_size);
        SetRefSize(TOTAL_REFS);
        return UMC::UMC_OK;
    }

    UMC::Status AV1DecoderVA::SubmitTiles(AV1DecoderFrame& frame, bool firstSubmission)
    {
        VM_ASSERT(va);
        UMC::Status sts = UMC::UMC_OK;

        if (firstSubmission)
        {
            // it's first submission for current frame - need to call BeginFrame
#ifdef UMC_VA_LINUX
            sts = va->BeginFrame(frame.GetMemID(SURFACE_RECON));
#else
            sts = va->BeginFrame(frame.GetMemID());
#endif
            if (sts != UMC::UMC_OK)
                return sts;

            VM_ASSERT(packer);
            packer->BeginFrame();

            frame.StartDecoding();
        }

        auto &tileSets = frame.GetTileSets();
        packer->PackAU(tileSets, frame, firstSubmission);

        const bool lastSubmission = (GetNumMissingTiles(frame) == 0);
        if (lastSubmission)
            packer->EndFrame();

        sts = va->Execute();

        if (lastSubmission) // it's last submission for current frame - need to call EndFrame
            sts = va->EndFrame();

        return sts;
    }

    void AV1DecoderVA::AllocateFrameData(UMC::VideoDataInfo const& info, UMC::FrameMemID id, AV1DecoderFrame& frame)
    {
        VM_ASSERT(id != UMC::FRAME_MID_INVALID);

        UMC::FrameData fd;
        fd.Init(&info, id, allocator);

        frame.AllocateAndLock(&fd);
    }

    inline bool InProgress(AV1DecoderFrame const& frame)
    {
        return frame.DecodingStarted() && !frame.DecodingCompleted();
    }

    bool AV1DecoderVA::QueryFrames()
    {
        std::unique_lock<std::mutex> auto_guard(guard);

        // form frame queue in decoded order
        DPBType decode_queue;
        for (DPBType::iterator frm = dpb.begin(); frm != dpb.end(); frm++)
            if (InProgress(**frm))
                decode_queue.push_back(*frm);

        std::sort(decode_queue.begin(), decode_queue.end(),
            [](AV1DecoderFrame const* f1, AV1DecoderFrame const* f2) {return f1->UID < f2->UID; });

        // below logic around "wasCompleted" was adopted from AVC/HEVC decoders
        bool wasCompleted = false;

        // iterate through frames submitted to the driver in decoded order
        for (DPBType::iterator frm = decode_queue.begin(); frm != decode_queue.end(); frm++)
        {
            AV1DecoderFrame& frame = **frm;
            uint32_t index = 0;

            VAStatus surfErr = VA_STATUS_SUCCESS;
            index = frame.GetMemID();
            auto_guard.unlock();
            UMC::Status sts =  packer->SyncTask(index, &surfErr);
            auto_guard.lock();

            frame.CompleteDecoding();
            wasCompleted = true;

            if (sts < UMC::UMC_OK)
            {
                // TODO: [Global] Add GPU hang reporting
            }
            else if (sts == UMC::UMC_OK)
            {
                switch (surfErr)
                {
                    case MFX_CORRUPTION_MAJOR:
                        frame.AddError(UMC::ERROR_FRAME_MAJOR);
                        break;

                    case MFX_CORRUPTION_MINOR:
                        frame.AddError(UMC::ERROR_FRAME_MINOR);
                        break;
                }
            }
        }

        return wasCompleted;
    }
}

#endif //MFX_ENABLE_AV1_VIDEO_DECODE
<|endoftext|>


<commit_before>/*
 * Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The ASF licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <decaf/lang/Thread.h>
#include <decaf/lang/Runnable.h>
#include <decaf/util/concurrent/CountDownLatch.h>
#include <activemq/core/ActiveMQConnectionFactory.h>
#include <activemq/library/ActiveMQCPP.h>
#include <decaf/lang/Integer.h>
#include <activemq/util/Config.h>
#include <decaf/util/Date.h>
#include <cms/Connection.h>
#include <cms/Session.h>
#include <cms/TextMessage.h>
#include <cms/BytesMessage.h>
#include <cms/MapMessage.h>
#include <cms/ExceptionListener.h>
#include <cms/MessageListener.h>
#include <stdlib.h>
#include <stdio.h>
#include <iostream>

using namespace activemq;
using namespace activemq::core;
using namespace decaf::lang;
using namespace decaf::util;
using namespace decaf::util::concurrent;
using namespace cms;
using namespace std;

////////////////////////////////////////////////////////////////////////////////
class SimpleAsyncConsumer : public ExceptionListener,
                            public MessageListener {
private:

    Connection* connection;
    Session* session;
    Destination* destination;
    MessageConsumer* consumer;
    bool useTopic;
    bool clientAck;
    std::string brokerURI;
    std::string destURI;

public:

    SimpleAsyncConsumer( const std::string& brokerURI,
                         const std::string& destURI,
                         bool useTopic = false,
                         bool clientAck = false ) {
        connection = NULL;
        session = NULL;
        destination = NULL;
        consumer = NULL;
        this->useTopic = useTopic;
        this->brokerURI = brokerURI;
        this->destURI = destURI;
        this->clientAck = clientAck;
    }

    virtual ~SimpleAsyncConsumer(){
        cleanup();
    }

    void runConsumer() {

        try {

            // Create a ConnectionFactory
            ActiveMQConnectionFactory* connectionFactory =
                new ActiveMQConnectionFactory( brokerURI );

            // Create a Connection
            connection = connectionFactory->createConnection();
            delete connectionFactory;
            connection->start();

            connection->setExceptionListener(this);

            // Create a Session
            if( clientAck ) {
                session = connection->createSession( Session::CLIENT_ACKNOWLEDGE );
            } else {
                session = connection->createSession( Session::AUTO_ACKNOWLEDGE );
            }

            // Create the destination (Topic or Queue)
            if( useTopic ) {
                destination = session->createTopic( destURI );
            } else {
                destination = session->createQueue( destURI );
            }

            // Create a MessageConsumer from the Session to the Topic or Queue
            consumer = session->createConsumer( destination );
            consumer->setMessageListener( this );

        } catch (CMSException& e) {
            e.printStackTrace();
        }
    }

    // Called from the consumer since this class is a registered MessageListener.
    virtual void onMessage( const Message* message ){

        static int count = 0;

        try
        {
            count++;
            const TextMessage* textMessage =
                dynamic_cast< const TextMessage* >( message );
            string text = "";

            if( textMessage != NULL ) {
                text = textMessage->getText();
            } else {
                text = "NOT A TEXTMESSAGE!";
            }

            if( clientAck ) {
                message->acknowledge();
            }

            printf( "Message #%d Received: %s\n", count, text.c_str() );
        } catch (CMSException& e) {
            e.printStackTrace();
        }
    }

    // If something bad happens you see it here as this class is also been
    // registered as an ExceptionListener with the connection.
    virtual void onException( const CMSException& ex AMQCPP_UNUSED) {
        printf("CMS Exception occurred.  Shutting down client.\n");
        exit(1);
    }

private:

    void cleanup(){

        //*************************************************
        // Always close destination, consumers and producers before
        // you destroy their sessions and connection.
        //*************************************************

        // Destroy resources.
        try{
            if( destination != NULL ) delete destination;
        }catch (CMSException& e) { e.printStackTrace(); }
        destination = NULL;

        try{
            if( consumer != NULL ) delete consumer;
        }catch (CMSException& e) { e.printStackTrace(); }
        consumer = NULL;

        // Close open resources.
        try{
            if( session != NULL ) session->close();
            if( connection != NULL ) connection->close();
        }catch (CMSException& e) { e.printStackTrace(); }

        // Now Destroy them
        try{
            if( session != NULL ) delete session;
        }catch (CMSException& e) { e.printStackTrace(); }
        session = NULL;

        try{
            if( connection != NULL ) delete connection;
        }catch (CMSException& e) { e.printStackTrace(); }
        connection = NULL;
    }
};

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

    activemq::library::ActiveMQCPP::initializeLibrary();

    std::cout << "=====================================================\n";
    std::cout << "Starting the example:" << std::endl;
    std::cout << "-----------------------------------------------------\n";

    // Set the URI to point to the IPAddress of your broker.
    // add any optional params to the url to enable things like
    // tightMarshalling or tcp logging etc.  See the CMS web site for
    // a full list of configuration options.
    //
    //  http://activemq.apache.org/cms/
    //
    // Wire Format Options:
    // =====================
    // Use either stomp or openwire, the default ports are different for each
    //
    // Examples:
    //    tcp://127.0.0.1:61616                      default to openwire
    //    tcp://127.0.0.1:61616?wireFormat=openwire  same as above
    //    tcp://127.0.0.1:61613?wireFormat=stomp     use stomp instead
    //
    std::string brokerURI =
        "failover:(tcp://127.0.0.1:61616"
//        "?wireFormat=openwire"
//        "&connection.useAsyncSend=true"
//        "&transport.commandTracingEnabled=true"
//        "&transport.tcpTracingEnabled=true"
//        "&wireFormat.tightEncodingEnabled=true"
        ")";

    //============================================================
    // This is the Destination Name and URI options.  Use this to
    // customize where the consumer listens, to have the consumer
    // use a topic or queue set the 'useTopics' flag.
    //============================================================
    std::string destURI = "TEST.FOO"; //?consumer.prefetchSize=1";

    //============================================================
    // set to true to use topics instead of queues
    // Note in the code above that this causes createTopic or
    // createQueue to be used in the consumer.
    //============================================================
    bool useTopics = false;

    //============================================================
    // set to true if you want the consumer to use client ack mode
    // instead of the default auto ack mode.
    //============================================================
    bool clientAck = false;

    // Create the consumer
    SimpleAsyncConsumer consumer( brokerURI, destURI, useTopics, clientAck );

    // Start it up and it will listen forever.
    consumer.runConsumer();

    // Wait to exit.
    std::cout << "Press 'q' to quit" << std::endl;
    while( std::cin.get() != 'q') {}

    std::cout << "-----------------------------------------------------\n";
    std::cout << "Finished with the example." << std::endl;
    std::cout << "=====================================================\n";

    activemq::library::ActiveMQCPP::shutdownLibrary();
}
<commit_msg>http://issues.apache.org/activemq/browse/AMQCPP-250<commit_after>/*
 * Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The ASF licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <decaf/lang/Thread.h>
#include <decaf/lang/Runnable.h>
#include <decaf/util/concurrent/CountDownLatch.h>
#include <activemq/core/ActiveMQConnectionFactory.h>
#include <activemq/core/ActiveMQConnection.h>
#include <activemq/transport/DefaultTransportListener.h>
#include <activemq/library/ActiveMQCPP.h>
#include <decaf/lang/Integer.h>
#include <activemq/util/Config.h>
#include <decaf/util/Date.h>
#include <cms/Connection.h>
#include <cms/Session.h>
#include <cms/TextMessage.h>
#include <cms/BytesMessage.h>
#include <cms/MapMessage.h>
#include <cms/ExceptionListener.h>
#include <cms/MessageListener.h>
#include <stdlib.h>
#include <stdio.h>
#include <iostream>

using namespace activemq;
using namespace activemq::core;
using namespace activemq::transport;
using namespace decaf::lang;
using namespace decaf::util;
using namespace decaf::util::concurrent;
using namespace cms;
using namespace std;

////////////////////////////////////////////////////////////////////////////////
class SimpleAsyncConsumer : public ExceptionListener,
                            public MessageListener,
                            public DefaultTransportListener {
private:

    Connection* connection;
    Session* session;
    Destination* destination;
    MessageConsumer* consumer;
    bool useTopic;
    bool clientAck;
    std::string brokerURI;
    std::string destURI;

public:

    SimpleAsyncConsumer( const std::string& brokerURI,
                         const std::string& destURI,
                         bool useTopic = false,
                         bool clientAck = false ) {
        connection = NULL;
        session = NULL;
        destination = NULL;
        consumer = NULL;
        this->useTopic = useTopic;
        this->brokerURI = brokerURI;
        this->destURI = destURI;
        this->clientAck = clientAck;
    }

    virtual ~SimpleAsyncConsumer(){
        cleanup();
    }

    void runConsumer() {

        try {

            // Create a ConnectionFactory
            ActiveMQConnectionFactory* connectionFactory =
                new ActiveMQConnectionFactory( brokerURI );

            // Create a Connection
            connection = connectionFactory->createConnection();
            delete connectionFactory;

            ActiveMQConnection* amqConnection = dynamic_cast<ActiveMQConnection*>( connection );
            if( amqConnection != NULL ) {
                amqConnection->addTransportListener( this );
            }

            connection->start();

            connection->setExceptionListener(this);

            // Create a Session
            if( clientAck ) {
                session = connection->createSession( Session::CLIENT_ACKNOWLEDGE );
            } else {
                session = connection->createSession( Session::AUTO_ACKNOWLEDGE );
            }

            // Create the destination (Topic or Queue)
            if( useTopic ) {
                destination = session->createTopic( destURI );
            } else {
                destination = session->createQueue( destURI );
            }

            // Create a MessageConsumer from the Session to the Topic or Queue
            consumer = session->createConsumer( destination );
            consumer->setMessageListener( this );

        } catch (CMSException& e) {
            e.printStackTrace();
        }
    }

    // Called from the consumer since this class is a registered MessageListener.
    virtual void onMessage( const Message* message ){

        static int count = 0;

        try
        {
            count++;
            const TextMessage* textMessage =
                dynamic_cast< const TextMessage* >( message );
            string text = "";

            if( textMessage != NULL ) {
                text = textMessage->getText();
            } else {
                text = "NOT A TEXTMESSAGE!";
            }

            if( clientAck ) {
                message->acknowledge();
            }

            printf( "Message #%d Received: %s\n", count, text.c_str() );
        } catch (CMSException& e) {
            e.printStackTrace();
        }
    }

    // If something bad happens you see it here as this class is also been
    // registered as an ExceptionListener with the connection.
    virtual void onException( const CMSException& ex AMQCPP_UNUSED ) {
        printf("CMS Exception occurred.  Shutting down client.\n");
        exit(1);
    }

    virtual void transportInterrupted() {
        std::cout << "The Connection's Transport has been Interrupted." << std::endl;
    }

    virtual void transportResumed() {
        std::cout << "The Connection's Transport has been Restored." << std::endl;
    }

private:

    void cleanup(){

        //*************************************************
        // Always close destination, consumers and producers before
        // you destroy their sessions and connection.
        //*************************************************

        // Destroy resources.
        try{
            if( destination != NULL ) delete destination;
        }catch (CMSException& e) { e.printStackTrace(); }
        destination = NULL;

        try{
            if( consumer != NULL ) delete consumer;
        }catch (CMSException& e) { e.printStackTrace(); }
        consumer = NULL;

        // Close open resources.
        try{
            if( session != NULL ) session->close();
            if( connection != NULL ) connection->close();
        }catch (CMSException& e) { e.printStackTrace(); }

        // Now Destroy them
        try{
            if( session != NULL ) delete session;
        }catch (CMSException& e) { e.printStackTrace(); }
        session = NULL;

        try{
            if( connection != NULL ) delete connection;
        }catch (CMSException& e) { e.printStackTrace(); }
        connection = NULL;
    }
};

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

    activemq::library::ActiveMQCPP::initializeLibrary();

    std::cout << "=====================================================\n";
    std::cout << "Starting the example:" << std::endl;
    std::cout << "-----------------------------------------------------\n";

    // Set the URI to point to the IPAddress of your broker.
    // add any optional params to the url to enable things like
    // tightMarshalling or tcp logging etc.  See the CMS web site for
    // a full list of configuration options.
    //
    //  http://activemq.apache.org/cms/
    //
    // Wire Format Options:
    // =====================
    // Use either stomp or openwire, the default ports are different for each
    //
    // Examples:
    //    tcp://127.0.0.1:61616                      default to openwire
    //    tcp://127.0.0.1:61616?wireFormat=openwire  same as above
    //    tcp://127.0.0.1:61613?wireFormat=stomp     use stomp instead
    //
    std::string brokerURI =
        "failover:(tcp://127.0.0.1:61616"
//        "?wireFormat=openwire"
//        "&connection.useAsyncSend=true"
//        "&transport.commandTracingEnabled=true"
//        "&transport.tcpTracingEnabled=true"
//        "&wireFormat.tightEncodingEnabled=true"
        ")";

    //============================================================
    // This is the Destination Name and URI options.  Use this to
    // customize where the consumer listens, to have the consumer
    // use a topic or queue set the 'useTopics' flag.
    //============================================================
    std::string destURI = "TEST.FOO"; //?consumer.prefetchSize=1";

    //============================================================
    // set to true to use topics instead of queues
    // Note in the code above that this causes createTopic or
    // createQueue to be used in the consumer.
    //============================================================
    bool useTopics = false;

    //============================================================
    // set to true if you want the consumer to use client ack mode
    // instead of the default auto ack mode.
    //============================================================
    bool clientAck = false;

    // Create the consumer
    SimpleAsyncConsumer consumer( brokerURI, destURI, useTopics, clientAck );

    // Start it up and it will listen forever.
    consumer.runConsumer();

    // Wait to exit.
    std::cout << "Press 'q' to quit" << std::endl;
    while( std::cin.get() != 'q') {}

    std::cout << "-----------------------------------------------------\n";
    std::cout << "Finished with the example." << std::endl;
    std::cout << "=====================================================\n";

    activemq::library::ActiveMQCPP::shutdownLibrary();
}
<|endoftext|>


<commit_before>#include <arpc.h>

#include <modlogger.h>
#include <misc_utils.h>

#include <dhash_types.h>
#include <dhash_common.h>
#include <merkle_sync_prot.h>

#include <merkle.h>

#include <maint_prot.h>
#include "maint_policy.h"

// {{{ Globals
static char *logfname;
static char *localdatapath;

static vec<ptr<maintainer> > maintainers;

enum maint_mode_t {
  MAINT_CARBONITE,
  MAINT_PASSINGTONE,
} maint_mode;
struct maint_mode_desc {
  maint_mode_t m;
  char *cmdline;
  maintainer_producer_t producer;
} maint_modes[] = {
  { MAINT_CARBONITE, "carbonite",
    wrap (carbonite::produce_maintainer) },
  { MAINT_PASSINGTONE, "passingtone",
    wrap (passingtone::produce_maintainer) }
};

enum sync_mode_t {
  SYNC_MERKLE,
  SYNC_TIME
} sync_mode;
struct sync_mode_desc {
  sync_mode_t m;
  char *cmdline;
  syncer_producer_t producer;
} sync_modes[] = {
  { SYNC_MERKLE, "merkle",
    wrap (merkle_sync::produce_syncer) }
};
// }}}

// {{{ General utility functions
template<class VS, class S>
static S
select_mode (const char *arg, const VS *modes, int nmodes)
{
  int i;
  S m = modes[0].m;
  for (i = 0; i < nmodes; i++) {
    if (strcmp (arg, modes[i].cmdline) == 0) {
      m = modes[i].m;
      break;
    }
  }
  if (i == nmodes) {
    strbuf s;
    for (i = 0; i < nmodes; i++)
      s << " " << modes[i].cmdline;
    fatal << "allowed modes are" << s << "\n";
  }
  return m;
}

static void
halt ()
{
  warnx << "Exiting on command.\n";
  while (maintainers.size ()) {
    maintainer *m = maintainers.pop_back ();
    delete m;
  }
  exit (0);
}

void
start_logs ()
{
  static int logfd (-1);
  // XXX please don't call setlogfd or change errfd anywhere else...

  if (logfname) {
    if (logfd >= 0)
      close (logfd);
    logfd = open (logfname, O_RDWR | O_CREAT, 0666);
    if (logfd < 0)
      fatal << "Could not open log file " << logfd << " for appending.\n";
    lseek (logfd, 0, SEEK_END);
    errfd = logfd;
    modlogger::setlogfd (logfd);
  }
}

static void
usage ()
{
  warnx << "Usage: " << progname
	<< "\t[-C maintd-ctlsock]\n"
	<< "\t[-d localdatapath]\n"
        << "\t[-L logfilename]\n"
	<< "\t[-m maintmode]\n"
	<< "\t[-s syncmode]\n"
	<< "\t[-t]\n";
  exit (1);
}
// }}}
// {{{ Remote-side RPC handling
static void srvaccept (int fd);
static void sync_dispatch (ptr<asrv> srv, svccb *sbp);

static void
init_remote_server (const net_address &addr)
{
  in_addr laddr;
  inet_aton (addr.hostname, &laddr);
  int tcpfd = inetsocket (SOCK_STREAM, addr.port-1,
      ntohl (laddr.s_addr));
  if (tcpfd < 0)
    fatal ("binding TCP addr %s port %d: %m\n",
	addr.hostname.cstr (), addr.port-1);
  int ret = listen (tcpfd, 5);
  if (ret < 0)
    fatal ("listen (%d, 5): %m\n", tcpfd);
  fdcb (tcpfd, selread, wrap (&srvaccept, tcpfd));
}

static void
srvaccept (int lfd)
{
  sockaddr_un sun;
  bzero (&sun, sizeof (sun));
  socklen_t sunlen = sizeof (sun);
  int fd = accept (lfd, reinterpret_cast<sockaddr *> (&sun), &sunlen);
  if (fd < 0) {
    warn ("accept: unexpected EOF: %m\n");
    return;
  }
  ref<axprt_stream> c = axprt_stream::alloc (fd, 1024*1025);
  // XXX should accept for whatever the chosen syncmode wants.
  ptr<asrv> s = asrv::alloc (c, merklesync_program_1);
  s->setcb (wrap (&sync_dispatch, s));
}

static void
sync_dispatch (ptr<asrv> srv, svccb *sbp)
{
  if (!sbp) {
    srv->setcb (NULL);
    srv = NULL;
    return;
  }
  bool ok = false;
  syncdest_t *discrim = sbp->Xtmpl getarg<syncdest_t> ();
  for (size_t i = 0; i < maintainers.size (); i++) {
    if (maintainers[i]->sync->sync_program ().progno == sbp->prog () &&
	maintainers[i]->sync->ctype == discrim->ctype &&
	maintainers[i]->host.vnode_num == (int) discrim->vnode)
    {
      ok = true;
      ptr<merkle_tree> t = maintainers[i]->localtree ();
      maintainers[i]->sync->dispatch (t, sbp);
      break;
    }
  }
  if (!ok)
    sbp->reject (PROG_UNAVAIL);
}
// }}}
// {{{ Control-side RPC execution
void
do_setmaint (svccb *sbp)
{
  maint_setmaintarg *arg = sbp->Xtmpl getarg<maint_setmaintarg> ();
  for (unsigned int i = 0; i < maintainers.size (); i++) {
    if (arg->enable) {
      maintainers[i]->start (arg->delay);
    } else {
      maintainers[i]->stop ();
    }
  }
  sbp->replyref (arg->enable);
}

void
do_initspace (svccb *sbp)
{
  maint_dhashinfo_t *arg = sbp->Xtmpl getarg<maint_dhashinfo_t> ();
  maint_status res (MAINTPROC_OK);

  chord_node host = arg->host;
  dhash_ctype ctype = arg->ctype;

  // Check that we don't already have a maintainer for this host/ctype
  for (unsigned int i = 0; i < maintainers.size (); i++) {
    if (maintainers[i]->host.r.hostname == host.r.hostname &&
	maintainers[i]->host.r.port     == host.r.port &&
	maintainers[i]->host.vnode_num  == host.vnode_num &&
	maintainers[i]->ctype == ctype)
    {
      res = MAINTPROC_ERR;
      sbp->replyref (res);
      return;
    }
  }

  ptr<syncer> s = sync_modes[sync_mode].producer (ctype);
  ptr<maintainer> m = maint_modes[maint_mode].producer (localdatapath, arg, s);
  maintainers.push_back (m);

  sbp->replyref (res);
}

void
do_listen (svccb *sbp)
{
  static bool initialized (false);
  maint_status res (MAINTPROC_OK);
  net_address *addr = sbp->Xtmpl getarg<net_address> ();
  if (!initialized) {
    init_remote_server (*addr);
    initialized = true;
  } else {
    res = MAINTPROC_ERR;
  }
  sbp->replyref (res);
}

void
do_getrepairs (svccb *sbp)
{
  maint_getrepairsarg *arg = sbp->Xtmpl getarg<maint_getrepairsarg> ();
  maint_getrepairsres *res = sbp->Xtmpl getarg<maint_getrepairsres> ();

  chord_node host = arg->host;
  dhash_ctype ctype = arg->ctype;
  ptr<maintainer> m = NULL;

  for (unsigned int i = 0; i < maintainers.size (); i++) {
    if (maintainers[i]->host.r.hostname == host.r.hostname &&
	maintainers[i]->host.r.port     == host.r.port &&
	maintainers[i]->host.vnode_num  == host.vnode_num &&
	maintainers[i]->ctype == ctype)
    {
      m = maintainers[i];
    }
  }
  if (!m) {
    res->status = MAINTPROC_ERR;
    sbp->reply (res);
    return;
  }

  m->getrepairs (arg->start, arg->thresh, arg->count, res->repairs);
  sbp->reply (res);
}
// }}}
// {{{ Control-side RPC accept and dispatch
static void accept_cb (int lfd);
void dispatch (ref<axprt_stream> s, ptr<asrv> a, svccb *sbp);
static void
listen_unix (str sock_name)
{
  unlink (sock_name);
  int clntfd = unixsocket (sock_name);
  if (clntfd < 0)
    fatal << "Error creating socket (UNIX)" << strerror (errno) << "\n";

  if (listen (clntfd, 128) < 0) {
    fatal ("Error from listen: %m\n");
    close (clntfd);
  } else {
    fdcb (clntfd, selread, wrap (accept_cb, clntfd));
  }
}

static void
accept_cb (int lfd)
{
  sockaddr_un sun;
  bzero (&sun, sizeof (sun));
  socklen_t sunlen = sizeof (sun);
  int fd = accept (lfd, reinterpret_cast<sockaddr *> (&sun), &sunlen);
  if (fd < 0)
    fatal ("EOF\n");

  ref<axprt_stream> x = axprt_stream::alloc (fd, 1024*1025);

  ptr<asrv> a = asrv::alloc (x, maint_program_1);
  a->setcb (wrap (dispatch, x, a));
}

void
dispatch (ref<axprt_stream> s, ptr<asrv> a, svccb *sbp)
{
  if (sbp == NULL) {
    warn << "EOF from client\n";
    a = NULL;
    return;
  }

  switch (sbp->proc ()) {
  case MAINTPROC_NULL:
    sbp->reply (NULL);
    break;
  case MAINTPROC_SETMAINT:
    do_setmaint (sbp);
    break;
  case MAINTPROC_INITSPACE:
    do_initspace (sbp);
    break;
  case MAINTPROC_LISTEN:
    do_listen (sbp);
    break;
  case MAINTPROC_GETREPAIRS:
    do_getrepairs (sbp);
    break;
  default:
    warn << "unknown procedure: " << sbp->proc () << "\n";
    sbp->reject (PROC_UNAVAIL);
  }

  return;
}
// }}}

int
main (int argc, char **argv)
{
  str ctlsock = "/tmp/maint-sock";
  char ch;

  setprogname (argv[0]);
  random_init ();

  localdatapath = "./maintdata/";
  maint_mode = MAINT_CARBONITE;
  sync_mode = SYNC_MERKLE;

  while ((ch = getopt (argc, argv, "C:d:L:m:s:t"))!=-1)
    switch (ch) {
    case 'C':
      ctlsock = optarg;
      break;
    case 'd':
      localdatapath = optarg;
      break;
    case 'L':
      logfname = optarg;
      break;
    case 'm':
      maint_mode = select_mode<maint_mode_desc, maint_mode_t> (optarg, maint_modes, sizeof (maint_modes)/sizeof (maint_modes[0]));
      break;
    case 's':
      sync_mode = select_mode<sync_mode_desc, sync_mode_t> (optarg, sync_modes, sizeof (sync_modes)/sizeof (sync_modes[0]));
      break;
    case 't':
      modlogger::setmaxprio (modlogger::TRACE);
      break;
    default:
      usage ();
      break;
    }

  start_logs ();

  warn << "Starting up " << maint_modes[maint_mode].cmdline
       << " maintenance, syncing with "
       << sync_modes[sync_mode].cmdline << ".\n";

  {
    struct stat sb;
    if (stat (localdatapath, &sb) < 0) {
      if (errno != ENOENT ||
	  (mkdir (localdatapath, 0755) < 0 && errno != EEXIST))
	fatal ("%s: %m\n", localdatapath);
      if (stat (localdatapath, &sb) < 0)
	fatal ("stat (%s): %m\n", localdatapath);
      warn << "Created " << localdatapath << " for maintenance state.\n";
    }
    if (!S_ISDIR (sb.st_mode))
      fatal ("%s: not a directory\n", localdatapath);
  }

  listen_unix (ctlsock);

  sigcb (SIGHUP, wrap (&start_logs));
  sigcb (SIGINT, wrap (&halt));
  sigcb (SIGTERM, wrap (&halt));

  amain ();
}

// vim: foldmethod=marker
<commit_msg>maintd: maintainers are ptr's so don't explicitly delete them.<commit_after>#include <arpc.h>

#include <modlogger.h>
#include <misc_utils.h>

#include <dhash_types.h>
#include <dhash_common.h>
#include <merkle_sync_prot.h>

#include <merkle.h>

#include <maint_prot.h>
#include "maint_policy.h"

// {{{ Globals
static char *logfname;
static char *localdatapath;

static vec<ptr<maintainer> > maintainers;

enum maint_mode_t {
  MAINT_CARBONITE,
  MAINT_PASSINGTONE,
} maint_mode;
struct maint_mode_desc {
  maint_mode_t m;
  char *cmdline;
  maintainer_producer_t producer;
} maint_modes[] = {
  { MAINT_CARBONITE, "carbonite",
    wrap (carbonite::produce_maintainer) },
  { MAINT_PASSINGTONE, "passingtone",
    wrap (passingtone::produce_maintainer) }
};

enum sync_mode_t {
  SYNC_MERKLE,
  SYNC_TIME
} sync_mode;
struct sync_mode_desc {
  sync_mode_t m;
  char *cmdline;
  syncer_producer_t producer;
} sync_modes[] = {
  { SYNC_MERKLE, "merkle",
    wrap (merkle_sync::produce_syncer) }
};
// }}}

// {{{ General utility functions
template<class VS, class S>
static S
select_mode (const char *arg, const VS *modes, int nmodes)
{
  int i;
  S m = modes[0].m;
  for (i = 0; i < nmodes; i++) {
    if (strcmp (arg, modes[i].cmdline) == 0) {
      m = modes[i].m;
      break;
    }
  }
  if (i == nmodes) {
    strbuf s;
    for (i = 0; i < nmodes; i++)
      s << " " << modes[i].cmdline;
    fatal << "allowed modes are" << s << "\n";
  }
  return m;
}

static void
halt ()
{
  warnx << "Exiting on command.\n";
  while (maintainers.size ()) {
    maintainers.pop_back ();
  }
  exit (0);
}

void
start_logs ()
{
  static int logfd (-1);
  // XXX please don't call setlogfd or change errfd anywhere else...

  if (logfname) {
    if (logfd >= 0)
      close (logfd);
    logfd = open (logfname, O_RDWR | O_CREAT, 0666);
    if (logfd < 0)
      fatal << "Could not open log file " << logfd << " for appending.\n";
    lseek (logfd, 0, SEEK_END);
    errfd = logfd;
    modlogger::setlogfd (logfd);
  }
}

static void
usage ()
{
  warnx << "Usage: " << progname
	<< "\t[-C maintd-ctlsock]\n"
	<< "\t[-d localdatapath]\n"
        << "\t[-L logfilename]\n"
	<< "\t[-m maintmode]\n"
	<< "\t[-s syncmode]\n"
	<< "\t[-t]\n";
  exit (1);
}
// }}}
// {{{ Remote-side RPC handling
static void srvaccept (int fd);
static void sync_dispatch (ptr<asrv> srv, svccb *sbp);

static void
init_remote_server (const net_address &addr)
{
  in_addr laddr;
  inet_aton (addr.hostname, &laddr);
  int tcpfd = inetsocket (SOCK_STREAM, addr.port-1,
      ntohl (laddr.s_addr));
  if (tcpfd < 0)
    fatal ("binding TCP addr %s port %d: %m\n",
	addr.hostname.cstr (), addr.port-1);
  int ret = listen (tcpfd, 5);
  if (ret < 0)
    fatal ("listen (%d, 5): %m\n", tcpfd);
  fdcb (tcpfd, selread, wrap (&srvaccept, tcpfd));
}

static void
srvaccept (int lfd)
{
  sockaddr_un sun;
  bzero (&sun, sizeof (sun));
  socklen_t sunlen = sizeof (sun);
  int fd = accept (lfd, reinterpret_cast<sockaddr *> (&sun), &sunlen);
  if (fd < 0) {
    warn ("accept: unexpected EOF: %m\n");
    return;
  }
  ref<axprt_stream> c = axprt_stream::alloc (fd, 1024*1025);
  // XXX should accept for whatever the chosen syncmode wants.
  ptr<asrv> s = asrv::alloc (c, merklesync_program_1);
  s->setcb (wrap (&sync_dispatch, s));
}

static void
sync_dispatch (ptr<asrv> srv, svccb *sbp)
{
  if (!sbp) {
    srv->setcb (NULL);
    srv = NULL;
    return;
  }
  bool ok = false;
  syncdest_t *discrim = sbp->Xtmpl getarg<syncdest_t> ();
  for (size_t i = 0; i < maintainers.size (); i++) {
    if (maintainers[i]->sync->sync_program ().progno == sbp->prog () &&
	maintainers[i]->sync->ctype == discrim->ctype &&
	maintainers[i]->host.vnode_num == (int) discrim->vnode)
    {
      ok = true;
      ptr<merkle_tree> t = maintainers[i]->localtree ();
      maintainers[i]->sync->dispatch (t, sbp);
      break;
    }
  }
  if (!ok)
    sbp->reject (PROG_UNAVAIL);
}
// }}}
// {{{ Control-side RPC execution
void
do_setmaint (svccb *sbp)
{
  maint_setmaintarg *arg = sbp->Xtmpl getarg<maint_setmaintarg> ();
  for (unsigned int i = 0; i < maintainers.size (); i++) {
    if (arg->enable) {
      maintainers[i]->start (arg->delay);
    } else {
      maintainers[i]->stop ();
    }
  }
  sbp->replyref (arg->enable);
}

void
do_initspace (svccb *sbp)
{
  maint_dhashinfo_t *arg = sbp->Xtmpl getarg<maint_dhashinfo_t> ();
  maint_status res (MAINTPROC_OK);

  chord_node host = arg->host;
  dhash_ctype ctype = arg->ctype;

  // Check that we don't already have a maintainer for this host/ctype
  for (unsigned int i = 0; i < maintainers.size (); i++) {
    if (maintainers[i]->host.r.hostname == host.r.hostname &&
	maintainers[i]->host.r.port     == host.r.port &&
	maintainers[i]->host.vnode_num  == host.vnode_num &&
	maintainers[i]->ctype == ctype)
    {
      res = MAINTPROC_ERR;
      sbp->replyref (res);
      return;
    }
  }

  ptr<syncer> s = sync_modes[sync_mode].producer (ctype);
  ptr<maintainer> m = maint_modes[maint_mode].producer (localdatapath, arg, s);
  maintainers.push_back (m);

  sbp->replyref (res);
}

void
do_listen (svccb *sbp)
{
  static bool initialized (false);
  maint_status res (MAINTPROC_OK);
  net_address *addr = sbp->Xtmpl getarg<net_address> ();
  if (!initialized) {
    init_remote_server (*addr);
    initialized = true;
  } else {
    res = MAINTPROC_ERR;
  }
  sbp->replyref (res);
}

void
do_getrepairs (svccb *sbp)
{
  maint_getrepairsarg *arg = sbp->Xtmpl getarg<maint_getrepairsarg> ();
  maint_getrepairsres *res = sbp->Xtmpl getarg<maint_getrepairsres> ();

  chord_node host = arg->host;
  dhash_ctype ctype = arg->ctype;
  ptr<maintainer> m = NULL;

  for (unsigned int i = 0; i < maintainers.size (); i++) {
    if (maintainers[i]->host.r.hostname == host.r.hostname &&
	maintainers[i]->host.r.port     == host.r.port &&
	maintainers[i]->host.vnode_num  == host.vnode_num &&
	maintainers[i]->ctype == ctype)
    {
      m = maintainers[i];
    }
  }
  if (!m) {
    res->status = MAINTPROC_ERR;
    sbp->reply (res);
    return;
  }

  m->getrepairs (arg->start, arg->thresh, arg->count, res->repairs);
  sbp->reply (res);
}
// }}}
// {{{ Control-side RPC accept and dispatch
static void accept_cb (int lfd);
void dispatch (ref<axprt_stream> s, ptr<asrv> a, svccb *sbp);
static void
listen_unix (str sock_name)
{
  unlink (sock_name);
  int clntfd = unixsocket (sock_name);
  if (clntfd < 0)
    fatal << "Error creating socket (UNIX)" << strerror (errno) << "\n";

  if (listen (clntfd, 128) < 0) {
    fatal ("Error from listen: %m\n");
    close (clntfd);
  } else {
    fdcb (clntfd, selread, wrap (accept_cb, clntfd));
  }
}

static void
accept_cb (int lfd)
{
  sockaddr_un sun;
  bzero (&sun, sizeof (sun));
  socklen_t sunlen = sizeof (sun);
  int fd = accept (lfd, reinterpret_cast<sockaddr *> (&sun), &sunlen);
  if (fd < 0)
    fatal ("EOF\n");

  ref<axprt_stream> x = axprt_stream::alloc (fd, 1024*1025);

  ptr<asrv> a = asrv::alloc (x, maint_program_1);
  a->setcb (wrap (dispatch, x, a));
}

void
dispatch (ref<axprt_stream> s, ptr<asrv> a, svccb *sbp)
{
  if (sbp == NULL) {
    warn << "EOF from client\n";
    a = NULL;
    return;
  }

  switch (sbp->proc ()) {
  case MAINTPROC_NULL:
    sbp->reply (NULL);
    break;
  case MAINTPROC_SETMAINT:
    do_setmaint (sbp);
    break;
  case MAINTPROC_INITSPACE:
    do_initspace (sbp);
    break;
  case MAINTPROC_LISTEN:
    do_listen (sbp);
    break;
  case MAINTPROC_GETREPAIRS:
    do_getrepairs (sbp);
    break;
  default:
    warn << "unknown procedure: " << sbp->proc () << "\n";
    sbp->reject (PROC_UNAVAIL);
  }

  return;
}
// }}}

int
main (int argc, char **argv)
{
  str ctlsock = "/tmp/maint-sock";
  char ch;

  setprogname (argv[0]);
  random_init ();

  localdatapath = "./maintdata/";
  maint_mode = MAINT_CARBONITE;
  sync_mode = SYNC_MERKLE;

  while ((ch = getopt (argc, argv, "C:d:L:m:s:t"))!=-1)
    switch (ch) {
    case 'C':
      ctlsock = optarg;
      break;
    case 'd':
      localdatapath = optarg;
      break;
    case 'L':
      logfname = optarg;
      break;
    case 'm':
      maint_mode = select_mode<maint_mode_desc, maint_mode_t> (optarg, maint_modes, sizeof (maint_modes)/sizeof (maint_modes[0]));
      break;
    case 's':
      sync_mode = select_mode<sync_mode_desc, sync_mode_t> (optarg, sync_modes, sizeof (sync_modes)/sizeof (sync_modes[0]));
      break;
    case 't':
      modlogger::setmaxprio (modlogger::TRACE);
      break;
    default:
      usage ();
      break;
    }

  start_logs ();

  warn << "Starting up " << maint_modes[maint_mode].cmdline
       << " maintenance, syncing with "
       << sync_modes[sync_mode].cmdline << ".\n";

  {
    struct stat sb;
    if (stat (localdatapath, &sb) < 0) {
      if (errno != ENOENT ||
	  (mkdir (localdatapath, 0755) < 0 && errno != EEXIST))
	fatal ("%s: %m\n", localdatapath);
      if (stat (localdatapath, &sb) < 0)
	fatal ("stat (%s): %m\n", localdatapath);
      warn << "Created " << localdatapath << " for maintenance state.\n";
    }
    if (!S_ISDIR (sb.st_mode))
      fatal ("%s: not a directory\n", localdatapath);
  }

  listen_unix (ctlsock);

  sigcb (SIGHUP, wrap (&start_logs));
  sigcb (SIGINT, wrap (&halt));
  sigcb (SIGTERM, wrap (&halt));

  amain ();
}

// vim: foldmethod=marker
<|endoftext|>


<commit_before>#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/transforms.h>
#include <pcl/registration/icp.h>

#include <boost/graph/graphviz.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/graph_traits.hpp>
using boost::graph_traits;

#include <boost/graph/adjacency_list.hpp>
using boost::edge_weight_t;

#include <iostream>
using std::cout;
using std::endl;
#include <string>
using std::string;
#include <fstream>
using std::ofstream;

#include <vector>
using std::vector;

#include <list>
using std::list;

#include <algorithm>
using std::swap;

typedef pcl::PointXYZ PointType;
typedef pcl::PointCloud<PointType> Cloud;
typedef Cloud::ConstPtr CloudConstPtr;
typedef Cloud::Ptr CloudPtr;
typedef std::pair<std::string, CloudPtr> CloudPair;
typedef std::vector<CloudPair> CloudVector;

typedef boost::adjacency_list<
  boost::listS, boost::vecS, boost::undirectedS,
  boost::no_property,
  boost::property< edge_weight_t, double > >
Graph;

/**
 * graph balancer algorithm computes the weights
 * @param g the graph
 * @param f index of the first node
 * @param l index of the last node
 * @param weights array for the weights
 */
void
loopOptimizerAlgorithm (Graph &g, int f, int l, double *weights)
{
  list<int> crossings, branches;
  crossings.push_back (f);
  crossings.push_back (l);
  weights[f] = 0;
  weights[l] = 1;

  int *p = new int[num_vertices (g)];
  int *p_min = new int[num_vertices (g)];
  double *d = new double[num_vertices (g)];
  double *d_min = new double[num_vertices (g)];
  double dist;
  bool do_swap = false;
  list<int>::iterator crossings_it, end_it, start_min, end_min;

  // process all junctions
  while (!crossings.empty ())
  {
    dist = -1;
    // find shortest crossing for all vertices on the loop
    for (crossings_it = crossings.begin (); crossings_it != crossings.end (); )
    {
      dijkstra_shortest_paths (g, *crossings_it, boost::predecessor_map (p).distance_map (d));
      end_it = crossings_it;
      end_it++;
      // find shortest crossing for one vertex
      for (; end_it != crossings.end (); end_it++)
      {
        if (*end_it != p[*end_it] && (dist < 0 || d[*end_it] < dist))
        {
          dist = d[*end_it];
          start_min = crossings_it;
          end_min = end_it;
          do_swap = true;
        }
      }
      if (do_swap)
      {
        swap (p, p_min);
        swap (d, d_min);
        do_swap = false;
      }
      // vertex starts a branch
      if (dist < 0)
      {
        branches.push_back (*crossings_it);
        crossings_it = crossings.erase (crossings_it);
      }
      else
        crossings_it++;
    }

    if (dist > -1)
    {
      remove_edge (*end_min, p_min[*end_min], g);
      for (int i = p_min[*end_min]; i != *start_min; i = p_min[i])
      {
        //even right with weights[*start_min] > weights[*end_min]! (math works)
        weights[i] = weights[*start_min] + (weights[*end_min] - weights[*start_min]) * d_min[i] / d_min[*end_min];
        remove_edge (i, p_min[i], g);
        if (degree (i, g) > 0)
        {
          crossings.push_back (i);
        }
      }

      if (degree (*start_min, g) == 0)
        crossings.erase (start_min);

      if (degree (*end_min, g) == 0)
        crossings.erase (end_min);
    }
  }

  delete[] p;
  delete[] p_min;
  delete[] d;
  delete[] d_min;

  graph_traits<Graph>::adjacency_iterator adjacent_it, adjacent_it_end;
  int s;

  // error propagation
  while (!branches.empty ())
  {
    s = branches.front ();
    branches.pop_front ();

    for (tie (adjacent_it, adjacent_it_end) = adjacent_vertices (s, g); adjacent_it != adjacent_it_end; ++adjacent_it)
    {
      weights[*adjacent_it] = weights[s];
      if (degree (*adjacent_it, g) > 1)
        branches.push_back (*adjacent_it);
    }
    clear_vertex (s, g);
  }
}

void
elch (int start, int end, const CloudVector &clouds)
{
  CloudPtr tmp (new Cloud);

  Eigen::Vector4f cstart;
  pcl::compute3DCentroid (*(clouds[start].second), cstart);
  Eigen::Vector3f cs2 (cstart[0], cstart[1], cstart[2]);

  Eigen::Vector4f cend2;
  pcl::compute3DCentroid (*(clouds[end].second), cend2);
  Eigen::Vector3f ce2 (cend2[0], cend2[1], cend2[2]);
  ce2 = cs2 - ce2;
  pcl::transformPointCloud (*(clouds[end].second), *tmp, ce2, Eigen::Quaternionf::Identity ());

  pcl::IterativeClosestPoint<PointType, PointType> icp;

  icp.setMaximumIterations (50);
  icp.setMaxCorrespondenceDistance (1.5);
  icp.setRANSACOutlierRejectionThreshold (1.5);
  //icp.setRANSACOutlierRejectionThreshold (DBL_MAX);

  icp.setInputTarget (clouds[start].second);

  icp.setInputCloud (tmp);

  icp.align (*tmp);

  Eigen::Matrix3f m (icp.getFinalTransformation ().block (0, 0, 3, 3));
  Eigen::Quaternionf q (m);
  Eigen::Vector4f ta (icp.getFinalTransformation ().col (3));
  Eigen::Vector3f t (ta[0], ta[1], ta[2]);
  //TODO hack
  t += ce2;

  std::cout << icp.getFinalTransformation () << std::endl;

  static Graph g;

  for (size_t i = num_vertices (g)+1; i < clouds.size (); i++)
    add_edge (i-1, i, g);

  Graph grb[4];

  graph_traits<Graph>::edge_iterator edge_it, edge_it_end;
  for (tie (edge_it, edge_it_end) = edges (g); edge_it != edge_it_end; edge_it++)
  {
    for (int j = 0; j < 4; j++)
      add_edge (source (*edge_it, g), target (*edge_it, g), 1, grb[j]);  //TODO add variance
  }

  double *weights[4];
  for (int i = 0; i < 4; i++)
  {
    weights[i] = new double[clouds.size ()];
    loopOptimizerAlgorithm (grb[i], start, end, weights[i]);
  }

  //TODO use pose
  Eigen::Vector4f cend;
  pcl::compute3DCentroid (*(clouds[end].second), cend);
  Eigen::Translation3f tend (cend[0], cend[1], cend[2]);
  Eigen::Affine3f aend (tend);
  Eigen::Affine3f aendI = aend.inverse ();

  for (size_t i = 0; i < clouds.size (); i++)
  {
    CloudPtr tmp (new Cloud);

    Eigen::Vector3f t2;
    t2[0] = t[0] * weights[0][i];
    t2[1] = t[1] * weights[1][i];
    t2[2] = t[2] * weights[2][i];

    Eigen::Quaternionf q2;
    q2 = Eigen::Quaternionf::Identity ().slerp (weights[3][i], q);

    //TODO use rotation from branch start
    Eigen::Translation3f t3 (t2);
    Eigen::Affine3f a (t3 * q2);
    a = aend * a * aendI;

    //std::cout << "transform cloud " << i << " to:" << std::endl << a.matrix () << std::endl;
    pcl::transformPointCloud (*(clouds[i].second), *(clouds[i].second), a);
  }
}

void
loopDetection (int end, const CloudVector &clouds, double dist)
{
  static double min_dist = -1;
  static int first, last;
  int state = 0;

  for (int i = end-1; i > 0; i--)
  {
    Eigen::Vector4f cstart, cend;
    //TODO use pose of scan
    pcl::compute3DCentroid (*(clouds[i].second), cstart);
    pcl::compute3DCentroid (*(clouds[end].second), cend);
    Eigen::Vector4f diff = cend - cstart;

    double norm = diff.norm ();

    //std::cout << "distance between " << i << " and " << end << " is " << norm << " state is " << state << std::endl;

    if (state == 0 && norm > dist)
    {
      state = 1;
      //std::cout << "state 1" << std::endl;
    }
    if (state > 0 && norm < dist)
    {
      state = 2;
      std::cout << "loop detected between scan " << i << " (" << clouds[i].first << ") and scan " << end << " (" << clouds[end].first << ")" << std::endl;
      if (min_dist < 0 || norm < min_dist)
      {
        min_dist = norm;
        first = i;
        last = end;
      }
    }
  }
  std::cout << "min_dist: " << min_dist << " state: " << state << " first: " << first << " end: " << end << std::endl;
  if (min_dist > 0 && (state < 2 || end == (int)clouds.size () - 1)) //TODO
  {
    min_dist = -1;
    std::cout << "calling elch with " << first << " and " << last << std::endl;
    elch (first, last, clouds);
    std::cout << "finished calling elch" << std::endl;
  }
}

int
main (int argc, char **argv)
{
  CloudVector clouds;
  for (int i = 1; i < argc; i++)
  {
    CloudPtr pc (new Cloud);
    pcl::io::loadPCDFile (argv[i], *pc);
    clouds.push_back (CloudPair (argv[i], pc));
    std::cout << "loading file: " << argv[i] << " size: " << pc->size () << std::endl;
  }

  for (size_t i = 0; i < clouds.size (); i++)
    loopDetection (i, clouds, 15.0);

  for (size_t i = 0; i < clouds.size (); i++)
  {
    std::string result_filename (clouds[i].first);
    result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
    pcl::io::savePCDFileBinary (result_filename.c_str (), *(clouds[i].second));
    cout << "saving result to " << result_filename << endl;
  }

  return 0;
}
<commit_msg>disabled graphviz (includes deprecated headers)<commit_after>#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/transforms.h>
#include <pcl/registration/icp.h>

//#include <boost/graph/graphviz.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/graph_traits.hpp>
using boost::graph_traits;

#include <boost/graph/adjacency_list.hpp>
using boost::edge_weight_t;

#include <iostream>
using std::cout;
using std::endl;
#include <string>
using std::string;
#include <fstream>
using std::ofstream;

#include <vector>
using std::vector;

#include <list>
using std::list;

#include <algorithm>
using std::swap;

typedef pcl::PointXYZ PointType;
typedef pcl::PointCloud<PointType> Cloud;
typedef Cloud::ConstPtr CloudConstPtr;
typedef Cloud::Ptr CloudPtr;
typedef std::pair<std::string, CloudPtr> CloudPair;
typedef std::vector<CloudPair> CloudVector;

typedef boost::adjacency_list<
  boost::listS, boost::vecS, boost::undirectedS,
  boost::no_property,
  boost::property< edge_weight_t, double > >
Graph;

/**
 * graph balancer algorithm computes the weights
 * @param g the graph
 * @param f index of the first node
 * @param l index of the last node
 * @param weights array for the weights
 */
void
loopOptimizerAlgorithm (Graph &g, int f, int l, double *weights)
{
  list<int> crossings, branches;
  crossings.push_back (f);
  crossings.push_back (l);
  weights[f] = 0;
  weights[l] = 1;

  int *p = new int[num_vertices (g)];
  int *p_min = new int[num_vertices (g)];
  double *d = new double[num_vertices (g)];
  double *d_min = new double[num_vertices (g)];
  double dist;
  bool do_swap = false;
  list<int>::iterator crossings_it, end_it, start_min, end_min;

  // process all junctions
  while (!crossings.empty ())
  {
    dist = -1;
    // find shortest crossing for all vertices on the loop
    for (crossings_it = crossings.begin (); crossings_it != crossings.end (); )
    {
      dijkstra_shortest_paths (g, *crossings_it, boost::predecessor_map (p).distance_map (d));
      end_it = crossings_it;
      end_it++;
      // find shortest crossing for one vertex
      for (; end_it != crossings.end (); end_it++)
      {
        if (*end_it != p[*end_it] && (dist < 0 || d[*end_it] < dist))
        {
          dist = d[*end_it];
          start_min = crossings_it;
          end_min = end_it;
          do_swap = true;
        }
      }
      if (do_swap)
      {
        swap (p, p_min);
        swap (d, d_min);
        do_swap = false;
      }
      // vertex starts a branch
      if (dist < 0)
      {
        branches.push_back (*crossings_it);
        crossings_it = crossings.erase (crossings_it);
      }
      else
        crossings_it++;
    }

    if (dist > -1)
    {
      remove_edge (*end_min, p_min[*end_min], g);
      for (int i = p_min[*end_min]; i != *start_min; i = p_min[i])
      {
        //even right with weights[*start_min] > weights[*end_min]! (math works)
        weights[i] = weights[*start_min] + (weights[*end_min] - weights[*start_min]) * d_min[i] / d_min[*end_min];
        remove_edge (i, p_min[i], g);
        if (degree (i, g) > 0)
        {
          crossings.push_back (i);
        }
      }

      if (degree (*start_min, g) == 0)
        crossings.erase (start_min);

      if (degree (*end_min, g) == 0)
        crossings.erase (end_min);
    }
  }

  delete[] p;
  delete[] p_min;
  delete[] d;
  delete[] d_min;

  graph_traits<Graph>::adjacency_iterator adjacent_it, adjacent_it_end;
  int s;

  // error propagation
  while (!branches.empty ())
  {
    s = branches.front ();
    branches.pop_front ();

    for (tie (adjacent_it, adjacent_it_end) = adjacent_vertices (s, g); adjacent_it != adjacent_it_end; ++adjacent_it)
    {
      weights[*adjacent_it] = weights[s];
      if (degree (*adjacent_it, g) > 1)
        branches.push_back (*adjacent_it);
    }
    clear_vertex (s, g);
  }
}

void
elch (int start, int end, const CloudVector &clouds)
{
  CloudPtr tmp (new Cloud);

  Eigen::Vector4f cstart;
  pcl::compute3DCentroid (*(clouds[start].second), cstart);
  Eigen::Vector3f cs2 (cstart[0], cstart[1], cstart[2]);

  Eigen::Vector4f cend2;
  pcl::compute3DCentroid (*(clouds[end].second), cend2);
  Eigen::Vector3f ce2 (cend2[0], cend2[1], cend2[2]);
  ce2 = cs2 - ce2;
  pcl::transformPointCloud (*(clouds[end].second), *tmp, ce2, Eigen::Quaternionf::Identity ());

  pcl::IterativeClosestPoint<PointType, PointType> icp;

  icp.setMaximumIterations (50);
  icp.setMaxCorrespondenceDistance (1.5);
  icp.setRANSACOutlierRejectionThreshold (1.5);
  //icp.setRANSACOutlierRejectionThreshold (DBL_MAX);

  icp.setInputTarget (clouds[start].second);

  icp.setInputCloud (tmp);

  icp.align (*tmp);

  Eigen::Matrix3f m (icp.getFinalTransformation ().block (0, 0, 3, 3));
  Eigen::Quaternionf q (m);
  Eigen::Vector4f ta (icp.getFinalTransformation ().col (3));
  Eigen::Vector3f t (ta[0], ta[1], ta[2]);
  //TODO hack
  t += ce2;

  std::cout << icp.getFinalTransformation () << std::endl;

  static Graph g;

  for (size_t i = num_vertices (g)+1; i < clouds.size (); i++)
    add_edge (i-1, i, g);

  Graph grb[4];

  graph_traits<Graph>::edge_iterator edge_it, edge_it_end;
  for (tie (edge_it, edge_it_end) = edges (g); edge_it != edge_it_end; edge_it++)
  {
    for (int j = 0; j < 4; j++)
      add_edge (source (*edge_it, g), target (*edge_it, g), 1, grb[j]);  //TODO add variance
  }

  double *weights[4];
  for (int i = 0; i < 4; i++)
  {
    weights[i] = new double[clouds.size ()];
    loopOptimizerAlgorithm (grb[i], start, end, weights[i]);
  }

  //TODO use pose
  Eigen::Vector4f cend;
  pcl::compute3DCentroid (*(clouds[end].second), cend);
  Eigen::Translation3f tend (cend[0], cend[1], cend[2]);
  Eigen::Affine3f aend (tend);
  Eigen::Affine3f aendI = aend.inverse ();

  for (size_t i = 0; i < clouds.size (); i++)
  {
    CloudPtr tmp (new Cloud);

    Eigen::Vector3f t2;
    t2[0] = t[0] * weights[0][i];
    t2[1] = t[1] * weights[1][i];
    t2[2] = t[2] * weights[2][i];

    Eigen::Quaternionf q2;
    q2 = Eigen::Quaternionf::Identity ().slerp (weights[3][i], q);

    //TODO use rotation from branch start
    Eigen::Translation3f t3 (t2);
    Eigen::Affine3f a (t3 * q2);
    a = aend * a * aendI;

    //std::cout << "transform cloud " << i << " to:" << std::endl << a.matrix () << std::endl;
    pcl::transformPointCloud (*(clouds[i].second), *(clouds[i].second), a);
  }
}

void
loopDetection (int end, const CloudVector &clouds, double dist)
{
  static double min_dist = -1;
  static int first, last;
  int state = 0;

  for (int i = end-1; i > 0; i--)
  {
    Eigen::Vector4f cstart, cend;
    //TODO use pose of scan
    pcl::compute3DCentroid (*(clouds[i].second), cstart);
    pcl::compute3DCentroid (*(clouds[end].second), cend);
    Eigen::Vector4f diff = cend - cstart;

    double norm = diff.norm ();

    //std::cout << "distance between " << i << " and " << end << " is " << norm << " state is " << state << std::endl;

    if (state == 0 && norm > dist)
    {
      state = 1;
      //std::cout << "state 1" << std::endl;
    }
    if (state > 0 && norm < dist)
    {
      state = 2;
      std::cout << "loop detected between scan " << i << " (" << clouds[i].first << ") and scan " << end << " (" << clouds[end].first << ")" << std::endl;
      if (min_dist < 0 || norm < min_dist)
      {
        min_dist = norm;
        first = i;
        last = end;
      }
    }
  }
  std::cout << "min_dist: " << min_dist << " state: " << state << " first: " << first << " end: " << end << std::endl;
  if (min_dist > 0 && (state < 2 || end == (int)clouds.size () - 1)) //TODO
  {
    min_dist = -1;
    std::cout << "calling elch with " << first << " and " << last << std::endl;
    elch (first, last, clouds);
    std::cout << "finished calling elch" << std::endl;
  }
}

int
main (int argc, char **argv)
{
  CloudVector clouds;
  for (int i = 1; i < argc; i++)
  {
    CloudPtr pc (new Cloud);
    pcl::io::loadPCDFile (argv[i], *pc);
    clouds.push_back (CloudPair (argv[i], pc));
    std::cout << "loading file: " << argv[i] << " size: " << pc->size () << std::endl;
  }

  for (size_t i = 0; i < clouds.size (); i++)
    loopDetection (i, clouds, 15.0);

  for (size_t i = 0; i < clouds.size (); i++)
  {
    std::string result_filename (clouds[i].first);
    result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
    pcl::io::savePCDFileBinary (result_filename.c_str (), *(clouds[i].second));
    cout << "saving result to " << result_filename << endl;
  }

  return 0;
}
<|endoftext|>