diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h b/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h index 06be040fe75069b61b9687a18bb2a61743635fa9..2f2d2939ef6b82af14c938a7851c712e48f8d396 100644 --- a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h +++ b/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h @@ -146,7 +146,7 @@ class SlamFilter : public ParticleFilter<SlamParticle> { * particles. The pose of the particle with the highest value is the first * element of the vector. */ - std::vector<Pose>* getParticlePoses() const; + std::vector<Pose> getParticlePoses() const; /** * @return vector of all particles diff --git a/homer_mapping/include/homer_mapping/slam_node.h b/homer_mapping/include/homer_mapping/slam_node.h index bb23f6709a370f351d18d93327027b8238f9f41b..7e6b6da93886ef631cca20e094dc986484b66402 100644 --- a/homer_mapping/include/homer_mapping/slam_node.h +++ b/homer_mapping/include/homer_mapping/slam_node.h @@ -95,7 +95,7 @@ private: void sendTfAndPose(Pose pose); - void sendPoseArray(std::vector<Pose>* poses); + void sendPoseArray(std::vector<Pose> poses); Pose getInterpolatedPose(nav_msgs::Odometry::ConstPtr pose1, nav_msgs::Odometry::ConstPtr pose2, diff --git a/homer_mapping/src/ParticleFilter/SlamFilter.cpp b/homer_mapping/src/ParticleFilter/SlamFilter.cpp index c77cebe78cfbe7db36d25dc1740af06443d5c831..5bc9c1c92e60e9d2b6a179c6aa7b83fb0a2ff469 100644 --- a/homer_mapping/src/ParticleFilter/SlamFilter.cpp +++ b/homer_mapping/src/ParticleFilter/SlamFilter.cpp @@ -168,12 +168,12 @@ void SlamFilter::setOccupancyMap(OccupancyMap* occupancyMap) m_OccupancyMap = occupancyMap; } -vector<Pose>* SlamFilter::getParticlePoses() const +vector<Pose> SlamFilter::getParticlePoses() const { - vector<Pose>* particlePoses = new vector<Pose>(); + vector<Pose> particlePoses; for (int i = 0; i < m_ParticleNum; i++) { - particlePoses->push_back(m_CurrentList[i]->getRobotPose()); + particlePoses.push_back(m_CurrentList[i]->getRobotPose()); } return particlePoses; } diff --git a/homer_mapping/src/slam_node.cpp b/homer_mapping/src/slam_node.cpp index f76f895de8a15968c4ab07c21eccd613ced72b82..7820e0640fa65a440c4904b62f53d4df09ab5467 100644 --- a/homer_mapping/src/slam_node.cpp +++ b/homer_mapping/src/slam_node.cpp @@ -158,25 +158,24 @@ void SlamNode::sendTfAndPose(Pose pose) transform, poseMsg.header.stamp, "map", "base_link")); } -void SlamNode::sendPoseArray(std::vector<Pose>* poses) +void SlamNode::sendPoseArray(std::vector<Pose> poses) { // Pose Array publishing geometry_msgs::PoseArray poseArray = geometry_msgs::PoseArray(); poseArray.header.stamp = ros::Time::now(); poseArray.header.frame_id = "/map"; - for (int i = 0; i < poses->size(); i++) + for (int i = 0; i < poses.size(); i++) { geometry_msgs::Pose tmpPose = geometry_msgs::Pose(); - tmpPose.position.x = poses->at(i).x(); - tmpPose.position.y = poses->at(i).y(); - tf::Quaternion quatTF = tf::createQuaternionFromYaw(poses->at(i).theta()); + tmpPose.position.x = poses.at(i).x(); + tmpPose.position.y = poses.at(i).y(); + tf::Quaternion quatTF = tf::createQuaternionFromYaw(poses.at(i).theta()); geometry_msgs::Quaternion quatMsg; tf::quaternionTFToMsg(quatTF, quatMsg); tmpPose.orientation = quatMsg; poseArray.poses.push_back(tmpPose); } - delete poses; m_PoseArrayPublisher.publish(poseArray); }