34 #include "robot_localization/GetState.h" 60 robot_localization::GetState::Response &res)
65 if ( !rll_.
getState(req.time_stamp, req.frame_id, state, covariance) )
67 ROS_ERROR(
"Robot Localization Listener Node: Listener instance returned false at getState call.");
73 res.state[i] = (*(state.data() + i));
76 for (
size_t i = 0; i < STATE_SIZE *
STATE_SIZE; i++)
78 res.covariance[i] = (*(covariance.data() + i));
81 ROS_DEBUG(
"Robot Localization Listener Node: Listener responded with state and covariance at the requested time.");
88 int main(
int argc,
char **argv)
90 ros::init(argc, argv,
"robot_localization_listener_node");
const int STATE_SIZE
Global constants that define our state vector size and offsets to groups of values within that state...
RobotLocalizationListenerNode()
RosRobotLocalizationListener class.
bool getState(const double time, const std::string &frame_id, Eigen::VectorXd &state, Eigen::MatrixXd &covariance, std::string world_frame_id="") const
Get a state from the localization estimator.
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
ros::ServiceServer service_
ROSCPP_DECL void spin(Spinner &spinner)
RosRobotLocalizationListener rll_
std::string getService() const
#define ROS_INFO_STREAM(args)
int main(int argc, char **argv)
bool getStateCallback(robot_localization::GetState::Request &req, robot_localization::GetState::Response &res)