Skip to content
Snippets Groups Projects
Commit 270772fc authored by Florian Polster's avatar Florian Polster
Browse files

remove pointer

parent 13f45761
No related branches found
No related tags found
1 merge request!4Recent changes
......@@ -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
......
......@@ -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,
......
......@@ -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;
}
......
......@@ -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);
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment