gazebo_bag_plugin.h

Go to the documentation of this file.

/*
 * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
 * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
 *
 * Licensed 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.
 */


#ifndef ROTORS_GAZEBO_PLUGINS_GAZEBO_BAG_PLUGIN_H
#define ROTORS_GAZEBO_PLUGINS_GAZEBO_BAG_PLUGIN_H

#include <string>

#include <gazebo/common/common.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/WrenchStamped.h>
#include <mav_msgs/Actuators.h>
#include <mav_msgs/AttitudeThrust.h>
#include <mav_msgs/default_topics.h>
#include <mav_msgs/RateThrust.h>
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Float32.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>

#include "rotors_comm/RecordRosbag.h"
#include "rotors_comm/WindSpeed.h"
#include "rotors_gazebo_plugins/common.h"


namespace gazebo {

// Default values, the rest from common.h
static const std::string kDefaultFrameId = "ground_truth_pose";
static const std::string kDefaultLinkName = "base_link";
static const std::string kDefaultBagFilename_ = "simulator.bag";
static const std::string kDefaultRecordingServiceName = "record_rosbag";
static constexpr bool kDefaultWaitToRecord = false;
static constexpr bool kDefaultIsRecording = false;

class GazeboBagPlugin : public ModelPlugin {
  typedef std::map<const unsigned int, const physics::JointPtr> MotorNumberToJointMap;
  typedef std::pair<const unsigned int, const physics::JointPtr> MotorNumberToJointPair;
 public:
  GazeboBagPlugin()
      : ModelPlugin(),
        namespace_(kDefaultNamespace),
        // DEFAULT TOPICS
        ground_truth_pose_topic_(mav_msgs::default_topics::GROUND_TRUTH_POSE),
        ground_truth_twist_topic_(mav_msgs::default_topics::GROUND_TRUTH_TWIST),
        imu_topic_(mav_msgs::default_topics::IMU),
        control_attitude_thrust_topic_(mav_msgs::default_topics::COMMAND_ATTITUDE_THRUST),
        control_motor_speed_topic_(mav_msgs::default_topics::COMMAND_ACTUATORS),
        control_rate_thrust_topic_(mav_msgs::default_topics::COMMAND_RATE_THRUST),
        wind_speed_topic_(mav_msgs::default_topics::WIND_SPEED),
        motor_topic_(mav_msgs::default_topics::MOTOR_MEASUREMENT),
        wrench_topic_(mav_msgs::default_topics::WRENCH),
        external_force_topic_(mav_msgs::default_topics::EXTERNAL_FORCE),
        waypoint_topic_(mav_msgs::default_topics::COMMAND_TRAJECTORY),
        command_pose_topic_(mav_msgs::default_topics::COMMAND_POSE),
        //---------------
        frame_id_(kDefaultFrameId),
        link_name_(kDefaultLinkName),
        bag_filename_(kDefaultBagFilename_),
        recording_service_name_(kDefaultRecordingServiceName),
        rotor_velocity_slowdown_sim_(kDefaultRotorVelocitySlowdownSim),
        wait_to_record_(kDefaultWaitToRecord),
        is_recording_(kDefaultIsRecording),
        node_handle_(nullptr),
        contact_mgr_(nullptr) {}

  virtual ~GazeboBagPlugin();

 protected:
  void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);

  void OnUpdate(const common::UpdateInfo& /*_info*/);

  void StartRecording();

  void StopRecording();

  void ImuCallback(const sensor_msgs::ImuConstPtr& imu_msg);

  void ExternalForceCallback(const geometry_msgs::WrenchStampedConstPtr& force_msg);

  void WaypointCallback(const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& trajectory_msg);

  void CommandPoseCallback(const geometry_msgs::PoseStampedConstPtr& pose_msg);

  void AttitudeThrustCallback(const mav_msgs::AttitudeThrustConstPtr& control_msg);

  void ActuatorsCallback(const mav_msgs::ActuatorsConstPtr& control_msg);

  void RateThrustCallback(const mav_msgs::RateThrustConstPtr& control_msg);

  void WindSpeedCallback(const rotors_comm::WindSpeedConstPtr& wind_speed_msg);

  void LogGroundTruth(const common::Time now);

  void LogMotorVelocities(const common::Time now);

  void LogWrenches(const common::Time now);

  bool RecordingServiceCallback(rotors_comm::RecordRosbag::Request& req,
                                rotors_comm::RecordRosbag::Response& res);

 private:
  event::ConnectionPtr update_connection_;

  physics::WorldPtr world_;
  physics::ModelPtr model_;
  physics::LinkPtr link_;

  physics::Link_V child_links_;

  MotorNumberToJointMap motor_joints_;

  physics::ContactManager *contact_mgr_;

  std::string namespace_;
  std::string ground_truth_pose_topic_;
  std::string ground_truth_twist_topic_;
  std::string imu_topic_;
  std::string external_force_topic_;
  std::string waypoint_topic_;
  std::string command_pose_topic_;
  std::string control_attitude_thrust_topic_;
  std::string control_motor_speed_topic_;
  std::string control_rate_thrust_topic_;
  std::string wind_speed_topic_;
  std::string wrench_topic_;
  std::string motor_topic_;
  std::string frame_id_;
  std::string link_name_;
  std::string bag_filename_;
  std::string recording_service_name_;
  double rotor_velocity_slowdown_sim_;

  boost::mutex mtx_;

  bool wait_to_record_;

  bool is_recording_;

  rosbag::Bag bag_;
  ros::NodeHandle *node_handle_;

  // Ros subscribers
  ros::Subscriber imu_sub_;
  ros::Subscriber external_force_sub_;
  ros::Subscriber waypoint_sub_;
  ros::Subscriber control_attitude_thrust_sub_;
  ros::Subscriber control_motor_speed_sub_;
  ros::Subscriber control_rate_thrust_sub_;
  ros::Subscriber wind_speed_sub_;
  ros::Subscriber command_pose_sub_;

  // Ros service server
  ros::ServiceServer recording_service_;

  std::ofstream csvOut;

  template<class T>
  void writeBag(const std::string& topic, const ros::Time& time, const T& msg) {
    boost::mutex::scoped_lock lock(mtx_);
    try {
      bag_.write(topic, time, msg);
    }
    catch (rosbag::BagIOException& e) {
      gzerr << "Error while writing to bag " << e.what() << std::endl;
    }
    catch (rosbag::BagException& e) {
      if (time < ros::TIME_MIN) {
        gzerr<<"Header stamp not set for msg published on topic: "<< topic << ". " << e.what() << std::endl;
      }
      else {
        gzerr << "Error while writing to bag " << e.what() << std::endl;
      }
    }
  }

  template<class T>
  void writeBag(const std::string& topic, const ros::Time& time, boost::shared_ptr<T const> const& msg) {
    boost::mutex::scoped_lock lock(mtx_);
    try {
      bag_.write(topic, time, msg);
    }
    catch (rosbag::BagIOException& e) {
      gzerr << "Error while writing to bag " << e.what() << std::endl;
    }
    catch (rosbag::BagException& e) {
      if (time < ros::TIME_MIN) {
        gzerr<<"Header stamp not set for msg published on topic: "<< topic << ". " << e.what() << std::endl;
      }
      else {
        gzerr << "Error while writing to bag " << e.what() << std::endl;
      }
    }
  }

};

} // namespace gazebo

#endif // ROTORS_GAZEBO_PLUGINS_GAZEBO_BAG_PLUGIN_H