diff --git a/include/state_estimator/state_estimator.h b/include/state_estimator/state_estimator.h index 019999c..a8dd3cf 100644 --- a/include/state_estimator/state_estimator.h +++ b/include/state_estimator/state_estimator.h @@ -26,6 +26,8 @@ #include +#include + class StateEstimator { public: @@ -81,6 +83,13 @@ class StateEstimator */ void estimated_pose(geometry_msgs::PoseWithCovarianceStamped& current_pose); + /*! + * @brief Repackages keyframes into a ROS message for publishing + * + * Repackages keyframes into a geometry_msgs::PoseWithCovarianceStampedArray ROS message for publishing + */ + void keyframes(lara_rgbd_msgs::PoseWithCovarianceStampedArray& keyframes); + private: /*! * @brief Performs state vector augmentation using current pose diff --git a/include/state_estimator/state_estimator_node.h b/include/state_estimator/state_estimator_node.h index 60825da..67ee3af 100644 --- a/include/state_estimator/state_estimator_node.h +++ b/include/state_estimator/state_estimator_node.h @@ -22,8 +22,10 @@ void odom_cb(const nav_msgs::Odometry::ConstPtr& odom_msg); // ROS Publisher Functions void pub_fake_pose_estimate(const nav_msgs::Odometry::ConstPtr& odom_msg); void pub_pose_estimate(); +void pub_keyframes_estimate(); ros::Publisher pub_pose; +ros::Publisher pub_keyframes; StateEstimator* state_estimator; diff --git a/launch/settings.rviz b/launch/settings.rviz index 1e69df4..b178ec7 100755 --- a/launch/settings.rviz +++ b/launch/settings.rviz @@ -170,13 +170,21 @@ Visualization Manager: Use rainbow: true Value: true - Alpha: 0.5 - Class: rviz_plugin_covariance/PoseWithCovariance + Class: rviz_lara_rgbd/PoseWithCovariance Color: 204; 51; 204 Enabled: true Name: PoseWithCovariance Scale: 1 Topic: /lara_rgbd/estimated_pose Value: true + - Alpha: 0.5 + Class: rviz_lara_rgbd/PoseWithCovarianceArray + Color: 255; 255; 0 + Enabled: true + Name: PoseWithCovarianceArray + Scale: 1 + Topic: /lara_rgbd/keyframes + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/src/state_estimator/state_estimator.cpp b/src/state_estimator/state_estimator.cpp index 3a619d2..a186f7b 100644 --- a/src/state_estimator/state_estimator.cpp +++ b/src/state_estimator/state_estimator.cpp @@ -79,7 +79,7 @@ void StateEstimator::cloud_measurement_model(const sensor_msgs::PointCloud2::Con // Separate good matches and go through cloud matching model equations } - if((num_keyframes_ == 0) || !matches_found) + if((num_keyframes_ == 0)/* || !matches_found*/) { // Augment state vector with initial pose augment_state_vector_(); @@ -115,6 +115,46 @@ void StateEstimator::estimated_pose(geometry_msgs::PoseWithCovarianceStamped& cu current_pose.pose.covariance[30+5] = 0.01; } +void StateEstimator::keyframes(lara_rgbd_msgs::PoseWithCovarianceStampedArray& keyframes) +{ + char frame_id[20]; + + for(int i = 0; i < num_keyframes_; i++) + { + geometry_msgs::PoseWithCovarianceStamped keyframe; + + sprintf(frame_id, "/base_link_%d", i); + + keyframe.header.frame_id = frame_id; + keyframe.header.stamp = keyframes.header.stamp; + + keyframe.pose.pose.position.x = keyframes_pose_[i](0); + keyframe.pose.pose.position.y = keyframes_pose_[i](1); + keyframe.pose.pose.position.z = keyframes_pose_[i](2); + keyframe.pose.pose.orientation.w = keyframes_pose_[i](3); + keyframe.pose.pose.orientation.x = keyframes_pose_[i](4); + keyframe.pose.pose.orientation.y = keyframes_pose_[i](5); + keyframe.pose.pose.orientation.z = keyframes_pose_[i](6); + + ROS_DEBUG("keyframes not fully implemented. returning fixed covariance for orientation!"); + for(int ii = 0; ii < 3; ii++) + { + for(int jj = 0; jj < 3; jj++) + { + keyframe.pose.covariance[6*ii+jj] = keyframes_cov_[i](ii,jj); + } + } + + keyframe.pose.covariance[18+3] = 0.01; + keyframe.pose.covariance[24+4] = 0.01; + keyframe.pose.covariance[30+5] = 0.01; + + //ROS_INFO_STREAM_THROTTLE(1, "keyframe #" << i << ":\n\n" << keyframe << "\n"); + + keyframes.poses.push_back(keyframe); + } +} + void StateEstimator::augment_state_vector_() { int n = state_estimate_.size(); diff --git a/src/state_estimator/state_estimator_node.cpp b/src/state_estimator/state_estimator_node.cpp index c80ac9d..aeec127 100644 --- a/src/state_estimator/state_estimator_node.cpp +++ b/src/state_estimator/state_estimator_node.cpp @@ -28,6 +28,9 @@ int main (int argc, char** argv) // Create a ROS publisher for estimated pose pub_pose = nh.advertise("estimated_pose", 1); + // Create a ROS publisher for keyframes + pub_keyframes = nh.advertise("keyframes", 1); + // Create SensorProcessor state_estimator = new StateEstimator(); @@ -39,6 +42,7 @@ int main (int argc, char** argv) state_estimator->quasiconstant_motion_model(); pub_pose_estimate(); + pub_keyframes_estimate(); ros::spinOnce(); loop_rate.sleep(); @@ -54,7 +58,7 @@ void odom_cb(const nav_msgs::Odometry::ConstPtr& odom_msg) { state_estimator->odom_motion_model(odom_msg); - //pub_fake_pose_estimate(odom_msg); + pub_fake_pose_estimate(odom_msg); } void pub_fake_pose_estimate(const nav_msgs::Odometry::ConstPtr& odom_msg) @@ -62,10 +66,11 @@ void pub_fake_pose_estimate(const nav_msgs::Odometry::ConstPtr& odom_msg) // Fake pose estimate from odometry geometry_msgs::PoseWithCovarianceStamped::Ptr estimated_pose_msg(new geometry_msgs::PoseWithCovarianceStamped); estimated_pose_msg->header = odom_msg->header; + estimated_pose_msg->header.frame_id = "world"; estimated_pose_msg->pose = odom_msg->pose; - estimated_pose_msg->pose.covariance[0] = 10; - estimated_pose_msg->pose.covariance[6+1] = 10; + estimated_pose_msg->pose.covariance[0] = 1; + estimated_pose_msg->pose.covariance[6+1] = 1; estimated_pose_msg->pose.covariance[12+2] = 0.1; estimated_pose_msg->pose.covariance[18+3] = 0.01; estimated_pose_msg->pose.covariance[24+4] = 0.01; @@ -85,3 +90,14 @@ void pub_pose_estimate() pub_pose.publish(current_pose); } + +void pub_keyframes_estimate() +{ + lara_rgbd_msgs::PoseWithCovarianceStampedArray keyframes; + + keyframes.header.stamp = ros::Time::now(); + keyframes.header.frame_id = "world"; + state_estimator->keyframes(keyframes); + + pub_keyframes.publish(keyframes); +}