Skip to content

Commit

Permalink
Revamped code; removed old code and most debug messages; separated PF…
Browse files Browse the repository at this point in the history 
… and PFPublisher; added compiler optimization
  • Loading branch information
Guilherme Lawless committed Jul 4, 2017
1 parent 2f258fa commit 87be2c9
Show file tree
Hide file tree
Showing 10 changed files with 626 additions and 668 deletions.
63 changes: 35 additions & 28 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,14 @@ cmake_minimum_required(VERSION 2.8.3)

project(pfuclt_omni_dataset)

#set(CMAKE_BUILD_TYPE Debug)
IF(NOT CMAKE_BUILD_TYPE)
SET(CMAKE_BUILD_TYPE Release)
ENDIF()

MESSAGE("Build type: " ${CMAKE_BUILD_TYPE})

set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3")

OPTION(USE_OpenMP "Use OpenMP" ON)
IF(USE_OpenMP)
Expand All @@ -16,8 +23,8 @@ ENDIF()

set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/CMakeModules")
find_package(
cmake_modules REQUIRED
catkin REQUIRED dynamic_reconfigure
cmake_modules REQUIRED
catkin REQUIRED dynamic_reconfigure
)

#external includes
Expand All @@ -27,56 +34,56 @@ INCLUDE_DIRECTORIES(include)
add_library(minicsv src/minicsv.cpp)

find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
tf
geometry_msgs
nav_msgs
message_generation
tf2
tf2_ros
dynamic_reconfigure
)
roscpp
rospy
std_msgs
tf
geometry_msgs
nav_msgs
message_generation
tf2
tf2_ros
dynamic_reconfigure
)

FIND_PACKAGE(read_omni_dataset REQUIRED)
INCLUDE_DIRECTORIES(${read_omni_dataset_INCLUDE_DIRS})

generate_dynamic_reconfigure_options(
config/Dynamic.cfg
config/Dynamic.cfg
)

INCLUDE_DIRECTORIES(${catkin_INCLUDE_DIRS} include)

## Generate messages in the 'msg' folder
add_message_files(
FILES
particle.msg
particles.msg
FILES
particle.msg
particles.msg
)

generate_messages(
DEPENDENCIES
geometry_msgs
DEPENDENCIES
geometry_msgs
)

catkin_package(
INCLUDE_DIRS include
# CATKIN_DEPENDS roscpp rospy
#CATKIN_DEPENDS roscpp rospy message_runtime read_omni_dataset
# DEPENDS system_lib
INCLUDE_DIRS include
# CATKIN_DEPENDS roscpp rospy
CATKIN_DEPENDS std_msgs roscpp read_omni_dataset
# DEPENDS system_lib
)

FIND_PACKAGE(Eigen3 REQUIRED)
INCLUDE_DIRECTORIES(${Eigen3_INCLUDE_DIRS})

find_package(Boost COMPONENTS filesystem REQUIRED)
find_package(Boost REQUIRED)
include_directories(${Boost_INCLUDE_DIRS})

set(HEADER_FILES include/pfuclt_omni_dataset/pfuclt_aux.h include/pfuclt_omni_dataset/pfuclt_omni_dataset.h include/pfuclt_omni_dataset/pfuclt_particles.h)
set(SOURCE_FILES src/pfuclt_omni_dataset.cpp src/pfuclt_aux.cpp src/pfuclt_particles.cpp)
set(HEADER_FILES include/pfuclt_omni_dataset/pfuclt_aux.h include/pfuclt_omni_dataset/pfuclt_omni_dataset.h include/pfuclt_omni_dataset/pfuclt_particles.h include/pfuclt_omni_dataset/pfuclt_publisher.h)
set(SOURCE_FILES src/pfuclt_omni_dataset.cpp src/pfuclt_aux.cpp src/pfuclt_particles.cpp src/pfuclt_publisher.cpp)

