Demo rosbag record code

verticle_approach_test.cpp

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, University of Colorado, Boulder
 *  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 the Univ of CO, Boulder 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: Dave Coleman
   Desc:   Generates a trajectory message that moves the end effector vertically. Used for testing PIDs
*/

// ROS
#include <ros/ros.h>
#include <ros/package.h> // for getting file path of package
#include <actionlib/client/simple_action_client.h>

// ROSBag
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <rosbag_record_cpp/rosbag_record.h>

// TF
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

// Msgs
#include <geometry_msgs/PoseArray.h>

// Eigen
#include <Eigen/Core>
#include <Eigen/Geometry>

// MoveIt
#include <moveit_msgs/RobotState.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
//#include <moveit/plan_execution/plan_execution.h>
//#include <moveit/plan_execution/plan_with_sensing.h>
//#include <moveit/trajectory_processing/trajectory_tools.h> // for plan_execution
//#include <moveit/trajectory_processing/iterative_time_parameterization.h>

// Grasp generation
#include <moveit_simple_grasps/simple_grasps.h>

// Baxter specific properties
#include <moveit_simple_grasps/grasp_data.h>
//#include <baxter_pick_place/custom_environment2.h>

// Baxter Utilities
#include <baxter_control/baxter_utilities.h>
#include <baxter_moveit_scripts/baxter_move_group_interface.h>

// Boost
#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>

namespace baxter_pick_place
{

// Class
class VerticleApproachTest
{
private:

  // A shared node handle
  ros::NodeHandle nh_;


  // MoveIt Components
  boost::shared_ptr<tf::TransformListener> tf_;
  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
  //trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_;
  //boost::shared_ptr<plan_execution::PlanExecution> plan_execution_;

  // class for publishing stuff to rviz
  moveit_visual_tools::MoveItVisualToolsPtr visual_tools_;

  // data for generating grasps
  moveit_simple_grasps::GraspData grasp_data_;

  // baxter helper
  baxter_control::BaxterUtilities baxter_util_;
  baxter_pick_place::BaxterMoveGroupInterfacePtr baxter_move_;

  // baxter data recorder
  rosbag_record_cpp::ROSBagRecord baxter_record_;

  // which baxter arm are we using
  std::string arm_;
  std::string planning_group_name_;

  // where to save trajectory files
  const std::string package_path_;
  std::string file_path_;
  int file_id_; // incremental file naming

  // Loaded/created trajectory to execute
  moveit_msgs::RobotTrajectory trajectory_msg_; // the resulting path
  moveit_msgs::RobotTrajectory reverse_trajectory_msg_;
     

public:

