diff --git a/cfg/AkpLocalPlanner.cfg b/cfg/AkpLocalPlanner.cfg index fa9058e70b7483ec47ab5e728fd1f8125dd0fcc5..1f02fe3cc4f353775634c35073a8975f400cb761 100755 --- a/cfg/AkpLocalPlanner.cfg +++ b/cfg/AkpLocalPlanner.cfg @@ -36,6 +36,9 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("frame_map", str_t, 0, "frame to transform poses to", "map") gen.add("frame_robot_footprint", str_t, 0, "frame to transform poses to", "ana/base_link") +gen.add("fuera_bolitas_goals_companion_markers_conf", bool_t, 0, " Fuera bolas markers", True) +gen.add("conf_debug_all_IVO", bool_t, 0, " All mesagenes to debug IVO", True) +gen.add("conf_debug_few_IVO", bool_t, 0, " Only few velocity messages to debug IVO", False) #### Parameters to start the simulation #### gen.add("Block_parameters_division1" , str_t, 0, "Parameters to start the simulation:","Parameters to start the simulation:") @@ -78,10 +81,11 @@ gen.add("proximity_distance_between_robot_and_person", double_t, 0, "Is the # set-up the best accompaniment angle gen.add("real_companion_angle_SideBySide", double_t, 0, "Is the companion angle set for the case without obstacles", 90, 0.0, 180.0) # set-up the maximum real robot speed -gen.add("max_real_speed_out", double_t, 0, "maximum robot real velocity", 1.0 , 0.0, 2.0) +gen.add("max_real_speed_out", double_t, 0, "maximum robot real velocity", 0.6 , 0.0, 2.0) # set-up the constant of the controller (if your robot velocity controller divides the velocity by 2 or something similar) => Normally, only used in the real robot. +gen.add("max_real_angular_speed_out", double_t, 0, " limite velocidad maxima de salida del robot", 0.4 , 0.0, 2.0) gen.add("speed_k", double_t, 0, " constant multiplying output linear speed", 1.5 , 1.0, 2.0) # For our real robot is needed a speed_k=1.5 - +gen.add("speed_k_angular", double_t, 0, " constant multiplying output linear speed", 1.2 , 1.0, 2.0) #### Parameters to do a slow stop in the real robot, when accompanies a person and this person stops #### diff --git a/config/IVO/move_base_params.yaml b/config/IVO/move_base_params.yaml index 9892cd98504052ded0518c40d32257fc6dfc4366..d835219d266fb29ac8124ec6385b55005f61f3d8 100755 --- a/config/IVO/move_base_params.yaml +++ b/config/IVO/move_base_params.yaml @@ -12,7 +12,7 @@ oscillation_distance: 0.2 recovery_behavior_enabled: false # true clearing_rotation_allowed: false # true -base_local_planner: "iri_atr_akp_local_planner_companion/AkpLocalPlanner" +base_local_planner: "iri_robot_aspsi/AkpLocalPlanner" #alternatives: base_local_planner/TrajectoryPlannerROS, dwa_local_planner/DWAPlannerROS base_global_planner: "navfn/NavfnROS" #alternatives: navfn/NavfnROS, global_planner/GlobalPlanner, carrot_planner/CarrotPlanner diff --git a/include/akp_local_planner_alg_node.h b/include/akp_local_planner_alg_node.h index 2a47dc4f98d955fe42c0eba63b505eccd0da17ea..a6a13a73fd29c651baaafd2cef03beac502e15af 100644 --- a/include/akp_local_planner_alg_node.h +++ b/include/akp_local_planner_alg_node.h @@ -516,11 +516,19 @@ class AkpLocalPlanner : public nav_core::BaseLocalPlanner double dist_betw_rob_and_comp_people_to_slow_velocity_; double ros_max_real_speed_out_; + double ros_max_real_angular_speed_out_; + double number_of_people_in_group_; double num_people_for_state_; bool output_screen_messages_; + + + + bool debug_all_IVO_; + bool debug_few_IVO_; + public: /** * \brief Constructor diff --git a/launch/ivo_launch/ivo_detector_and_tracker.launch b/launch/ivo_launch/ivo_detector_and_tracker.launch index 317858704f652229d4625a48aca4a502fc54af03..81554b7d625bcee810e611e24bb319b3bfea1a42 100644 --- a/launch/ivo_launch/ivo_detector_and_tracker.launch +++ b/launch/ivo_launch/ivo_detector_and_tracker.launch @@ -36,7 +36,7 @@ <arg name="map_name" default="fme_door_open"/> <arg name="use_amcl" default="true"/> <arg name="amcl_config" default="$(arg path)/amcl.yaml"/> - <arg name="initial_x" default="-1.0"/> + <arg name="initial_x" default="-1.5"/> <arg name="initial_y" default="8.0"/> <arg name="initial_yaw" default="-1.57"/> <!-- FME: initial_x=-1.5; initial_y=8.0; initial_yaw=-1.57--> @@ -56,8 +56,8 @@ <!--<arg name="sim_sufix" value="" unless="$(arg public_sim)"/>--> <group if="$(arg use_map)"> - <group if="$(arg use_map_server)"> - <include file="$(find iri_atr_akp_local_planner_companion)/launch/ivo_launch/launchs_internos_modificados/launchs_in_iri_rosnav_needed_for_tiago_robot/launchs_in_iri_rosnav_needed_for_tiago_robot/map_server_tiago.launch"> + <!-- <group if="$(arg use_map_server)"> + <include file="$(find iri_robot_aspsi)/launch/ivo_launch/launchs_internos_modificados/launchs_in_iri_rosnav_needed_for_tiago_robot/launchs_in_iri_rosnav_needed_for_tiago_robot/map_server_tiago.launch"> <arg name="ns" value="$(arg ns)"/> <arg name="map_frame_id" value="$(arg map_frame_id)"/> <arg name="map_name" value="$(arg map_name)"/> @@ -66,10 +66,10 @@ <arg name="output" value="$(arg output)" /> <arg name="launch_prefix" value="$(arg launch_prefix)" /> </include> - </group> + </group>--> <group if="$(arg use_amcl)"> - <include file="$(find iri_atr_akp_local_planner_companion)/launch/ivo_launch/launchs_internos_modificados/iri_rosnav/launch/include/amcl_ivo.launch"> + <include file="$(find iri_robot_aspsi)/launch/ivo_launch/launchs_internos_modificados/iri_rosnav/launch/include/amcl_ivo.launch"> <arg name="ns" value="$(arg ns)"/> <arg name="config" value="$(arg amcl_config)"/> <arg name="map_frame_id" value="$(arg map_frame_id)"/> @@ -130,7 +130,7 @@ <!-- args from iri --> <arg name="rviz" default="true"/> - <arg name="rviz_file" default="$(find iri_atr_akp_local_planner_companion)/rviz/navigation_public_sim_TiagoDual.rviz"/> + <arg name="rviz_file" default="$(find iri_robot_aspsi)/rviz/navigation_public_sim_TiagoDual.rviz"/> <arg name="move_base_params" default="move_base_params.yaml"/> <arg name="costmap_common_params" default="common_params.yaml"/> <arg name="costmap_local_params" default="local_params.yaml"/> diff --git a/launch/ivo_launch/ivo_nav_companion_only.launch b/launch/ivo_launch/ivo_nav_companion_only.launch index 0a8753bb246055f2edc4c2cc9a859aa7e6ca01d7..62518923a944e5ba9649a7d194555d2e0af1f022 100644 --- a/launch/ivo_launch/ivo_nav_companion_only.launch +++ b/launch/ivo_launch/ivo_nav_companion_only.launch @@ -53,7 +53,7 @@ <!-- NEW variables, maybe I need to add them--> <param name="AkpLocalPlanner/detection_laser_obstacle_distances" value="3.5"/> - <param name="AkpLocalPlanner/real_companion_angle_SideBySide" value="70.0"/> <!-- No lo setea bien desde aquÃ, si hace falta, mejor cambiarlo en el dynamic reconfigure.--> + <param name="AkpLocalPlanner/real_companion_angle_SideBySide" value="90.0"/> <!-- No lo setea bien desde aquÃ, si hace falta, mejor cambiarlo en el dynamic reconfigure.--> <param name="AkpLocalPlanner/speed_k" value="1.0"/> <!--MUY importante: en sim, speed_k=1.0; en robot real, speed_k=1.5; --> <param name="AkpLocalPlanner/speed_k_angular" value="1.0"/> <param name="AkpLocalPlanner/conf_id_to_remove" value="100000"/> diff --git a/src/akp_local_planner_alg_node.cpp b/src/akp_local_planner_alg_node.cpp index fe0c6db5f08a48b6fff2b2409e62db34e03f4f1a..1d14d327f6fb513f82908768b6c90a13dc30171e 100644 --- a/src/akp_local_planner_alg_node.cpp +++ b/src/akp_local_planner_alg_node.cpp @@ -64,7 +64,9 @@ AkpLocalPlanner::AkpLocalPlanner(void) : debug_sideBySide2019_(false), first_iter_for_update_scene_(true), output_screen_messages_(false), -simulation_(false) + simulation_(false), + debug_all_IVO_(false), + debug_few_IVO_(false) { ROS_INFO("CREATE ALG NODE"); @@ -452,9 +454,9 @@ void AkpLocalPlanner::initialize(std::string name, tf2_ros::Buffer* tf, costmap_ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) { - //if(output_screen_messages_){ - //ROS_INFO(" NODE iri_robot_aspsi this->planner_.get_number_of_vertex( )=%d; horizon_time=%f; ",this->planner_.get_number_of_vertex( ),this->planner_.get_horizon_time()); - //} //conf_dist_betw_rob_and_comp_people_to_slow_velocity + if(debug_all_IVO_){ + ROS_INFO(" NODE iri_robot_aspsi this->planner_.get_number_of_vertex( )=%d; horizon_time=%f; ",this->planner_.get_number_of_vertex( ),this->planner_.get_horizon_time()); + } //conf_dist_betw_rob_and_comp_people_to_slow_velocity //ROS_INFO("simulation_=%d; ",simulation_); @@ -799,15 +801,20 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) double START_time_secs2 =ros::Time::now().toSec(); - if(output_screen_messages_){ - //ROS_INFO( "1 (IMPORTANTE after person plan y con person v, reduction) dt_real=%f;", ros_time_to_sec_general_before_iter_-ros_time_to_sec); - } + //if(debug_all_IVO_){ + // ROS_INFO( "1 (IMPORTANTE after person plan y con person v, reduction) dt_real=%f;", ros_time_to_sec_general_before_iter_-ros_time_to_sec); + //} // Function that computes the local-planner, the Robot's Adaptive Side-by-Side Accompaniment of One Person (ASPSI) robot_plan_succed = this->planner_.robot_plan_companion3(best_next_pose,reactive_); ros_time_to_sec_general_before_iter_=ros::Time::now().toSec(); + if(debug_all_IVO_){ + ROS_INFO( " (IMPORTANTE after person plan y con person v, reduction) d_r_p1=%f; init_V_robot=%f", this->robot_pose_.distance(person_companion_position_),best_next_pose.v ); + } + + /* LIMIT velocity if very near of people_companion's!!!!! */ if(this->robot_pose_.distance(person_companion_position_)<dist_betw_rob_and_comp_people_to_slow_velocity_ ){ // distance entre robot y personas que acompaña, menor que 2 metros. dist_betw_rob_and_comp_people_to_slow_velocity_=4 @@ -816,6 +823,11 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) v_people=person_companion_position_.v(); v_lim_actual=v_people+0.2; // v_p + 0.2 m/s + if((debug_all_IVO_)||(debug_few_IVO_)){ + ROS_INFO( "LIMIT velocity if very near of people_companion's (IMPORTANTE antes person plan y con person v, reduction) v_people=%f; v_p1=%f; v_p2=%f;", v_people, person_companion_position_.v() , second_person_companion_position_.v()); + } + + if((best_next_pose.v>v_lim_actual)&&(v_people>0.2)){ best_next_pose.v=v_lim_actual; } @@ -860,7 +872,7 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) START_time_secs10 =ros::Time::now().toSec(); - if(output_screen_messages_){ + if(debug_all_IVO_){ ROS_INFO( "2 after robot_plan_companion3; this->move_base=%d this->robot_pose_.v=%f; this->robot_pose_.w=%f",this->move_base ,this->robot_pose_.v, this->robot_pose_.w); } @@ -894,7 +906,7 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) best_next_pose_robot_companion_markers_.id = 30; // MarkerArray_msg_.markers.push_back( best_next_pose_robot_companion_markers_ ); //esfera que marca el siguiente goal del robot, hacia donde va! - if(output_screen_messages_){ + if(fuera_bolitas_goals_companion_markers_){ MarkerArray_msg_comp_markers_.markers.push_back( best_next_pose_robot_companion_markers_ ); } double END_time_secs5; @@ -1018,14 +1030,41 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) cmd_vel.linear.x*=this->config_.speed_k; // IMPORTANT, do not change for TIBI. It is for real robot. OUR velocity controller divides the robot's velocity by 2. Then, we need to multiply with this speed_k here. + cmd_vel.angular.z*=this->config_.speed_k_angular; if((see_std_out_mesages_ros_)||(see_std_out_velocities_ros_)){ ROS_INFO( " cmd_vel.linear.x * Speed_k (cmd_vel=%f) ", cmd_vel.linear.x); } - if(ros_max_real_speed_out_<cmd_vel.linear.x){ - cmd_vel.linear.x=ros_max_real_speed_out_; - } + //reducir velocidad lineal final con el maximo valor de salida. + if(ros_max_real_speed_out_<cmd_vel.linear.x){ + cmd_vel.linear.x=ros_max_real_speed_out_; + if((debug_all_IVO_)||(debug_few_IVO_)){ + ROS_INFO( "(positive) cmd_vel.linear.x max_LIMIT (cmd_vel_linear=%f) ", cmd_vel.linear.x); + } + } + if((-ros_max_real_speed_out_)>cmd_vel.linear.x){ + cmd_vel.linear.x=-ros_max_real_speed_out_; + if((debug_all_IVO_)||(debug_few_IVO_)){ + ROS_INFO( "(negative) cmd_vel.linear.x max_LIMIT (cmd_vel_linear=%f) ", cmd_vel.linear.x); + } + } + //reducir velocidad anglular final con el maximo valor de salida. + + if(ros_max_real_angular_speed_out_<cmd_vel.angular.z){ + cmd_vel.angular.z=ros_max_real_angular_speed_out_; + if((debug_all_IVO_)||(debug_few_IVO_)){ + ROS_INFO( " (positive) cmd_vel.angular.z max_LIMIT (cmd_vel_angular=%f) ", cmd_vel.angular.z); + } + + } + if((-ros_max_real_angular_speed_out_)>cmd_vel.angular.z){ + cmd_vel.angular.z=-ros_max_real_angular_speed_out_; + if((debug_all_IVO_)||(debug_few_IVO_)){ + ROS_INFO( " (negative) cmd_vel.angular.z max_LIMIT (cmd_vel_angular=%f) ", cmd_vel.angular.z); + } + + } //to stop slowly: @@ -1050,9 +1089,9 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) cmd_vel.angular.z = 0.0; } - if(output_screen_messages_){ + if((debug_all_IVO_)||(debug_few_IVO_)){ ROS_ERROR(" (out) STOP slowly ROBOT!; step_decreace_velocity_stop_robot_=%f, cmd_vel.linear.x=%f; this->robot_pose_.v=%f",step_decreace_velocity_stop_robot_,cmd_vel.linear.x,this->robot_pose_.v); - } + } } //TODO: caso Vr>0.5 o 0.6 y se va la velocidad a 0.0 de golpe! tambien he de frenar lento! (hay que incluirlo aquà al final, gestion de cambios muy bruscos de velocidad) @@ -1153,7 +1192,7 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) ///FIN USE diferent params for side-by-side central and lateral, for people companion! - if(output_screen_messages_){ + if((debug_all_IVO_)||(debug_few_IVO_)){ ROS_ERROR(" (LAST velocity robot) cmd_vel.linear.x=%f; cmd_vel.linear.y=%f this->robot_pose_.v=%f; cmd_vel.angular.z=%f; robot_plan_succed=%d",cmd_vel.linear.x,cmd_vel.linear.y,this->robot_pose_.v,cmd_vel.angular.z,robot_plan_succed); } @@ -1573,6 +1612,12 @@ void AkpLocalPlanner::reconfigureCallback(iri_robot_aspsi::AkpLocalPlannerConfig config.id_person_companion=this->config_.id_person_companion; }*/ + debug_all_IVO_=config.conf_debug_all_IVO; + this->planner_.set_debug_few_IVO(config.conf_debug_all_IVO); + debug_few_IVO_=config.conf_debug_few_IVO; + this->planner_.set_debug_few_IVO(config.conf_debug_few_IVO); + + /// Fin Selec closes person as companion!!! std::vector<double> params(5,0.0); //params_side_by_side_central_.reserve(5); @@ -1757,7 +1802,8 @@ ROS_INFO("AkpLocalPlanner::scan2points:transform: frame_map_: %s-->frame_robot_f // limit final speed robot. ros_max_real_speed_out_=config.max_real_speed_out; - //fuera_bolitas_goals_companion_markers_=config.fuera_bolitas_goals_companion_markers_conf; + ros_max_real_angular_speed_out_=config.max_real_angular_speed_out; + fuera_bolitas_goals_companion_markers_=config.fuera_bolitas_goals_companion_markers_conf; // INI select closes person as companion (CASE 1 People) if(config.select_near_pers_as_companion_one_person){