From afd394277a992061820717decf68eaa263c105fb Mon Sep 17 00:00:00 2001
From: Ely Repiso Polo <erepiso@iri.upc.edu>
Date: Mon, 17 May 2021 11:39:21 +0000
Subject: [PATCH] add changes in robot ASP-SI, only one person accompaniment,
last version in the docker. This version works perfectly.
---
include/akp_local_planner_alg_node.h | 4 +--
launch/gazebo_ASPSI_OK_ana.launch | 22 ++++++++++++++++
launch/gazebo_ASPSI_OK_ana_brl.launch | 24 +++++++++++++++++
src/akp_local_planner_alg_node.cpp | 38 +++++++++++++--------------
4 files changed, 67 insertions(+), 21 deletions(-)
diff --git a/include/akp_local_planner_alg_node.h b/include/akp_local_planner_alg_node.h
index 2724e6a..50c3ef7 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 ba1860a..3db1b61 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 8361d8f..6d60eb1 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 a14eb69..e333a77 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;
}
--
GitLab