  // Constructor
  VerticleApproachTest()
    : arm_("right"),
      planning_group_name_(arm_+"_arm"),
      package_path_(ros::package::getPath( "baxter_experimental" ) + "/"),
      baxter_record_(), 
      file_path_("/home/dave/ros/ws_baxter/src/baxter_experimental/verticle_approach_matlab/data/"),
      file_id_(0),
      nh_("~")
  {
    static const std::string PLANNING_GROUP = "right_arm";

    mkdir(file_path_.c_str(), 0700);

    // ---------------------------------------------------------------------------------------------
    // Load grasp data specific to our robot
    if (!grasp_data_.loadRobotGraspData(nh_, arm_+"_hand"))
      ros::shutdown();

    // ---------------------------------------------------------------------------------------------
    // Create planning scene monitor
    if(!loadPlanningSceneMonitor())
    {
      ROS_ERROR_STREAM_NAMED("verticle_test","Unable to load planning scene monitor");
      return;
    }

    // ---------------------------------------------------------------------------------------------
    // Create trajectory execution manager
    /*
    if( !trajectory_execution_manager_ )
    {
      // Create a trajectory execution manager
      trajectory_execution_manager_.reset(new trajectory_execution_manager::TrajectoryExecutionManager
        (planning_scene_monitor_->getRobotModel()));
      plan_execution_.reset(new plan_execution::PlanExecution(planning_scene_monitor_, trajectory_execution_manager_));
    } 
    */

    // ---------------------------------------------------------------------------------------------
    // Load the Robot Viz Tools for publishing to Rviz
    visual_tools_.reset(new moveit_visual_tools::MoveItVisualTools(grasp_data_.base_link_));

    // Load the move_group_inteface
    baxter_move_.reset(new baxter_pick_place::BaxterMoveGroupInterface(PLANNING_GROUP, planning_scene_monitor_));

    // ---------------------------------------------------------------------------------------------
    // Enable baxter
    if( !baxter_util_.enableBaxter() )
      return;

    // ---------------------------------------------------------------------------------------------
    // Create start pose
    geometry_msgs::Pose start_pose = createStartPose();

    // ---------------------------------------------------------------------------------------------
    // Move into start position
    ROS_INFO_STREAM_NAMED("verticle_test","Sending arm to start position ----------------------------------");
    const moveit_msgs::PlanningScene planning_scene_diff;
    if( !baxter_move_->sendToPose(start_pose, visual_tools_, planning_scene_diff))
    {
      ROS_ERROR_STREAM_NAMED("verticle_test","Failed to go to start position");
    }
    else
    {
      ROS_INFO_STREAM_NAMED("verticle_test","Start position succeeded");
    }
    std::cout << "before sleep " << std::endl;
    ros::Duration(30).sleep();

    // ---------------------------------------------------------------------------------------------
    // Run the trajectory
    runVerticleTrajectory(start_pose);

    ROS_INFO_STREAM_NAMED("verticle_test","Success! Waiting 5 sec before shutting down.");
    ros::Duration(5).sleep();

    // Shutdown
    //baxter_util_.disableBaxter();
  }

  ~VerticleApproachTest()
  {
  }

  bool runVerticleTrajectory(const geometry_msgs::Pose& start_pose)
  {
    // Compute the trajectory if we haven't already saved it to a rosbag file

    // Check if the file exists
    std::string trajectory_file = package_path_ + "trajectory/cached_trajectory.bag";
    rosbag::Bag trajectory_bag;
    if (boost::filesystem::exists(trajectory_file))
    {
      ROS_INFO_STREAM_NAMED("verticle_test","Loading trajectory from file.");
      trajectory_bag.open(trajectory_file, rosbag::bagmode::Read);
      std::vector<std::string> topics;

      // Get the forward trajectory
      topics.clear();
      topics.push_back(std::string("trajectory"));
      rosbag::View view(trajectory_bag, rosbag::TopicQuery(topics));

      BOOST_FOREACH(rosbag::MessageInstance const msg_instance, view)
      {
        moveit_msgs::RobotTrajectory::ConstPtr traj = msg_instance.instantiate<moveit_msgs::RobotTrajectory>();
        if (traj == NULL)
          ROS_ERROR_STREAM_NAMED("verticle_test","Problem reading file");
        trajectory_msg_ = *traj;
      }

      // Get reverse trajectory
      topics.clear();
      topics.push_back(std::string("reverse_trajectory"));
      rosbag::View view2(trajectory_bag, rosbag::TopicQuery(topics));

      BOOST_FOREACH(rosbag::MessageInstance const msg_instance, view2)
      {
        moveit_msgs::RobotTrajectory::ConstPtr traj = msg_instance.instantiate<moveit_msgs::RobotTrajectory>();
        if (traj == NULL)
          ROS_ERROR_STREAM_NAMED("verticle_test","Problem reading file");
        reverse_trajectory_msg_ = *traj;
      }
    }
    else
    {
      ROS_INFO_STREAM_NAMED("verticle_test","Creating new trajectory because no cached_trajectory.bag file found.");

      // Create the trajectory ourselves
      if( !createVerticleTrajectory(start_pose) )
      {
        ROS_ERROR_STREAM_NAMED("verticle_test","Unable to create trajectory");
        return false;
      }

      // Save the trajectory to file
      trajectory_bag.open(trajectory_file, rosbag::bagmode::Write);

      // Write
      trajectory_bag.write("trajectory", ros::Time::now(), trajectory_msg_);
      trajectory_bag.write("reverse_trajectory", ros::Time::now(), reverse_trajectory_msg_);
    }
    trajectory_bag.close();

    bool runTrajectory = true;

    // Create options for our rosbag recorder
    rosbag_record_cpp::RecorderOptions opts;
    opts.topics.push_back("/robot/"+arm_+"_velocity_trajectory_controller/state");
    //opts.topics.push_back("/robot/"+arm_+"_position_trajectory_controller/state");
    opts.append_date = false;
  
    while(ros::ok() && file_id_ < 10)
    {
      // -------------------------------------------------------------------------------------------
      ROS_INFO_STREAM_NAMED("verticle_test","Lowering down " << file_id_ << "--------------------------------------");

      // Display the generated path
      //if( !visual_tools_->publishTrajectoryPath(trajectory_msg_, !runTrajectory ) )
      //  return false;

      // Record the path
      opts.prefix = file_path_ + "test" + boost::to_string(file_id_); // name of file to output to    
      file_id_++;
      baxter_record_.startRecording(opts);

      // Execute the path
      if(runTrajectory)
      {
        baxter_move_->executeTrajectoryMsg(trajectory_msg_);
      }

      ros::Duration(1).sleep();

      // -------------------------------------------------------------------------------------------
      ROS_INFO_STREAM_NAMED("verticle_test","Raising up " << file_id_ << "-----------------------------------------");

      // Display the generated path
      //if( !visual_tools_->publishTrajectoryPath(reverse_trajectory_msg_, !runTrajectory ) )
      //  return false;

      // Execute the path
      if(runTrajectory)
      {
        baxter_move_->executeTrajectoryMsg(reverse_trajectory_msg_);
      }

      // Save the recorded path to file
      baxter_record_.stopRecording();

      ros::Duration(1).sleep();
    }

    // ---------------------------------------------------------------------------------------------
    // Demo will automatically reset arm
    ROS_INFO_STREAM_NAMED("verticle_test","Finished ------------------------------------------------");
  }

