Hello! I am trying to create a node that does that following:

• receives a command to tell what type of planning to do (automatic, manual, waypoints, etc...)
• plans the path
• sends the trajectory to a topic motionPlan so that the user can make a decision as to whether or not to proceed/execute the plan (some type of preview)
• receive a command to execute the plan.

I think the problem is that I want the motionPlan that was found to stay around until the user can decide to execute it. And since I can't subscribe to two messages in one callback function, i tried creating a class to do this. I haven't had any luck getting it to work and I am not really sure my structure is correct. Any input would be helpful. The automaticCallback was working fine prior to enabling execution via logic. The other functions I haven't written yet.

Thanks!

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group.h>
#include "std_msgs/String.h"
#include "std_msgs/Float32.h"
#include "std_msgs/Float32MultiArray.h"
#include "geometry_msgs/Pose.h"
#include "trajectory_msgs/JointTrajectoryPoint.h"
#include "moveit_msgs/RobotTrajectory.h"
#include "sensor_msgs/JointState.h"
#include <sstream>


class automaticClass
{
  private:
    move_group_interface::MoveGroup group("sia5dArm");
    moveit::planning_interface::MoveGroup::Plan my_plan;

  public:
    void executeCallback(const std_msgs::String::ConstPtr& msg);
    void automaticCallback(const geometry_msgs::Pose::ConstPtr& msg);
};


void automaticClass::executeCallback(const std_msgs::String::ConstPtr& msg)
{
  std::string executeCommand = msg->data;
  if(executeCommand == "execute")
  {
    group.execute(my_plan);
  }
  else
  {
    ROS_INFO("not executed");
  }
}


// subscribe to the point topic
void automaticClass::automaticCallback(const geometry_msgs::Pose::ConstPtr& msg)
{

  ros::AsyncSpinner spinner(1);
  spinner.start();

  group.setPoseTarget(*msg);

  ROS_INFO("test");
  bool success = group.plan(my_plan);
  ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");

  if (success == true)
  {

    std::vector<trajectory_msgs::JointTrajectoryPoint> trajectory_points;
    trajectory_points = my_plan.trajectory_.joint_trajectory.points;

    std::vector<int>::size_type size1 = trajectory_points.size();

    std::vector<double> motionPlan_msg;

    for (unsigned i = 0; i<size1; i++)
    {
      std::vector<int>::size_type size2 = trajectory_points[i].positions.size();


      for (unsigned j = 0; j<size2; j++)
      {
        motionPlan_msg.push_back(trajectory_points[i].positions[j]);

      } 
    }

    sensor_msgs::JointState motionPlan_position;
    motionPlan_position.position = motionPlan_msg;

    ros::NodeHandle n;
    ros::Publisher motionPlan_pub = n.advertise<sensor_msgs::JointState>("motionPlan",1000);
    motionPlan_pub.publish(motionPlan_position);
    ros::spin();

  }

    ros::NodeHandle n;

    ros::Subscriber sub2 = n.subscribe("execute",1000,executeCallback);
  // plan the motion and then move the group to the sampled target
  // group.move();

  ros::waitForShutdown();
}



void waypointsCallback(const geometry_msgs::Pose::ConstPtr& msg)
{

}


// subscribe to the command topic
void commandCallback(const std_msgs::String::ConstPtr& msg)
{

  std::string command = msg->data;
  //std::cout<<command<<std::endl;
  if(command == "automatic")
  {
    ros::NodeHandle n;

    ros::Subscriber sub2 = n.subscribe("point",1000,automaticClass::automaticCallback);

    ROS_INFO("automatic control");
  }
  else if(command == "waypoints")
  {
    ros::NodeHandle n;

    ros::Subscriber sub3 = n.subscribe("point",1000,waypointsCallback);

    ROS_INFO("waypoint control");
  }
}

//Primary Node Function
int main(int argc, char **argv)
{
  ros::init(argc, argv, "planner");

  ros::NodeHandle n;

  ros::Subscriber sub1 = n.subscribe("command",1000,commandCallback);

  ros::spin();

  return 0;

}

The error I am getting looks like this:

error: expected identifier before string constant
     move_group_interface::MoveGroup group("sia5dArm");

which to me seems like my structure may be wrong.

    rosbag::Bag bag;
bag.open("test.bag", rosbag::bagmode::Write);
bag.write("my_plan_trajectory", ros::Time::now(), my_plan.trajectory_);
bag.close();

rosbag::Bag bag;
bag.open("test.bag", rosbag::bagmode::Read);

std::vector<std::string> topics;
topics.push_back(std::string("my_plan_trajectory"));
rosbag::View view(bag, rosbag::TopicQuery(topics));

foreach(rosbag::MessageInstance const m, view)
{
    moveit_msgs::RobotTrajectory::ConstPtr s = m.instantiate<moveit_msgs::RobotTrajectory>();
    if (s != NULL)
    {
        ROS_INFO("executing command");
        //execute a trajectory
    }
}
bag.close();