add_executable(pfuclt_omni_dataset ${HEADER_FILES} ${SOURCE_FILES})
target_compile_options(pfuclt_omni_dataset PRIVATE ${OpenMP_FLAGS})
add_dependencies(pfuclt_omni_dataset pfuclt_omni_dataset_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(pfuclt_omni_dataset pfuclt_omni_dataset_generate_messages_cpp pfuclt_omni_dataset_gencfg ${catkin_EXPORTED_TARGETS})
target_link_libraries(pfuclt_omni_dataset ${catkin_LIBRARIES} ${rosbag_LIBRARIES} ${Eigen3_LIBRARIES} ${Boost_LIBRARIES} ${read_omni_dataset_LIBRARIES} minicsv ${OpenMP_LIBS})
17 changes: 9 additions & 8 deletions include/pfuclt_omni_dataset/pfuclt_aux.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,14 +4,13 @@
#include <ros/ros.h>
#include <vector>
#include <fstream>
#include <pfuclt_omni_dataset/pfuclt_aux.h>
#include <boost/ref.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>

namespace pfuclt_aux
namespace pfuclt_omni_dataset
{
/**
* @brief The ORDER_TYPE enum - use to define an ascending or descending
Expand Down Expand Up @@ -46,13 +45,13 @@ std::vector<Landmark> getLandmarks(const char* filename);
* @param vec - vector of type T with values to calculate std.dev
* @return standard deviation as a double
*/
template <typename T> double calc_stdDev(const std::vector<T>& vec)
template <typename T> T calc_stdDev(const std::vector<T>& vec)
{
using namespace boost::accumulators;

accumulator_set<T, stats<tag::variance> > acc;
std::for_each(vec.begin(), vec.end(), boost::bind<void>(boost::ref(acc), _1));
return (double)sqrt(extract::variance(acc));
return (T)sqrt(extract::variance(acc));
}

/**
Expand All @@ -66,7 +65,7 @@ template <typename T> double calc_stdDev(const std::vector<T>& vec)
*/
template <typename T>
std::vector<unsigned int> order_index(std::vector<T> const& values,
const ORDER_TYPE order = pfuclt_aux::DESC)
const ORDER_TYPE order = DESC)
{
// from http://stackoverflow.com/a/10585614 and modified
// return sorted indices of vector values
Expand All @@ -77,7 +76,7 @@ std::vector<unsigned int> order_index(std::vector<T> const& values,
std::vector<unsigned int> indices(values.size());
int i = 0;
std::transform(values.begin(), values.end(), indices.begin(), ref(i)++);
if (order == pfuclt_aux::DESC)
if (order == DESC)
{
std::sort(indices.begin(), indices.end(),
ref(values)[arg1] > ref(values)[arg2]);
Expand Down Expand Up @@ -149,8 +148,10 @@ bool readParam(ros::NodeHandle& nh, const std::string name,
ROS_INFO("%s", oss.str().c_str());
return true;
}
else
else {
ROS_ERROR("Failed to receive parameter %s", name.c_str());
return false;
}
}

/**
Expand Down Expand Up @@ -196,7 +197,7 @@ typedef struct timeEval_s

}TimeEval;

// end of namespace
// end of namespace pfuclt_omni_dataset
}

#endif // PFUCLT_AUX_H
8 changes: 4 additions & 4 deletions include/pfuclt_omni_dataset/pfuclt_omni_dataset.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,11 +23,11 @@
// Auxiliary libraries
#include <pfuclt_omni_dataset/pfuclt_aux.h>
#include <pfuclt_omni_dataset/pfuclt_particles.h>
#include <pfuclt_omni_dataset/pfuclt_publisher.h>

namespace pfuclt
namespace pfuclt_omni_dataset
{
using namespace pfuclt_ptcls;
#define NUM_WEIGHT 1

#define STATES_PER_ROBOT 3
#define HEURISTICS_THRESH_DEFAULT \
{ \
Expand Down Expand Up @@ -137,5 +137,5 @@ class Robot
bool hasStarted() { return started_; }
};

// end of namespace
// end of namespace pfuclt_omni_dataset
}
119 changes: 9 additions & 110 deletions include/pfuclt_omni_dataset/pfuclt_particles.h
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#ifndef PARTICLES_H
#define PARTICLES_H

#include <ros/ros.h>
#include <pfuclt_omni_dataset/pfuclt_aux.h>

#include <vector>
Expand All @@ -11,19 +12,13 @@
#include <boost/thread/mutex.hpp>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#include <tf2_ros/transform_broadcaster.h>

#include <read_omni_dataset/RobotState.h>
#include <read_omni_dataset/LRMGTData.h>
#include <read_omni_dataset/Estimate.h>
#include <pfuclt_omni_dataset/particle.h>
#include <pfuclt_omni_dataset/particles.h>

#include <dynamic_reconfigure/server.h>
#include <pfuclt_omni_dataset/DynamicConfig.h>

#include <geometry_msgs/Vector3.h>

#define NUM_ALPHAS 4

// ideally later this will be a parameter, when it makes sense to
Expand Down Expand Up @@ -52,11 +47,8 @@

//#define MORE_DEBUG true

namespace pfuclt_ptcls
namespace pfuclt_omni_dataset
{

using namespace pfuclt_aux;

typedef float pdata_t;

typedef double (*estimatorFunc)(const std::vector<double>&,
Expand Down Expand Up @@ -97,7 +89,7 @@ class ParticleFilter
{
private:
boost::mutex mutex_;
dynamic_reconfigure::Server<pfuclt_omni_dataset::DynamicConfig>
dynamic_reconfigure::Server<DynamicConfig>
dynamicServer_;

protected:
Expand All @@ -122,8 +114,8 @@ class ParticleFilter
*/
struct State
{
uint nRobots;
uint nStatesPerRobot;
uint nRobots;

/**
* @brief The robotState_s struct - saves information on the belief of a
Expand Down Expand Up @@ -172,10 +164,10 @@ class ParticleFilter
*/
struct PFinitData
{
ros::NodeHandle& nh;
const uint mainRobotID, nTargets, statesPerRobot, nRobots, nLandmarks;
const std::vector<bool>& robotsUsed;
const std::vector<Landmark>& landmarksMap;
ros::NodeHandle& nh;

/**
* @brief PFinitData
Expand Down Expand Up @@ -209,17 +201,17 @@ class ParticleFilter
ros::NodeHandle& nh_;
uint nParticles_;
const uint mainRobotID_;
const std::vector<Landmark>& landmarksMap_;
const std::vector<bool>& robotsUsed_;
const uint nTargets_;
const uint nRobots_;
const uint nStatesPerRobot_;
const uint nRobots_;
const uint nSubParticleSets_;
const uint nLandmarks_;
particles_t particles_;
particles_t weightComponents_;
RNGType seed_;
bool initialized_;
const std::vector<Landmark>& landmarksMap_;
const std::vector<bool>& robotsUsed_;
std::vector<std::vector<LandmarkObservation> > bufLandmarkObservations_;
std::vector<TargetObservation> bufTargetObservations_;
TimeEval targetIterationTime_, odometryTime_;
Expand Down Expand Up @@ -554,99 +546,6 @@ class ParticleFilter
void saveAllTargetMeasurementsDone(const uint robotNumber);
};

/**
* @brief The PFPublisher class - implements publishing for the ParticleFilter
* class using ROS
*/
class PFPublisher : public ParticleFilter
{
public:
struct PublishData
{
float robotHeight;

/**
* @brief PublishData - contains information necessary for the PFPublisher
* class
* @param nh - the node handle object
* @param robotHeight - the fixed robot height
*/
PublishData(float robotHeight) : robotHeight(robotHeight) {}
};

/**
* @brief resize_particles - change to a different number of particles and
* resize the publishing message
* @param n - the desired number of particles
*/
void resize_particles(const uint n)
{
// Call base class method
ParticleFilter::resize_particles(n);

ROS_INFO("Resizing particle message");

// Resize particle message
msg_particles_.particles.resize(n);
for (uint p = 0; p < n; ++p)
{
msg_particles_.particles[p].particle.resize(nSubParticleSets_);
}
}

private:
ros::Subscriber GT_sub_;
ros::Publisher estimatePublisher_, particlePublisher_,
targetEstimatePublisher_, targetGTPublisher_, targetParticlePublisher_;
std::vector<ros::Publisher> particleStdPublishers_;
std::vector<ros::Publisher> robotGTPublishers_;
std::vector<ros::Publisher> robotEstimatePublishers_;
ros::Publisher targetObservationsPublisher_;

read_omni_dataset::LRMGTData msg_GT_;
pfuclt_omni_dataset::particles msg_particles_;
read_omni_dataset::Estimate msg_estimate_;

std::vector<tf2_ros::TransformBroadcaster> robotBroadcasters;

struct PublishData pubData;

void publishParticles();
void publishRobotStates();
void publishTargetState();
void publishEstimate();
void publishGTData();
void publishTargetObservations();

public:
/**
* @brief PFPublisher - constructor
* @param data - a structure with the necessary initializing data for the
* ParticleFilter class
* @param publishData - a structure with some more data for this class
*/
PFPublisher(struct ParticleFilter::PFinitData& data,
struct PublishData publishData);

/**
* @brief getPFReference - retrieve a reference to the base class's members
* @remark C++ surely is awesome
* @return returns a reference to the base ParticleFilter for this object
*/
ParticleFilter* getPFReference() { return (ParticleFilter*)this; }

/**
* @brief gtDataCallback - callback of ground truth data
*/
void gtDataCallback(const read_omni_dataset::LRMGTData::ConstPtr&);

/**
* @brief nextIteration - extends the base class method to add the ROS
* publishing
*/
void nextIteration();
};

// end of namespace pfuclt_ptcls
// end of namespace pfuclt_omni_dataset
}
#endif // PARTICLES_H
Loading

0 comments on commit 87be2c9

Please sign in to comment.