  bool createVerticleTrajectory(const geometry_msgs::Pose& start_pose)
  {
    double desired_approach_distance = 0.4; // The distance the origin of a robot link needs to travel
    Eigen::Vector3d approach_direction; // Approach direction (negative z axis)
    approach_direction << 0, 0, -1;

    if( !computeStraightLinePath(approach_direction, desired_approach_distance, trajectory_msg_,
        reverse_trajectory_msg_) )
    {
      ROS_ERROR_STREAM_NAMED("verticle_test","Failed to generate straight line path");
      return false;
    }

    return true;
  }

  /**
   * \brief Function for testing multiple directions
   * \param approach_direction - direction to move end effector straight
   * \param desired_approach_distance - distance the origin of a robot link needs to travel
   * \param trajectory_msg - resulting path
   * \return true on success
   */
  bool computeStraightLinePath( Eigen::Vector3d approach_direction, double desired_approach_distance,
    moveit_msgs::RobotTrajectory& trajectory_msg, moveit_msgs::RobotTrajectory& reverse_trajectory_msg )
  {
    // ---------------------------------------------------------------------------------------------
    // Get planning scene
    const planning_scene::PlanningScenePtr planning_scene = planning_scene_monitor_->getPlanningScene();
    robot_state::RobotState approach_state = planning_scene->getCurrentState();

    // Output state info
    //approach_state.printStateInfo();
    //approach_state.printTransforms();

    // ---------------------------------------------------------------------------------------------
    // Settings for computeCartesianPath

    // End effector parent link
    const std::string &ik_link = grasp_data_.ee_parent_link_; 
    const moveit::core::LinkModel *ik_link_model = approach_state.getLinkModel(ik_link);

    // Joint model group
    const moveit::core::JointModelGroup *joint_model_group
      = approach_state.getJointModelGroup(planning_group_name_);

    // Resolution of trajectory
    double max_step = 0.001; // The maximum distance in Cartesian space between consecutive points on the resulting path

    // Jump threshold for preventing consequtive joint values from 'jumping' by a large amount in joint space
    double jump_threshold = 0.0; // disabled

    // ---------------------------------------------------------------------------------------------
    // Check for kinematic solver
    if( !joint_model_group->canSetStateFromIK( ik_link ) )
    {
      // Set kinematic solver
      const std::pair<robot_model::JointModelGroup::KinematicsSolver,
        robot_model::JointModelGroup::KinematicsSolverMap>& allocators =
        approach_state.getJointModelGroup(planning_group_name_)->getGroupKinematics();
      //const std::pair<robot_model::SolverAllocatorFn, robot_model::SolverAllocatorMapFn> &allocators =
      //approach_state.getJointModelGroup(planning_group_name_)->getSolverAllocators();
      if( !allocators.first)
      {
        ROS_ERROR_STREAM_NAMED("verticle_test","No IK Solver loaded - make sure moveit_config/kinamatics.yaml is loaded in this namespace");
        return false;
      }
    }

    // -----------------------------------------------------------------------------------------------
    // Compute Cartesian Path
    ROS_INFO_STREAM_NAMED("verticle_test","Preparing to computer cartesian path");

    std::vector<robot_state::RobotStatePtr> robot_state_trajectory; // create resulting generated trajectory (result)

    double d_approach =
      approach_state.computeCartesianPath(
        joint_model_group,
        robot_state_trajectory,
        ik_link_model,
        approach_direction,
        true,                      // direction is in global reference frame
        desired_approach_distance,
        max_step,
        jump_threshold
      );

    ROS_INFO_STREAM("Approach distance: " << d_approach );

    if( d_approach == 0 )
    {
      ROS_ERROR_STREAM_NAMED("verticle_test","Failed to computer cartesian path: distance is 0");
      return false;
    }

    // -----------------------------------------------------------------------------------------------
    // Get current RobotState  (in order to specify all joints not in robot_state_trajectory)
    //robot_state::RobotState this_robot_state = planning_scene->getCurrentState();

    // -----------------------------------------------------------------------------------------------
    // Smooth the path and add velocities/accelerations
    //const std::vector<moveit_msgs::JointLimits> &joint_limits = joint_model_group->getVariableLimits();

    // Copy the vector of RobotStates to a RobotTrajectory
    robot_trajectory::RobotTrajectoryPtr robot_trajectory(new robot_trajectory::RobotTrajectory(
        planning_scene->getRobotModel(), planning_group_name_));

    for (std::size_t k = 0 ; k < robot_state_trajectory.size() ; ++k)
      robot_trajectory->addSuffixWayPoint(robot_state_trajectory[k], 0.0);

    // Perform iterative parabolic smoothing
    trajectory_processing::IterativeParabolicTimeParameterization iterative_smoother;
    iterative_smoother.computeTimeStamps( *robot_trajectory );
    /*                                         robot_trajectory,
                                               trajectory_out,
                                               joint_limits,
                                               this_robot_state // start_state
                                               );
    */

    // Convert trajectory to a message
    robot_trajectory->getRobotTrajectoryMsg(trajectory_msg);

    ROS_INFO_STREAM("New trajectory:\n" << trajectory_msg);

    // ----------------------------------------------------------------------
    // Reverse the trajectory
    robot_trajectory::RobotTrajectoryPtr robot_trajectory_reverse(new robot_trajectory::RobotTrajectory(
        planning_scene->getRobotModel(), planning_group_name_));

    for (std::size_t k = 0 ; k < robot_state_trajectory.size() ; ++k)
      robot_trajectory_reverse->addPrefixWayPoint(robot_state_trajectory[k], 0.0);

    // Perform iterative parabolic smoothing
    iterative_smoother.computeTimeStamps( *robot_trajectory_reverse );

    // Convert trajectory to a message
    robot_trajectory_reverse->getRobotTrajectoryMsg(reverse_trajectory_msg);


    return true;
  }

