Planning and executing with logic commands: how do I keep a plan "alive" so I can decide to execute it later?
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 ...
Please make sure to use the
101010
button to properly format your code (and to remove extraneous spacing and comments) to make it legible.