Skip to content

Commit

Permalink
add move the common files to a lib and reformat using clang-format
Browse files Browse the repository at this point in the history
  • Loading branch information
@weblucas
weblucas committed Aug 6, 2018
1 parent 3d80ccb commit b2aaa9d
Show file tree
Hide file tree
Showing 10 changed files with 379 additions and 397 deletions.
19 changes: 19 additions & 0 deletions .clang-format
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
---
Language: Cpp
BasedOnStyle: Google
DerivePointerAlignment: false
PointerAlignment: Left
ColumnLimit: 80
AllowShortFunctionsOnASingleLine: Empty
AllowShortIfStatementsOnASingleLine: false
AllowShortLoopsOnASingleLine: false
AlignAfterOpenBracket: AlwaysBreak
IncludeCategories:
- Regex: '^<.*'
Priority: 1
- Regex: '.*'
Priority: 2
---
Language: Proto
BasedOnStyle: Google
...
6 changes: 5 additions & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,11 @@ catkin_simple()
FILE(GLOB_RECURSE LibFiles "include/*.h")
add_custom_target(headers SOURCES ${LibFiles})

cs_add_executable(ros_backend_node src/main.cc src/ros_backend_node.cc src/simple_waypoint_planner.cc)

cs_add_library(visensor_sim_backend src/ros_backend_node.cc )

cs_add_executable(ros_backend_node src/main.cc src/simple_waypoint_planner.cc)
target_link_libraries(ros_backend_node ${catkin_LIBRARIES} visensor_sim_backend)

