Issues with Spin() and AsyncSpinner: multithreaded application

edit

Hello the following code gives me this error: SingleThreadedSpinner: You've attempted to call spin from multiple threads. Use a MultiThreadedSpinner instead.

And this warning: AsyncSpinnerImpl: Attempt to start() an AsyncSpinner failed because another AsyncSpinner is already running. Note that the other AsyncSpinner might not be using the same callback queue as this AsyncSpinner, in which case no callbacks in your callback queue will be serviced.

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
#include <moveit/move_group_interface/move_group.h>
#include <sstream>
#include "std_msgs/String.h"
#include "std_msgs/Float32.h"
#include "std_msgs/Float32MultiArray.h"
#include "geometry_msgs/Pose.h"
#include "moveit_msgs/RobotTrajectory.h"
#include "sensor_msgs/JointState.h"
#include "trajectory_msgs/JointTrajectoryPoint.h"
#define foreach BOOST_FOREACH

// subscribe to the point topic: This callback takes a geometry pose, plans to it, and publishes the times and waypoints to a motionPlan topic
void automaticCallback(const geometry_msgs::Pose::ConstPtr& msg)
{
  ros::AsyncSpinner spinner(4);
  spinner.start();

  std::string jointgroup = "sia5dArm";
  move_group_interface::MoveGroup group(jointgroup);

  moveit::planning_interface::MoveGroup::Plan my_plan;
  group.setPoseTarget(*msg);
  bool success = group.plan(my_plan);

    //write the trajectory to a bag file to use later
    rosbag::Bag bag;
    bag.open("test.bag", rosbag::bagmode::Write);
    bag.write("my_plan_trajectory", ros::Time::now(), my_plan.trajectory_);
    bag.close();

  ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");

  // Convert robot trajectory to a sensor_msg, for complicated reasons
  if (success == true)
  {
    std::vector<trajectory_msgs::JointTrajectoryPoint> trajectory_points;
    trajectory_points = my_plan.trajectory_.joint_trajectory.points;


    std::vector<int>::size_type points_number = trajectory_points.size();
    std::vector<double> motionPlan_msg;
    std::vector<double> motionPlan_times;

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

      double trajectory_time = ros::Duration(trajectory_points[i].time_from_start).toSec();

      motionPlan_times.push_back(trajectory_time); 

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

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

    // publish the motionPlan in sensor message format to a motionPlan topic    
    ros::NodeHandle n;
    ros::Publisher motionPlan_pub = n.advertise<sensor_msgs::JointState>("motionPlan",1000);
    motionPlan_pub.publish(motionPlan_position);
    ros::spin();
  }
  ros::waitForShutdown();
}

// subscribe to the command topic, based on the command the robot will either be automatically controlled or it will look in a bag file for a motionPlan to execute
void commandCallback(const std_msgs::String::ConstPtr& msg)
{
  std::string command = msg->data;

  if(command == "automatic")
  {
    ros::NodeHandle n;

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

    ros::spin();

    ROS_INFO("automatic control");
  }
  else if(command == "execute")
  {
    std::string jointgroup = "sia5dArm";
    move_group_interface::MoveGroup group(jointgroup);

    // read in motion Plan from a bag file so it can be executed.
    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");

          moveit::planning_interface::MoveGroup::Plan my_plan2;
          my_plan2.trajectory_ = *s;
            group.execute(my_plan2);

          ROS_INFO("plan executed");

            ros::NodeHandle n;
        ros::Publisher motionPlan_pub2 = n.advertise<moveit_msgs::RobotTrajectory>("motionPlan_trajectory",1000);
        motionPlan_pub2.publish(my_plan2.trajectory_);

        ros::spin();

        }
    }
    bag.close();
  }
}


//****************************
//----------------------------
//Primary ...