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
#include
#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
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;
//std::cout< trajectory_points;
trajectory_points = my_plan.trajectory_.joint_trajectory.points;
std::vector::size_type size1 = trajectory_points.size();
std::vector motionPlan_msg;
for (unsigned i = 0; i::size_type size2 = trajectory_points[i].positions.size();
//std::cout<("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)
{
}
//****************************
//----------------------------
void manualCallback(const geometry_msgs::Pose::ConstPtr& msg)
{
}
//****************************
//----------------------------
void graspCallback(const geometry_msgs::Pose::ConstPtr& msg)
{
}
//****************************
//----------------------------
void stopCallback(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<
↧