cs_add_library(poltergeist_gimbal_plugin src/gazebo_poltergeist_gimbal_plugin.cpp)
target_link_libraries(poltergeist_gimbal_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
Expand Down
20 changes: 7 additions & 13 deletions include/visensor_simulator/builtin_planner.h
View file
Original file line number Diff line number Diff line change
@@ -1,21 +1,15 @@
#ifndef VISENSOR_SIMULATOR_BUILTIN_PLANNER_H
#define VISENSOR_SIMULATOR_BUILTIN_PLANNER_H

#include <string>
#include <nav_msgs/Odometry.h>
#include <string>

class BuiltInPlanner{
public:
enum PlannerStatus
{
INVALID,
STARTING,
RUNNING,
COMPLETED
};
class BuiltInPlanner {
public:
enum PlannerStatus { INVALID, STARTING, RUNNING, COMPLETED };

virtual PlannerStatus getStatus()=0;
virtual bool loadConfigurationFromFile(const std::string& filepath )=0;
virtual PlannerStatus getStatus() = 0;
virtual bool loadConfigurationFromFile(const std::string& filepath) = 0;
};

#endif //VISENSOR_SIMULATOR_BUILTIN_PLANNER_H
#endif // VISENSOR_SIMULATOR_BUILTIN_PLANNER_H
250 changes: 115 additions & 135 deletions include/visensor_simulator/logger.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,156 +2,136 @@
#ifndef VISENSOR_SIMULATOR_LOGGER_H
#define VISENSOR_SIMULATOR_LOGGER_H

#include <vector>
#include <fstream>
#include <ros/time.h>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#include <fstream>
#include <ros/time.h>
#include <vector>

struct ImuSensorReadings {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

ImuSensorReadings()
: gyroscopes(),
accelerometers(),timestamp(0) {

}

ImuSensorReadings(ros::Time timestamp_, Eigen::Vector3d gyroscopes_,
Eigen::Vector3d accelerometers_)
: gyroscopes(gyroscopes_),
accelerometers(accelerometers_),timestamp(timestamp_) {
}
Eigen::Vector3d gyroscopes; ///< Gyroscope measurement.
Eigen::Vector3d accelerometers; ///< Accelerometer measurement.
ros::Time timestamp;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

ImuSensorReadings() : gyroscopes(), accelerometers(), timestamp(0) {}

ImuSensorReadings(
ros::Time timestamp_, Eigen::Vector3d gyroscopes_,
Eigen::Vector3d accelerometers_)
: gyroscopes(gyroscopes_),
accelerometers(accelerometers_),
timestamp(timestamp_) {}
Eigen::Vector3d gyroscopes; ///< Gyroscope measurement.
Eigen::Vector3d accelerometers; ///< Accelerometer measurement.
ros::Time timestamp;
};

struct PoseReadings {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Eigen::Vector3d position;
Eigen::Quaterniond orientation;
ros::Time timestamp;

EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Eigen::Vector3d position;
Eigen::Quaterniond orientation;
ros::Time timestamp;
};


std::ostream& operator<<(std::ostream& os, const PoseReadings& T)
{
Eigen::Vector3d p_WS_W = T.position;
Eigen::Quaterniond q_WS = T.orientation;
os << T.timestamp.toNSec() << "," << std::scientific
<< std::setprecision(18) << p_WS_W[0] << "," << p_WS_W[1] << ","
<< p_WS_W[2] << "," << q_WS.x() << "," << q_WS.y() << ","
<< q_WS.z() << "," << q_WS.w();
return os;
std::ostream& operator<<(std::ostream& os, const PoseReadings& T) {
Eigen::Vector3d p_WS_W = T.position;
Eigen::Quaterniond q_WS = T.orientation;
os << T.timestamp.toNSec() << "," << std::scientific << std::setprecision(18)
<< p_WS_W[0] << "," << p_WS_W[1] << "," << p_WS_W[2] << "," << q_WS.x()
<< "," << q_WS.y() << "," << q_WS.z() << "," << q_WS.w();
return os;
}

std::ostream& operator<<(std::ostream& os, const ImuSensorReadings& T)
{
Eigen::Vector3d acc = T.accelerometers;
Eigen::Vector3d gyro = T.gyroscopes;
os << T.timestamp.toNSec() << "," << std::scientific
<< std::setprecision(18) << acc[0] << "," << acc[1] << ","
<< acc[2] << "," << gyro[0] << "," << gyro[1] << ","
<< gyro[2];
return os;
std::ostream& operator<<(std::ostream& os, const ImuSensorReadings& T) {
Eigen::Vector3d acc = T.accelerometers;
Eigen::Vector3d gyro = T.gyroscopes;
os << T.timestamp.toNSec() << "," << std::scientific << std::setprecision(18)
<< acc[0] << "," << acc[1] << "," << acc[2] << "," << gyro[0] << ","
<< gyro[1] << "," << gyro[2];
return os;
}


class Logger{
public:
Logger():is_valid_(false){}
~Logger(){stop();}

bool startLogger( std::string output_folder )
{
if( !output_folder.empty() && output_folder.at(output_folder.length()-1) != '/' )
output_folder+="/";

std::string imu_filename = output_folder + "imu_data.csv";
std::string pose_filename = output_folder + "pose_data.csv";
//ROS_INFO_STREAM(" " << imu_filename);
imu_file.open(imu_filename);
pose_file.open(pose_filename);

if(imu_file.is_open() && pose_file.is_open())
{
is_valid_ = true;
}
else
{
is_valid_ = false;
imu_file.close();
pose_file.close();
}

imu_data.clear();
imu_data.reserve(100000);
pose_data.clear();
pose_data.reserve(100000);

return is_valid_;
class Logger {
public:
Logger() : is_valid_(false) {}
~Logger() {
stop();
}

bool startLogger(std::string output_folder) {
if (!output_folder.empty() &&
output_folder.at(output_folder.length() - 1) != '/')
output_folder += "/";

std::string imu_filename = output_folder + "imu_data.csv";
std::string pose_filename = output_folder + "pose_data.csv";
// ROS_INFO_STREAM(" " << imu_filename);
imu_file.open(imu_filename);
pose_file.open(pose_filename);

if (imu_file.is_open() && pose_file.is_open()) {
is_valid_ = true;
} else {
is_valid_ = false;
imu_file.close();
pose_file.close();
}

bool logPose( const PoseReadings & measurement)
{
if(!is_valid_)
return false;

pose_data.push_back( measurement );
return true;
}

bool logIMU( const ImuSensorReadings & measurement )
{
if(!is_valid_)
return false;

imu_data.push_back( measurement );
return true;
}

void stop()
{
if(!is_valid_)
return;

serializeImu();
serializePoses();

is_valid_ = false;
imu_file.close();
pose_file.close();

}

private:
bool is_valid_;
std::vector<ImuSensorReadings,Eigen::aligned_allocator<ImuSensorReadings>> imu_data;
std::vector<PoseReadings,Eigen::aligned_allocator<PoseReadings>> pose_data;

std::ofstream imu_file;
std::ofstream pose_file;

void serializeImu()
{
imu_file << "timestamp, acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z" << std::endl;
for (const auto& reading : imu_data)
imu_file << reading << std::endl;
}


void serializePoses()
{
pose_file << "timestamp, p_x, p_y,p_z, q_x, q_y,q_z, q_w" << std::endl;
for (const auto& reading : pose_data)
pose_file << reading << std::endl;
}



imu_data.clear();
imu_data.reserve(100000);
pose_data.clear();
pose_data.reserve(100000);

return is_valid_;
}

bool logPose(const PoseReadings& measurement) {
if (!is_valid_)
return false;

pose_data.push_back(measurement);
return true;
}

bool logIMU(const ImuSensorReadings& measurement) {
if (!is_valid_)
return false;

imu_data.push_back(measurement);
return true;
}

void stop() {
if (!is_valid_)
return;

serializeImu();
serializePoses();

is_valid_ = false;
imu_file.close();
pose_file.close();
}

private:
bool is_valid_;
std::vector<ImuSensorReadings, Eigen::aligned_allocator<ImuSensorReadings>>
imu_data;
std::vector<PoseReadings, Eigen::aligned_allocator<PoseReadings>> pose_data;

std::ofstream imu_file;
std::ofstream pose_file;

void serializeImu() {
imu_file << "timestamp, acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z"
<< std::endl;
for (const auto& reading : imu_data)
imu_file << reading << std::endl;
}

void serializePoses() {
pose_file << "timestamp, p_x, p_y,p_z, q_x, q_y,q_z, q_w" << std::endl;
for (const auto& reading : pose_data)
pose_file << reading << std::endl;
}
};


#endif
Loading

0 comments on commit b2aaa9d

Please sign in to comment.