  /**
   * \brief Execute planned trajectory
   * \param trajectory_msg
   * \return true if successful
   */
  /*
  bool executeTrajectoryMsg(const moveit_msgs::RobotTrajectory& trajectory_msg)
  {
    ROS_INFO_STREAM_NAMED("verticle_test","Executing trajectory");

    plan_execution_->getTrajectoryExecutionManager()->clear();
    if(plan_execution_->getTrajectoryExecutionManager()->push(trajectory_msg))
    {
      plan_execution_->getTrajectoryExecutionManager()->execute();

      // wait for the trajectory to complete
      moveit_controller_manager::ExecutionStatus es = plan_execution_->getTrajectoryExecutionManager()->waitForExecution();
      if (es == moveit_controller_manager::ExecutionStatus::SUCCEEDED)
        ROS_INFO_STREAM_NAMED("verticle_test","Trajectory execution succeeded");
      else
      {
        if (es == moveit_controller_manager::ExecutionStatus::PREEMPTED)
          ROS_INFO_STREAM_NAMED("verticle_test","Trajectory execution preempted");
        else
          if (es == moveit_controller_manager::ExecutionStatus::TIMED_OUT)
            ROS_INFO_STREAM_NAMED("verticle_test","Trajectory execution timed out");
          else
            ROS_INFO_STREAM_NAMED("verticle_test","Trajectory execution control failed");
        return false;
      }
    }
    else
    {
      ROS_ERROR_STREAM_NAMED("verticle_test","Failed to push trajectory");
      return false;
    }

    return true;
  }
  */



