DWAPlanner::checkTrajectory always success [closed]

edit

Hello, I try to use the dwa_local_planner::DWAPlanner::checkTrajectory.

I have edit the dwa_planner_ros.cpp and .h in order to subsrcibe to a path and i want to check if the trajectory is right. I use the dwa_planner initialize and update by the dwa_planner_ros.cpp

  costmap_ros_->getRobotPose(current_pose_); // (costmap_ros_ is the costmap_2d::Costmap2DROS)

    //we want to clear the robot footprint from the costmap we're using
    costmap_ros_->clearRobotFootprint();

    // make sure to update the costmap we'll use for this cycle
    costmap_ros_->getCostmapCopy(costmap_);
        tf::Stamped<tf::Pose> robot_vel;
        odom_helper_.getRobotVel(robot_vel);

        tf::Stamped<tf::Pose> drive_cmds;
        drive_cmds.frame_id_ = costmap_ros_->getBaseFrameID();


        dp_->updatePlanAndLocalCosts(current_pose_,req.path); // dp_ is the dwa_local_planner::DWAPlanner
        planner_util_.setPlan(req.path);
        dp_->findBestPath(current_pose_, robot_vel, drive_cmds, costmap_ros_->getRobotFootprint());
        Eigen::Vector3f pos,vel,vel_samples;
        pos[0]=current_pose_.getOrigin().getX();
    pos[1]=current_pose_.getOrigin().getY();
    pos[2]=current_pose_.getOrigin().getZ();
        vel[0]=robot_vel.getOrigin().getX();
        vel[1]=robot_vel.getOrigin().getY();
        vel[2]=robot_vel.getOrigin().getZ();
        vel_samples[0]=3;
        vel_samples[1]=10;
        vel_samples[2]=20;

        if(dp_->checkTrajectory(pos,vel,vel_samples)){ROS_INFO("success");}else{ROS_INFO("fail");}

But even with a wrong plan ( i can visualize that the path go into walls) the checkTrajectory function always return true.

Someone can tell me what is my mistake, what I have forget?

thanks