update_logger.h

Go to the documentation of this file.

#ifndef INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_UPDATE_FILTERS_UPDATE_LOGGER_H_
#define INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_UPDATE_FILTERS_UPDATE_LOGGER_H_

#include <stomp_moveit/update_filters/stomp_update_filter.h>
#include <fstream>

namespace stomp_moveit
{
namespace update_filters
{

class UpdateLogger : public StompUpdateFilter
{
public:
  UpdateLogger();
  virtual ~UpdateLogger();

  virtual bool initialize(moveit::core::RobotModelConstPtr robot_model_ptr,
                          const std::string& group_name,const XmlRpc::XmlRpcValue& config);

  virtual bool configure(const XmlRpc::XmlRpcValue& config);

  virtual bool setMotionPlanRequest(const planning_scene::PlanningSceneConstPtr& planning_scene,
                   const moveit_msgs::MotionPlanRequest &req,
                   const stomp_core::StompConfiguration &config,
                   moveit_msgs::MoveItErrorCodes& error_code);

  virtual bool filter(std::size_t start_timestep,
                      std::size_t num_timesteps,
                      int iteration_number,
                      const Eigen::MatrixXd& parameters,
                      Eigen::MatrixXd& updates,
                      bool& filtered) override;

  virtual std::string getGroupName() const override
  {
    return group_name_;
  }

  virtual std::string getName() const override
  {
    return name_ + "/" + group_name_;
  }

  virtual void done(bool success, int total_iterations,double final_cost,const Eigen::MatrixXd& parameters) override;

protected:

  std::string name_;
  std::string group_name_;

  // parameters
  std::string filename_;
  std::string package_;
  std::string directory_;

  // config
  stomp_core::StompConfiguration stomp_config_;

  // logging
  std::stringstream stream_;
  std::string full_file_name_;
  std::ofstream file_stream_;
  Eigen::IOFormat format_;

};

} /* namespace smoothers */
} /* namespace stomp_moveit */

#endif /* INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_UPDATE_FILTERS_UPDATE_LOGGER_H_ */