diff --git a/include/people_simulation_alg_node.h b/include/people_simulation_alg_node.h
index ba48585fcca503e1d4d981d174d9fddeff5a752d..eb3dfa5b5c2a5faa1999d07fb631204a5d9326c7 100644
--- a/include/people_simulation_alg_node.h
+++ b/include/people_simulation_alg_node.h
@@ -28,7 +28,13 @@
#include <iri_base_algorithm/iri_base_algorithm.h>
#include "people_simulation_alg.h"
#include "scene_sim.h"
-#include <tf/transform_listener.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2/LinearMath/Vector3.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/transform_datatypes.h>
+#include <tf2/impl/utils.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
// [publisher subscriber headers]
#include <visualization_msgs/MarkerArray.h>
@@ -86,13 +92,15 @@ class PeopleSimulationAlgNode : public algorithm_base::IriBaseAlgorithm<PeopleSi
// [class atributes]
int n_persons_;
- simulation_mode simulation_mode_;
+ simulation_mode simulation_mode_;
Cscene_sim scene_;
void init_sim();
std::string force_map_path_, destination_map_path_;
bool vis_mode_,remove_targets_, freeze_;
- tf::TransformListener tf_listener_;
- std::string robot_;
+ tf2_ros::TransformListener tf2_listener_;
+ tf2_ros::Buffer tf2_buffer; // for version in melodic
+ tf2_ros::Buffer* tf_;
+ std::string robot_;
unsigned int id_person_to_approach_ros_;
diff --git a/launch/people_simulation.launch b/launch/people_simulation.launch
index 006886f032dba16bc06d0f89f1c4f93e456e26aa..71314be889055f9ad15da97bdbd38946b511210e 100644
--- a/launch/people_simulation.launch
+++ b/launch/people_simulation.launch
@@ -7,13 +7,13 @@
type="iri_people_simulation_approaching"
name="people_simulation_approaching"
output="screen">
- <param name="number_persons" value="1"/>
+ <param name="number_persons" value="10"/>
<param name="vis_mode" value="True"/>
<param name="robot" value="$(optenv ROBOT tibi)" />
<param name="destination_map_path" value="$(find iri_akp_local_planner_approaching)/maps/$(arg map)_destinations.txt" />
<!-- TODO change map with arg -->
- <remap from="~tracks" to="/$(optenv ROBOT tibi)/mht/tracks"/>
- <remap from="~tracksMarkers" to="/$(optenv ROBOT tibi)/mht/tracksMarkers"/>
+ <remap from="~tracks" to="/$(optenv ROBOT tibi)/tracks"/>
+ <remap from="~tracksMarkers" to="/$(optenv ROBOT tibi)/tracksMarkers"/>
<remap from="~init_simulations" to="/$(optenv ROBOT tibi)/init_simulations"/>
</node>
diff --git a/src/people_simulation_alg_node.cpp b/src/people_simulation_alg_node.cpp
index 269776c412217b7267e8e750f9235ad57f54f4fd..7e1e87e599e3f95da6bca6a8a50cce9264c35a16 100644
--- a/src/people_simulation_alg_node.cpp
+++ b/src/people_simulation_alg_node.cpp
@@ -3,7 +3,9 @@
PeopleSimulationAlgNode::PeopleSimulationAlgNode(void) :
algorithm_base::IriBaseAlgorithm<PeopleSimulationAlgorithm>(),
n_persons_(0), simulation_mode_( PeopleSimulationAlgNode::Normal ),
- tf_listener_(ros::Duration(10.f))
+ tf2_listener_(tf2_buffer),
+ tf_(NULL)
+ // tf2_listener_(ros::Duration(10.f))
{
//init class attributes if necessary
//this->loop_rate_ = 10;//in [Hz] Es de ubuntu 14.04
@@ -47,7 +49,7 @@ void PeopleSimulationAlgNode::init_sim()
scene_.set_number_virtual_people( n_persons_ );
scene_.set_remove_targets( remove_targets_ );
- peopleTrackingArray_msg_.header.frame_id = "/map";
+ peopleTrackingArray_msg_.header.frame_id = "map";
//read destinations
if ( !scene_.read_destination_map( destination_map_path_.c_str() ) )
@@ -70,7 +72,7 @@ void PeopleSimulationAlgNode::init_sim()
//scene_.set_remove_targets(true);
//target marker
- track_marker_.header.frame_id = "/map";
+ track_marker_.header.frame_id = "map";
track_marker_.ns = "tracks";
track_marker_.type = visualization_msgs::Marker::CYLINDER;
track_marker_.action = visualization_msgs::Marker::ADD;
@@ -84,7 +86,7 @@ void PeopleSimulationAlgNode::init_sim()
track_marker_.color.b = 0.0;
track_marker_.pose.position.z = 0.3;
- id_marker_.header.frame_id = "/map";
+ id_marker_.header.frame_id = "map";
id_marker_.ns = "ids";
id_marker_.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
id_marker_.action = visualization_msgs::Marker::ADD;
@@ -124,27 +126,38 @@ void PeopleSimulationAlgNode::mainNodeThread(void)
this->alg_.lock();
// ------------------------------------------------------------------------
//Get Robot position: transform empty/zero pose from base_link to map
- std::string target_frame = "/map";
+ std::string target_frame = "map";
//TODO set param in .launch <param name="~/robot" type="string" value="$(env ROBOT)" />
- std::string source_frame = "/" + this->robot_ + "/base_link";
+ std::string source_frame = this->robot_ + "/base_link";
ros::Time target_time = ros::Time::now();
- bool tf_exists = tf_listener_.waitForTransform(target_frame, source_frame, target_time, ros::Duration(1), ros::Duration(0.01));
- if(tf_exists)
- {
+ ros::Duration timeout = ros::Duration(0.1); // new for ros melodic
+
+//bool tf2_exists = this->tf_->waitForTransform(target_frame, source_frame, target_time, ros::Duration(1), ros::Duration(0.01));
+bool tf2_exists = tf2_buffer.canTransform(target_frame, source_frame, target_time);
+ // antes de ros melodic=> bool tf_exists = tf_listener_.waitForTransform(target_frame, source_frame, target_time, ros::Duration(1), ros::Duration(0.01));
+
+//ROS_INFO("PeopleSimulationAlgNode::mainNodeThread:bool=%d source_frame=%s-->target_frame=%s",tf2_exists,source_frame.c_str(), target_frame.c_str());
+
+ // if(tf2_exists)
+ // {
geometry_msgs::PoseStamped poseIn;
geometry_msgs::PoseStamped poseOut;
poseIn.header.stamp = target_time;
poseIn.header.frame_id = source_frame;
poseIn.pose.orientation.z = 1.0; //valid quaternion
- tf_listener_.transformPose(target_frame, poseIn, poseOut);
+ geometry_msgs::TransformStamped source_frame_to_target_frame; // new for tf2 ros melodic
+ //source_frame_to_target_frame = tf2_buffer.lookupTransform(target_frame, source_frame, target_time);
+ // source_frame_to_target_frame = tf2_buffer.lookupTransform(target_frame, source_frame, target_time,timeout); // new for tf2 ros melodic
+ // tf2::doTransform( poseIn, poseOut,source_frame_to_target_frame);
+ //antes de tf2 melodic=> tf2_listener_.transformPose(target_frame, poseIn, poseOut);
//TODO add velocities in /map coordinates to the scene update
+poseOut=poseIn; //new with the comments.
scene_.update_robot(Spose(poseOut.pose.position.x, poseOut.pose.position.y, poseOut.header.stamp.toSec()));
- }
- else
- {
- ROS_INFO("fail, no transform found");
- }
-
+// }
+// else
+ // {
+ // ROS_INFO(" (de momento no se si afecta)fail, no transform found");
+ // }
std::vector<SdetectionObservation> obs_scene;
obs_scene.push_back( SdetectionObservation(0, ros::Time::now().toSec() ));//void observation, just for the timestamp
@@ -182,6 +195,8 @@ void PeopleSimulationAlgNode::mainNodeThread(void)
person.covariances[27] = (*iit)->get_current_pointV().cov[14];//vy vx
person.covariances[28] = (*iit)->get_current_pointV().cov[15];//vy vy
peopleTrackingArray_msg_.detection.push_back(person);
+
+//ROS_INFO("PeopleSimulationAlgNode: person.position.x=%f; person.position.y=%f", person.position.x , person.position.y);
}
}