diff --git a/launch/tibi_dabo_state_machine_companion.launch b/launch/tibi_dabo_state_machine_companion.launch
index c841c77608615b76bd6035ff44da96e0d16372c3..f93b0f7d36f2e838795ecb329e805cf61b540e13 100644
--- a/launch/tibi_dabo_state_machine_companion.launch
+++ b/launch/tibi_dabo_state_machine_companion.launch
@@ -12,14 +12,14 @@
<!-- service clients: -->
<!-- service servers: -->
<!-- action clients: -->
- <!-- action servers: -->
+ <!-- action servers:name="state_mach_comp"-->
+ <!--respawn="true"-->
<!--output="screen"-->
<node pkg ="iri_state_machine_companion"
type="iri_state_machine_companion"
name="state_mach_comp"
- respawn="true"
- machine="nav"
- >
+ machine="visio"
+ output="screen">
<remap from="/$(env ROBOT)/state_mach_comp/people_tracks"
to="/$(env ROBOT)/mht/tracks" />
<remap from="/$(env ROBOT)/state_mach_comp/odom"
@@ -28,7 +28,7 @@
to="/$(env ROBOT)/companionState" />
<!-- <remap from="/$(env ROBOT)/tibi_akp_status"
to="/$(env ROBOT)/tibi_akp_status" /> -->
- <param name="~/threshold_max_dist_between_robot_and_person_conf" value="5.0" />
+ <param name="~/threshold_max_dist_between_robot_and_person_conf" value="5.0" />
</node>
</group>
diff --git a/src/iri_state_machine_companion_alg_node.cpp b/src/iri_state_machine_companion_alg_node.cpp
index 36e1f800c6de493d16e6b73b3fb47197e0d3c2cb..74d1c09a5a379a1049a4a4bcfefebd79f523266c 100644
--- a/src/iri_state_machine_companion_alg_node.cpp
+++ b/src/iri_state_machine_companion_alg_node.cpp
@@ -5,7 +5,7 @@ IriStateMachineCompanionAlgNode::IriStateMachineCompanionAlgNode(void) :
{
//init class attributes if necessary
this->loop_rate_ = 2;//in [Hz] TODO: ver a cuantos ercios ir.
-
+ROS_INFO("ini node");
this->fixed_frame = "/map";
this->robot_frame = "/tibi/base_link";
@@ -28,7 +28,7 @@ IriStateMachineCompanionAlgNode::IriStateMachineCompanionAlgNode(void) :
// [init clients]
// [init action servers]
-
+ROS_INFO("node created");
// [init action clients]
}
@@ -83,32 +83,36 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
companionState_companionStateResults_msg_.estoyEvitandoObstaculosDynamicos=this->planner_.get_bool_evitando_obst_dynamicos();
this->companionState_publisher_.publish(this->companionState_companionStateResults_msg_); */
+
+ ROS_INFO("error1");
this->tibi_akp_status_UInt64_msg_.data =0; // case no status.
+ ROS_INFO("error2");
unsigned int actual_numCompanionPeople=save_companionStatus_mesage_global_.numCompanionPeople;
if(actual_numCompanionPeople==0){
this->tibi_akp_status_UInt64_msg_.data =1; // status no people.
}
-
+ ROS_INFO("error3");
// TODO status fake person ==2 and status return other goal ==3
bool aboiding_stat_obst = save_companionStatus_mesage_global_.estoyEvitandoObstaculosEstaticos;
-
+ ROS_INFO("error4");
if(aboiding_stat_obst==true){
this->tibi_akp_status_UInt64_msg_.data =4; // aboiding static obstacles
}
-
+ ROS_INFO("error5");
bool aboiding_dyn_obst = save_companionStatus_mesage_global_.estoyEvitandoObstaculosDynamicos;
-
+ ROS_INFO("error6");
if(aboiding_dyn_obst==true){
this->tibi_akp_status_UInt64_msg_.data =5; // aboiding dynamic obstacles
}
+ ROS_INFO("error7");
if(!save_companionStatus_mesage_global_.idsCompanionPeople.empty()){
id_person_companion_=save_companionStatus_mesage_global_.idsCompanionPeople[0];
if(save_companionStatus_mesage_global_.idsCompanionPeople.size()>1){
id_second_person_companion_=save_companionStatus_mesage_global_.idsCompanionPeople[1];
}
}
-
+ ROS_INFO("error8");
// TODO: status side-by-side. ==6
double person_companion_velocity=0.0;
double distance_robot_person_comp;
@@ -119,17 +123,19 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
}
//std::cout<< "this->obs; id=" <<this->obs[u].id<<"; x="<<this->obs[u].x<<"; y="<<this->obs[u].y<<"; vx="<<this->obs[u].vx<<"; vy="<<this->obs[u].vy<< std::endl;
}
+
+ ROS_INFO("error9");
double max_robot_velocity=save_companionStatus_mesage_global_.maximumRobotVelocity;
if(max_robot_velocity < person_companion_velocity){
this->tibi_akp_status_UInt64_msg_.data =7; // status walk slowly, please. I can not follow you.
}
-
+ ROS_INFO("error10");
threshold_max_dist_between_robot_and_person_=config_.threshold_max_dist_between_robot_and_person_conf;
if(distance_robot_person_comp > threshold_max_dist_between_robot_and_person_){
this->tibi_akp_status_UInt64_msg_.data =8; //status wait for me, please.
}
-
+ ROS_INFO("11");
//this->tibi_akp_status_UInt64_msg_.data = my_var;
@@ -193,13 +199,16 @@ void IriStateMachineCompanionAlgNode::odom_callback(const nav_msgs::Odometry::Co
std::string target_frame = this->fixed_frame;
std::string source_frame = this->robot_frame;
ros::Time target_time = ros::Time::now();
-
+ROS_INFO("odom1");
double time_start=ros::Time::now().toSec();
try
{
+ ROS_INFO("odom2");
bool tf_exists = this->tf_->waitForTransform(target_frame, source_frame, target_time, ros::Duration(1), ros::Duration(0.01));
- if(tf_exists)
+ ROS_INFO("odom2.1");
+ if(tf_exists)
{
+ ROS_INFO("odom3");
geometry_msgs::PoseStamped poseIn;
geometry_msgs::PoseStamped poseOut;
poseIn.header.stamp = target_time;
@@ -209,13 +218,14 @@ void IriStateMachineCompanionAlgNode::odom_callback(const nav_msgs::Odometry::Co
actual_odom_robot_pose.x = poseOut.pose.position.x;
actual_odom_robot_pose.y = poseOut.pose.position.y;
actual_odom_robot_pose.time_stamp = poseOut.header.stamp.toSec();
-
+ROS_INFO("odom4");
//vector of the orientation
poseIn.pose.position.x = 1.0;
this->tf_->transformPose(target_frame, poseIn, poseOut);
actual_odom_robot_pose.theta = atan2(poseOut.pose.position.y - this->robot_pose_.y , poseOut.pose.position.x - this->robot_pose_.x);
//this->planner_.update_robot(this->robot_pose_);//updated in main
//ROS_DEBUG("IriStateMachineCompanionAlgNode::odom_callback: robot pose x,y,th,v,w: %f, %f, %f, %f, %f", this->robot_pose_.x, this->robot_pose_.y, this->robot_pose_.theta, this->robot_pose_.v, this->robot_pose_.w);
+ ROS_INFO("odom5");
}
else
{
@@ -232,14 +242,14 @@ void IriStateMachineCompanionAlgNode::odom_callback(const nav_msgs::Odometry::Co
//ROS_INFO("IriStateMachineCompanionAlgNode::odom_callback: tf_time=%f", time_end-time_start);
-
+ROS_INFO("odom6");
this->odom_mutex_enter();
this->robot_pose_=actual_odom_robot_pose;
this->odom_mutex_exit();
-
+ROS_INFO("odom7");
//ROS_INFO("AkpLocalPlanner::odom_callback: x=%f; y=%f; theta=%f; v=%f; w=%f ",this->robot_pose_.x,this->robot_pose_.y,this->robot_pose_.theta,this->robot_pose_.v,this->robot_pose_.w);
//ROS_INFO("AkpLocalPlanner::odom_callback: Exit");