ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Issues with Spin() and AsyncSpinner: multithreaded application

asked 2018-07-12 19:00:39 -0500

Levi_Manring gravatar image

updated 2018-07-17 10:31:36 -0500

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-07-13 01:55:55 -0500

updated 2018-07-14 04:49:40 -0500

There are a few things I can see that stand out. Firstly can you post more of your code, this snippet is incomplete and even has unbalanced parentheses.

The code you've shown appears to mostly be a callback function, inside this callback you're starting the asyncspinner as well as a normal spin at the end. Neither of these make any sense being inside a callback and could result in very strange recursive behaviours.

Your callback seems to create a motion plan and publish a plan message if it was successful. This doesn't require a spin at any point in this function.

As I've said, if you can post more of your code hopefully we can help you fix this, but as is it doesn't make much sense.

Edit. You should only need a spin in your main function, if it's not working with than then there is a problem with your code you should not try and solve it by adding more spin calls. To be clear ros::spin() will not return until the node has been shutdown, either through a call to ros::shutdown() or a Ctrl-C. So calling it inside a callback will do strange recurrsive things!

There is a serious problem with the code snippet below:

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

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

    //ros::spin();

    ROS_INFO("automatic control");
  }

The sub2 subscriber object will go out of scope and be destroyed when the end parenthesis of the if statement is reached. If you want it to remain subscribed you should make sub2 a global variable instead. Adding a spin here is causing the callbacks to be recursive which is why your code is trying to make multiple asyncspinner objects and crashing.

Remove all the spins and asyncspinners apart from main. You'll also need to remove the ros::waitForShutdown(); this will stop your node from doing anything.

I hope this helps, but I'd recommend learning some more about c++ and the thread of execution.

edit flag offensive delete link more

Comments

Thanks for your help. I posted my whole node in an edit, so hopefully it makes a bit more sense. I will say that I tried doing what you suggested in terms of eliminating the spinners inside automaticCallback but that resulted in motionPlan never getting published. Any other ideas?

Levi_Manring gravatar image Levi_Manring  ( 2018-07-13 13:18:32 -0500 )edit

I have also just tried eliminating every spinner except the once inside the main init node clause, and if I do that, nothing happens. The robot arm doesn't move in RVIZ, motionPlan isn't published...

Levi_Manring gravatar image Levi_Manring  ( 2018-07-13 13:57:50 -0500 )edit
1

I've updated my answer. You do need to remove all the spins except for in main, but there are a few other problems that need fixing in your code too.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2018-07-14 04:50:55 -0500 )edit

Thanks for your help. I have tried doing what you have suggested in a number of forms, with no success. I eliminated the if statement and did a bunch of other testing, still nothing is published to motionPlan, so this isn't working for me yet.

Levi_Manring gravatar image Levi_Manring  ( 2018-07-16 15:06:40 -0500 )edit

Can you post the modified code you've tried. There were a lot of things to fix in your old code so it's hard to keep track of the changes individually.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2018-07-17 02:35:55 -0500 )edit

I have posted the changes in a edit at the bottom of my question. I eliminated all the if statements and simplified as much as possible. Still the motionPlan is unpublished. The robot moves in RViz and does what it is supposed to, minus the crucial publishing stage.

Levi_Manring gravatar image Levi_Manring  ( 2018-07-17 10:34:27 -0500 )edit

I will say that it never executes the ROS_INFO statements in the code. So it stops after the group.plan command.

Levi_Manring gravatar image Levi_Manring  ( 2018-07-17 10:35:40 -0500 )edit

You cannot create a publisher object and instantly publish to it, they take a little bit of time to attach to any subscribers. It's also bad practice to create and destroy them repeatedly, that's not how they were intended to be used.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2018-07-17 15:38:06 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2018-07-12 19:00:39 -0500

Seen: 1,731 times

Last updated: Jul 17 '18