How to use move group interface in a sub function?
Hello,
Because my robot have 3 different targets need to reach in a loop, I want to merge the same line of each move group in a sub function to let the whole program more easy to read.
I think it will look like this:
void move_robot (moveit::planning_interface::MoveGroupInterface group, geometry_msgs::PoseStamped ObjectPose)
{
group.setStartState(*group.getCurrentState());
group.setPoseTarget(ObjectPose);
moveit::planning_interface::MoveGroupInterface::Plan group_Planer;
bool success = (group.plan(group_Planer) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
if (success)
{
ros::Duration(1).sleep();
group.execute(group_Planer);
}
group.clearPoseTargets();
ros::Duration(1).sleep();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "move");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(1);
spinner.start();
moveit::planning_interface::MoveGroupInterface group("manipulator");
move_robot(group, pose1);
move_robot(group, pose2);
move_robot(group, pose3);
return 0;
}
I got some error in this form, so I would like to know how to modify and run it. Thank for your answer!