  /**
   * \brief Create the location for the end effector to start at
   * \return the pose
   */
  geometry_msgs::Pose createStartPose()
  {
    geometry_msgs::Pose start_pose;

    // Position
    start_pose.position.x = 0.6;
    start_pose.position.z = 0.2;
    if( arm_.compare("right") == 0 ) // equal
      start_pose.position.y = -0.4;
    else
      start_pose.position.y = 0.3;

    // Orientation
    double angle = M_PI;
    Eigen::Quaterniond quat(Eigen::AngleAxis<double>(double(angle), Eigen::Vector3d::UnitY()));
    start_pose.orientation.x = quat.x();
    start_pose.orientation.y = quat.y();
    start_pose.orientation.z = quat.z();
    start_pose.orientation.w = quat.w();

    return start_pose;
  }

  /**
   * \brief Load a planning scene monitor
   * \return true if successful in loading
   */
  bool loadPlanningSceneMonitor()
  {
    // ---------------------------------------------------------------------------------------------
    // Create planning scene monitor
    tf_.reset(new tf::TransformListener());
    planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor("robot_description", tf_));

    ros::spinOnce();
    ros::Duration(0.1).sleep();
    ros::spinOnce();

    if (planning_scene_monitor_->getPlanningScene())
    {
      //planning_scene_monitor_->startWorldGeometryMonitor();
      //planning_scene_monitor_->startSceneMonitor("/move_group/monitored_planning_scene");
      planning_scene_monitor_->startStateMonitor("/joint_states", "/attached_collision_object");
      //planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
      //"dave_planning_scene");
    }
    else
    {
      ROS_FATAL_STREAM_NAMED("verticle_test","Planning scene not configured");
      return false;
    }

    ros::spinOnce();
    ros::Duration(0.1).sleep();
    ros::spinOnce();

    // Wait for complete state to be recieved
    std::vector<std::string> missing_joints;
    int counter = 0;
    while( !planning_scene_monitor_->getStateMonitor()->haveCompleteState() && ros::ok() )
    {
      ROS_INFO_STREAM_NAMED("verticle_test","Waiting for complete state...");
      ros::Duration(0.25).sleep();
      ros::spinOnce();

      // Show unpublished joints
      if( counter > 6 )
      {
        planning_scene_monitor_->getStateMonitor()->haveCompleteState( missing_joints );
        for(int i = 0; i < missing_joints.size(); ++i)
          ROS_WARN_STREAM_NAMED("verticle_test","Unpublished joints: " << missing_joints[i]);
      }
      counter++;
    }

    return true;
  }

}; // end of class

} // namespace

int main(int argc, char** argv)
{
  ROS_INFO_STREAM_NAMED("verticle_test","Verticle Approach Test");

  ros::init(argc, argv, "verticle_approach_test");

  // Allow the action server to recieve and send ros messages
  //ros::AsyncSpinner spinner(4);
  //spinner.start();

  baxter_pick_place::VerticleApproachTest tester;

  ROS_INFO_STREAM_NAMED("verticle_test","Shutting down.");

  return 0;
}