diff --git a/include/akp_local_planner_alg_node.h b/include/akp_local_planner_alg_node.h
index 2724e6afbec23318d76a8f266222b27c686423c5..50c3ef7d5da26d9363c175ceaad2c5a3167d12f7 100644
--- a/include/akp_local_planner_alg_node.h
+++ b/include/akp_local_planner_alg_node.h
@@ -42,7 +42,7 @@
#include <iri_robot_aspsi/AkpLocalPlannerConfig.h>
// [publisher subscriber headers]
-// #include <iri_nav_msgs/companionStateResults.h>
+#include <iri_nav_msgs/companionStateResults.h>
#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/UInt64.h>
#include <geometry_msgs/Twist.h>
@@ -150,7 +150,7 @@ class AkpLocalPlanner : public nav_core::BaseLocalPlanner
geometry_msgs::PoseWithCovarianceStamped tibi_restart_pose_;
ros::Publisher companionState_publisher_; //publisher of planner state, for the companion state to do that tibi talks.
- // iri_nav_msgs::companionStateResults companionState_companionStateResults_msg_; // was to obtain som states from severan situations and do a authomatic robot's speech for human-robot interaction. Do not needed for only accompaniment.
+ iri_nav_msgs::companionStateResults companionState_companionStateResults_msg_; // was to obtain som states from severan situations and do a authomatic robot's speech for human-robot interaction. Do not needed for only accompaniment.
// [publisher attributes]
ros::Publisher g_plan_pub_;
diff --git a/launch/gazebo_ASPSI_OK_ana.launch b/launch/gazebo_ASPSI_OK_ana.launch
index ba1860aec5b9d1a99ebb41a7a9d5b500c3e87c5c..3db1b61dbf31bcfab7709dcdf75f9b1ed3af0bcf 100644
--- a/launch/gazebo_ASPSI_OK_ana.launch
+++ b/launch/gazebo_ASPSI_OK_ana.launch
@@ -44,8 +44,30 @@
<param name="AkpLocalPlanner/sim_target_per" value="False"/>
<param name="AkpLocalPlanner/frame_robot_footprint" value="ana/base_link"/>
</node>
+
+<!-- node states companion -->
+ <node pkg ="iri_state_machine_companion"
+ type="iri_state_machine_companion"
+ name="state_mach_comp"
+ output="screen">
+ <remap from="/ana/state_mach_comp/people_tracks"
+ to="/ana/mht/tracks" />
+ <remap from="/ana/state_mach_comp/odom"
+ to="/ana/odom" />
+ <remap from="/ana/state_mach_comp/companionState"
+ to="/ana/companionState" />
+ <remap from="/ana/state_mach_comp/tibi_akp_status"
+ to="/ana/ana_akp_tatus"/>
+ <param name="~/threshold_max_dist_between_robot_and_person_conf" value="3.0" />
+ <param name="~/frame_map" value="map" />
+ <param name="~/frame_robot_footprint" value="ana/base_link" />
+ <param name="~/robot" value="ana" />
+ <param name="terrinet_project_conf" value="true"/>
+ </node>
</group>
+
+
<node name="dr" pkg ="rqt_reconfigure" type="rqt_reconfigure" />
</launch>
diff --git a/launch/gazebo_ASPSI_OK_ana_brl.launch b/launch/gazebo_ASPSI_OK_ana_brl.launch
index 8361d8f89701ab3d48b383ae25640c3b247c7122..6d60eb1d05b36cca6bf4f8a7f7b19cc236c083f7 100644
--- a/launch/gazebo_ASPSI_OK_ana_brl.launch
+++ b/launch/gazebo_ASPSI_OK_ana_brl.launch
@@ -44,8 +44,32 @@
<param name="AkpLocalPlanner/sim_target_per" value="False"/>
<param name="AkpLocalPlanner/frame_robot_footprint" value="ana/base_link"/>
</node>
+
+<!-- node states companion -->
+ <node pkg ="iri_state_machine_companion"
+ type="iri_state_machine_companion"
+ name="state_mach_comp"
+ output="screen">
+ <remap from="/ana/state_mach_comp/people_tracks"
+ to="/ana/mht/tracks" />
+ <remap from="/ana/state_mach_comp/odom"
+ to="/ana/odom" />
+ <remap from="/ana/state_mach_comp/companionState"
+ to="/ana/companionState" />
+ <remap from="/ana/state_mach_comp/tibi_akp_status"
+ to="/ana/ana_akp_tatus"/>
+ <param name="~/threshold_max_dist_between_robot_and_person_conf" value="3.0" />
+ <param name="~/frame_map" value="map" />
+ <param name="~/frame_robot_footprint" value="ana/base_link" />
+ <param name="~/robot" value="ana" />
+ <param name="terrinet_project_conf" value="true"/>
+ </node>
+
</group>
+
+
+
<node name="dr" pkg ="rqt_reconfigure" type="rqt_reconfigure" />
</launch>
diff --git a/src/akp_local_planner_alg_node.cpp b/src/akp_local_planner_alg_node.cpp
index a14eb69a19ed42b18e8c57600134f98babe9fba3..e333a77c7db667ef546f6a671b22015672613292 100644
--- a/src/akp_local_planner_alg_node.cpp
+++ b/src/akp_local_planner_alg_node.cpp
@@ -215,7 +215,7 @@ void AkpLocalPlanner::init()
this->tibi_pose_for_sim_ = this->public_node_handle_.advertise<geometry_msgs::PoseWithCovarianceStamped>("tibi_pose_for_sim", 10); // publisher to restart robot pose in simulation
- //this->companionState_publisher_ = this->public_node_handle_.advertise<iri_nav_msgs::companionStateResults>("companionState", 100);
+ this->companionState_publisher_ = this->public_node_handle_.advertise<iri_nav_msgs::companionStateResults>("companionState", 100);
//publisher teleop, para cambiar mis parametros internos.
@@ -1105,40 +1105,40 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
// ESTADO planner.
if(!stop_robot_manually_){
if(output_screen_messages_){ROS_INFO(" no people msg");}
- //companionState_companionStateResults_msg_.numCompanionPeople=0; // if no_people poner .numCompanionPeople=0; // si no encuentro los ids de las person companion
+ companionState_companionStateResults_msg_.numCompanionPeople=0; // if no_people poner .numCompanionPeople=0; // si no encuentro los ids de las person companion
}
}else{
- //companionState_companionStateResults_msg_.numCompanionPeople=num_people_for_state_;//num_people_for_state_; Only one person accompaniment.
+ companionState_companionStateResults_msg_.numCompanionPeople=num_people_for_state_;//num_people_for_state_; Only one person accompaniment.
}
- //this->companionState_companionStateResults_msg_.idsCompanionPeople.resize(0);
+ this->companionState_companionStateResults_msg_.idsCompanionPeople.resize(0);
- // this->companionState_companionStateResults_msg_.idsCompanionPeople.resize(1);
- // companionState_companionStateResults_msg_.idsCompanionPeople[0]=id_person_companion_;
+ this->companionState_companionStateResults_msg_.idsCompanionPeople.resize(1);
+ companionState_companionStateResults_msg_.idsCompanionPeople[0]=id_person_companion_;
- //companionState_companionStateResults_msg_.maximumRobotVelocity=this->planner_.get_max_v_by_system();
+ companionState_companionStateResults_msg_.maximumRobotVelocity=this->planner_.get_max_v_by_system();
Sdestination robot_actual_dest=this->planner_.get_robot_destination();
- //companionState_companionStateResults_msg_.destinations.resize(1);
- //iri_nav_msgs::destination actual_dest_pers_robot;
- //actual_dest_pers_robot.personid=id_person_companion_;
- //actual_dest_pers_robot.position.x=robot_actual_dest.x;
-// actual_dest_pers_robot.position.y=robot_actual_dest.y;
-// actual_dest_pers_robot.position.z=0.0;
- //companionState_companionStateResults_msg_.destinations[0]=actual_dest_pers_robot;
+ companionState_companionStateResults_msg_.destinations.resize(1);
+ iri_nav_msgs::destination actual_dest_pers_robot;
+ actual_dest_pers_robot.personid=id_person_companion_;
+ actual_dest_pers_robot.position.x=robot_actual_dest.x;
+ actual_dest_pers_robot.position.y=robot_actual_dest.y;
+ actual_dest_pers_robot.position.z=0.0;
+ companionState_companionStateResults_msg_.destinations[0]=actual_dest_pers_robot;
// estas dos siguientes era mejor no usarlas y no decir nada. Hubiera sido un incordio y ya se veia que estaba evitando obstaculos.
- //companionState_companionStateResults_msg_.estoyEvitandoObstaculosEstaticos=false; //this->planner_.get_bool_evitando_obst_estaticos();
- //companionState_companionStateResults_msg_.estoyEvitandoObstaculosDynamicos=false;//this->planner_.get_bool_evitando_obst_dynamicos();
- //companionState_companionStateResults_msg_.estateNoPathObstacleClose=this->planner_.get_obstacle_very_close_no_path();
+ companionState_companionStateResults_msg_.estoyEvitandoObstaculosEstaticos=false; //this->planner_.get_bool_evitando_obst_estaticos();
+ companionState_companionStateResults_msg_.estoyEvitandoObstaculosDynamicos=false;//this->planner_.get_bool_evitando_obst_dynamicos();
+ companionState_companionStateResults_msg_.estateNoPathObstacleClose=this->planner_.get_obstacle_very_close_no_path();
bool robot_in_lat=true;
robot_in_lat=true;
- //companionState_companionStateResults_msg_.robotLateral=robot_in_lat;
+ companionState_companionStateResults_msg_.robotLateral=robot_in_lat;
if(output_screen_messages_){
ROS_INFO("(actual) NO path; this->planner_.get_obstacle_very_close_no_path()=%d",this->planner_.get_obstacle_very_close_no_path());
}
- // this->companionState_publisher_.publish(this->companionState_companionStateResults_msg_);
+ this->companionState_publisher_.publish(this->companionState_companionStateResults_msg_);
if((see_std_out_mesages_ros_)||(see_std_out_velocities_ros_)){
std::cout<< "LAST ROBOT VELOCITY (Si V!=0, TODO BIEN) => V.x="<<cmd_vel.linear.x<<"; y="<<cmd_vel.linear.y <<"; W="<< cmd_vel.angular.z<<"; this->config_.speed_k="<<this->config_.speed_k<< std::endl;
}