use robot_state::RobotState robot_state(*planning_display_->getQueryGoalState());