Non initialized pose header in motion planning request might create an issue

[ChamzasKonstantinos]

We need to make sure the header frame and stamp are set correctly in the poses set inside the goalConstraints.

For example it was observed that the following code needs to used in order to have the correct goal sampler.

auto setGoal = [this, &request](RobotPose &eef_pose) {
            geometry_msgs::PoseStamped poseMsg;
            poseMsg.header.frame_id = robot->getModelConst()->getModelFrame();
            poseMsg.header.stamp = ros::Time::now();
            poseMsg.pose = TF::poseEigenToMsg(eef_pose);
            request.clearGoals();
            request.getRequest().goal_constraints.push_back(
                kinematic_constraints::constructGoalConstraints(
                    robot->getSolverTipFrames(group)[0], poseMsg,
                    {1e-3, 1e-3, 1e-3}, {1e-2, 1e-2, constants::pi}));
            return true;
        }; 

[ChamzasKonstantinos]

@LeeYiyuan Maybe you should be aware of this.