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

Planning and executing with logic commands: how do I keep a plan "alive" so I can decide to execute it later?

asked 2018-07-09 16:09:16 -0600

Levi_Manring gravatar image

updated 2018-07-09 18:17:59 -0600

jayess gravatar image

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.


#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
    move_group_interface::MoveGroup group("sia5dArm");
    moveit::planning_interface::MoveGroup::Plan my_plan;

    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")
    ROS_INFO("not executed");

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

  ros::AsyncSpinner spinner(1);


  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++)


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

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


    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();


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;
  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);


  return 0;


The error I am getting looks like this:

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

which to ... (more)

edit retag flag offensive close merge delete



Please make sure to use the 101010 button to properly format your code (and to remove extraneous spacing and comments) to make it legible.

jayess gravatar image jayess  ( 2018-07-09 17:30:16 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2018-07-12 12:11:53 -0600

Levi_Manring gravatar image

What I ended up doing was writing the motion plan to a .bag file, and then reading from it at a later point. The timing is so far working fine, so the problem is eliminated on that point. However, since I had to convert the plan to a RobotTrajectory message I need to still figure out how to execute the plan (commented here)


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


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

std::vector<std::string> topics;
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
edit flag offensive delete link more

Question Tools



Asked: 2018-07-09 16:09:16 -0600

Seen: 387 times

Last updated: Jul 12 '18