From f7dab752dbcd9346589046a553523b4e8c9a13bd Mon Sep 17 00:00:00 2001
From: Ely Repiso Polo <erepiso@iri.upc.edu>
Date: Wed, 8 May 2019 11:20:33 +0000
Subject: [PATCH] the method works for an approached person moving
---
include/people_simulation_alg_node.h | 2 +-
launch/people_sim.launch | 2 +-
launch/people_simulation.launch | 2 +-
map/empty_destinations.txt | 12 ++++----
src/people_simulation_alg_node.cpp | 43 +++++++++++++++++-----------
5 files changed, 37 insertions(+), 24 deletions(-)
diff --git a/include/people_simulation_alg_node.h b/include/people_simulation_alg_node.h
index 3443b1b..653de2e 100644
--- a/include/people_simulation_alg_node.h
+++ b/include/people_simulation_alg_node.h
@@ -94,7 +94,7 @@ class PeopleSimulationAlgNode : public algorithm_base::IriBaseAlgorithm<PeopleSi
tf::TransformListener tf_listener_;
std::string robot_;
-
+ unsigned int id_person_to_approach_ros_;
public:
/**
diff --git a/launch/people_sim.launch b/launch/people_sim.launch
index 080ca27..15f5e7f 100644
--- a/launch/people_sim.launch
+++ b/launch/people_sim.launch
@@ -16,7 +16,7 @@
type="iri_people_simulation_approaching"
name="iri_people_simulation_approaching"
output="screen">
- <param name="number_persons" value="20"/>
+ <param name="number_persons" value="40"/>
<param name="vis_mode" value="True"/>
<param name="robot" type="string" value="$(optenv ROBOT tibi)" />
<param name="destination_map_path" value="$(find iri_people_simulation_approaching)/map/4_destinations.txt" />
diff --git a/launch/people_simulation.launch b/launch/people_simulation.launch
index 9854cf8..006886f 100644
--- a/launch/people_simulation.launch
+++ b/launch/people_simulation.launch
@@ -7,7 +7,7 @@
type="iri_people_simulation_approaching"
name="people_simulation_approaching"
output="screen">
- <param name="number_persons" value="20"/>
+ <param name="number_persons" value="1"/>
<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" />
diff --git a/map/empty_destinations.txt b/map/empty_destinations.txt
index 4d07d23..551e10c 100644
--- a/map/empty_destinations.txt
+++ b/map/empty_destinations.txt
@@ -1,5 +1,7 @@
-4
-0, 0, -15, 0.5, 1, 1
-1, 15, 15, 0.5, 1, 0
-2, 0, 15, 0.5, 1, 3
-3, 15, -15, 0.5, 1, 2
+6
+0, 0, -25, 0.5, 5, 1,2,3,4,5
+1, 25, 25, 0.5, 5, 0,2,3,4,5
+2, 0, 25, 0.5, 5, 0,1,3,4,5
+3, 25, -25, 0.5, 5, 0,1,2,4,5
+4, 15, 25, 0.5, 5, 0,1,2,3,5
+5, 15, -25, 0.5, 5, 0,1,2,3,4
diff --git a/src/people_simulation_alg_node.cpp b/src/people_simulation_alg_node.cpp
index 106dec2..1236a5d 100644
--- a/src/people_simulation_alg_node.cpp
+++ b/src/people_simulation_alg_node.cpp
@@ -194,26 +194,36 @@ void PeopleSimulationAlgNode::mainNodeThread(void)
for( std::list<Cperson_abstract*>::const_iterator iit = person_list->begin();
iit!=person_list->end(); iit++ )
{
- track_marker_.id = (*iit)->get_id();
- track_marker_.pose.position.x = (*iit)->get_current_pointV().x;
- track_marker_.pose.position.y = (*iit)->get_current_pointV().y;
- MarkerArray_msg_.markers.push_back(track_marker_);
- id_marker_.id = (*iit)->get_id();
- id_marker_.pose.position.x = (*iit)->get_current_pointV().x;
- id_marker_.pose.position.y = (*iit)->get_current_pointV().y;
- std::ostringstream target_id;
- target_id << id_marker_.id;
- id_marker_.text = target_id.str();
- MarkerArray_msg_.markers.push_back(id_marker_);
- }
+ track_marker_.id = (*iit)->get_id();
+ track_marker_.pose.position.x = (*iit)->get_current_pointV().x;
+ track_marker_.pose.position.y = (*iit)->get_current_pointV().y;
+ if(track_marker_.id==id_person_to_approach_ros_){
+ track_marker_.color.r = 1.0;
+ track_marker_.color.g = 0.0;
+ track_marker_.color.b = 0.0;
+ }else{
+ track_marker_.color.r = 0.0;
+ track_marker_.color.g = 1.0;
+ track_marker_.color.b = 0.0;
+ }
+
+ MarkerArray_msg_.markers.push_back(track_marker_);
+ id_marker_.id = (*iit)->get_id();
+ id_marker_.pose.position.x = (*iit)->get_current_pointV().x;
+ id_marker_.pose.position.y = (*iit)->get_current_pointV().y;
+ std::ostringstream target_id;
+ target_id << id_marker_.id;
+ id_marker_.text = target_id.str();
+ MarkerArray_msg_.markers.push_back(id_marker_);
+ }
//draw robot if any
if ( scene_.get_robot() != NULL )
{
- track_marker_.id = 0;
+ track_marker_.id = 0;
track_marker_.ns = "robot";
- track_marker_.pose.position.x = scene_.get_robot()->get_current_pointV().x;
- track_marker_.pose.position.y = scene_.get_robot()->get_current_pointV().y;
- MarkerArray_msg_.markers.push_back(track_marker_);
+ track_marker_.pose.position.x = scene_.get_robot()->get_current_pointV().x;
+ track_marker_.pose.position.y = scene_.get_robot()->get_current_pointV().y;
+ MarkerArray_msg_.markers.push_back(track_marker_);
track_marker_.ns = "tracks";
}
@@ -289,6 +299,7 @@ void PeopleSimulationAlgNode::node_config_update(Config &config, uint32_t level)
freeze_ = config.freeze;
scene_.set_number_virtual_people( n_persons_ );
scene_.set_id_person_to_apperoach(config.id_person_to_approach);
+ id_person_to_approach_ros_=config.id_person_to_approach;
this->alg_.unlock();
}
--
GitLab