text: #include "ros/ros.h"
  #include "actionlib/server/simple_action_server.h"
  #include "generate_traj/TrajAction.h"
  #include "plan_and_run/demo_application.h"
  class TrajAction
  {
  protected:
  
    ros::NodeHandle nh_; 
    // NodeHandle instance must be created before this line. 
    // Otherwise strange error occurs.
    actionlib::SimpleActionServer<generate_traj::TrajAction> as_;
    std::string action_name_;
    // create messages that are used to published feedback/result
    generate_traj::TrajFeedback feedback_;
    generate_traj::TrajResult result_;
    // creating application from plan_and_run
    plan_and_run::DemoApplication application;
    plan_and_run::DescartesTrajectory traj;
    plan_and_run::DescartesTrajectory output_path;
  
  public:
  
    TrajAction(std::string name) :
      as_(nh_, name, boost::bind(&TrajAction::executeCB, this, _1), false),
      action_name_(name)
    {
      as_.start();
    }
  
    ~TrajAction(void)
    {
    }
  
    // Define the callback function
    void executeCB(const generate_traj::TrajGoalConstPtr &goal)
    {
      // helper variables
      ros::Rate r(1);
      bool success = true;
  
      // Pre-set the feedback(add 0 to the feedback sequence here)
      feedback_.sequence.clear();
      feedback_.sequence.push_back(0);
  
      // Publish info to the console for the user
      // Test the goal
      ROS_INFO("%s: Executing, starting for our goal ...... ", action_name_.c_str());
  
      // Start executing the action
      for(int i=1; i<=10; i++)
      {
        // check that preempt has not been requested by the client
        if (as_.isPreemptRequested() || !ros::ok())
        {
          ROS_INFO("%s: Preempted", action_name_.c_str());
          // set the action state to preempted
          as_.setPreempted();
          success = false;
          break;
        }
        
        ROS_INFO("Working on it ......");
  
        // Upate the feedback msg
        feedback_.sequence.push_back(i);
        // publish the feedback
        as_.publishFeedback(feedback_);
  
        // this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
        r.sleep();
      }
      
      // Try to figure out a way 
      // avoid initialize every time in the callback function,
      // but for now, we can keep this for test the generate function
      ROS_INFO("Loading and initializing now!");
      // loading parameters
      application.loadParameters();
      // initializing ros components
      application.initRos();
      // initializing descartes
      application.initDescartes();
  
      ROS_INFO("All is well! Everyone is happy! You can start planning now!");
      // moving to home position
      application.moveHome();
      // generating trajectory
      EigenSTL::vector_Affine3d poses;
      Eigen::Affine3d pose;
      int i;
      poses.clear();
      for(i=0;i<goal->x.size();i++)
      {
        Eigen::Matrix3d rot;
        rot <<0, 1, 0
            ,1, 0, 0
            ,0, 0, -1;
  
        pose = Eigen::Translation3d(goal->offset[0] + goal->x[i],
                                    goal->offset[1] + goal->y[i],
                                    goal->offset[2] + goal->z[i]) * rot;
  
        poses.push_back(pose);
      }
      ROS_INFO_STREAM("Trajectory with "<<poses.size()<<" points was generated");
      ROS_INFO("Got the goal waypoints! Start planning now!");
  
      using namespace plan_and_run;
      using namespace descartes_core;
      using namespace descartes_trajectory;
      DescartesTrajectory traj;
      // creating descartes trajectory points
      traj.clear();
      traj.reserve(poses.size());
      for(unsigned int i = 0; i < poses.size(); i++)
      {
        const Eigen::Affine3d& pose = poses[i];
  
        /*  Fill Code:
        * Goal:
        * - Create AxialSymetricPt objects in order to define a trajectory cartesian point with
        *    rotational freedom about the tool's z axis.
        *
        * Hint:
        * - The point can be constructed as follows:
        *
        *    new AxialSymmetricPt(Pose ,Increment, Free Axis)
        *
        * - The Pose can be found in the for loop's "pose" variable.
        * - The Increment can be found in the "ORIENTATION_INCREMENT" global variable.
        * - The Free Axis can be selected from the AxialSymmetricPt::FreeAxis::Z_AXIS enumeration constants.
        *
        */
        //descartes_core::TrajectoryPtPtr pt = descartes_core::TrajectoryPtPtr(/*[ COMPLETE HERE*/);
  
        descartes_core::TrajectoryPtPtr pt = descartes_core::TrajectoryPtPtr(
            new descartes_trajectory::AxialSymmetricPt(pose,ORIENTATION_INCREMENT,
                                                      descartes_trajectory::AxialSymmetricPt::FreeAxis::Z_AXIS,
                                                        descartes_core::TimingConstraint(0.5) ) );
  
        // saving points into trajectory
        traj.push_back(pt);
      }
      ROS_INFO("Generate traj completed!");
      //application.generateTrajectory(traj);
  
      // planning robot path
      application.planPath(traj,output_path);
      // running robot path
      application.runPath(output_path);
      ros::Duration(10).sleep();
      
  
      if(success)
      {
        result_.result = 1;
        ROS_INFO("%s: Succeeded. Everyone is happy!", action_name_.c_str());
        // set the action state to succeeded
        as_.setSucceeded(result_);
      }
    }
  };
  
  
  int main(int argc, char** argv)
  {
    // Setup the server
    ros::init(argc, argv, "traj_action_server");
    TrajAction traj_action("traj_action");
    // Info
    ROS_INFO("Waiting for the client ......");
    ros::spin();
    return 0;
  }
  