/* Constructor */
EkfNode::EkfNode(): private_nh_("~") {
// Create a publisher object to publish the determined state of the robot. Odometry messages contain both Pose and Twist with covariance.
stateMapPub_ = public_nh_.advertise<nav_msgs::Odometry>("odom_map",1);
stateOdomPub_ = public_nh_.advertise<nav_msgs::Odometry>("odom",1);
// Create a subscriber object to subscribe to the topic
dwSub_ = public_nh_.subscribe("dw/t0/data",1,&EkfNode::dwSubCB,this);
imuSub_ = public_nh_.subscribe("imu/data",1,&EkfNode::imuSubCB,this);
encSub_ = public_nh_.subscribe("enc",1,&EkfNode::encSubCB,this);
// Wait for time to not equal zero. A zero time means that no message has
// been received on the /clock topic
ros::Time timeZero(0.0);
while (ros::Time::now() == timeZero) { }
// Sleep for a small time to make sure publishing and subscribing works.
ros::Duration(0.1).sleep();
// Initialize
init();
}
/* Constructor */
Mapper::Mapper(): private_nh_("~") {
// Set up the publisher and subsciber objects
occupancyGrid_pub_ = public_nh_.advertise<nav_msgs::OccupancyGrid>("mowed_map",1);
static_grass_map_pub_ = public_nh_.advertise<nav_msgs::OccupancyGrid>("grass_map", 1, true);
// Publish the percent of the grass_map_ that has been mowed each update
percent_pub_ = public_nh_.advertise<std_msgs::Float64>("percent_complete",1, true);
odom_sub_ = public_nh_.subscribe("odom",10,&Mapper::odomCB,this);
// Offer three map servers that can be queried. One returns the original grass map (statis_grass_map), one returns the current location of the grass (grass_map), and one returns the areas that have been covered (mowed_map).
static_grass_map_srv_ = public_nh_.advertiseService("static_grass_map", &Mapper::staticGrassMapCallback, this);
grass_map_srv_ = public_nh_.advertiseService("grass_map", &Mapper::grassMapCallback, this);
mowed_map_srv_ = public_nh_.advertiseService("mowed_map", &Mapper::mowedMapCallback, this);
// Callback functions that reset the map, put the pen up, and put the pen down.
reset_map_srv_ = public_nh_.advertiseService("reset_map", &Mapper::resetMapCallback, this);
penUp_srv_ = public_nh_.advertiseService("penup", &Mapper::penUpCallback, this);
penDown_srv_ = public_nh_.advertiseService("pendown", &Mapper::penDownCallback, this);
// Wait for time to not equal zero. A zero time means that no message has been received on the /clock topic
ros::Time timeZero(0.0);
while (ros::Time::now() == timeZero) { }
// Sleep for a small time to make sure publishing and subscribing works.
ros::Duration(0.1).sleep();
// TODO: Get all these parameters from the ROS parameter server.
// Make options for defining number of cells, ppm, and meters.
// Any two of those should deterimine the other three.
// Cells = ppm * meters
// Put the pen Down (Get ready to start marking).
penDown_ = true;
init();
// Publish the empty map
occupancyGrid_pub_.publish(mowed_map_);
};