diff --git a/launch/Ana_launchs/BRL/gazebo_ASPSI_OK_ana_brl.launch b/launch/Ana_launchs/BRL/gazebo_ASPSI_OK_ana_brl.launch new file mode 100644 index 0000000000000000000000000000000000000000..6d60eb1d05b36cca6bf4f8a7f7b19cc236c083f7 --- /dev/null +++ b/launch/Ana_launchs/BRL/gazebo_ASPSI_OK_ana_brl.launch @@ -0,0 +1,75 @@ +<!-- --> +<launch> + + + <!-- Ini Navigation Companion Nodes --> + <!--launch-prefix="xterm -e ddd -args" --> + + <group ns="$(optenv ROBOT ana)"> + <rosparam command="delete" ns="/ana/move_base" /> + + <!--launch-prefix="xterm -e ddd -args" machine="visio"--> + <node pkg ="move_base" + type="move_base" + name="move_base" + output="screen"> + <remap from="/ana/move_base_ana/cmd_vel" + to="/ana/navigation/cmd_vel" /> + <remap from="/ana/move_base_ana/odom" + to="/ana/odom" /> + <remap from="/ana/front_scan" + to="/ana/sensors/scan" /> + <remap from="/ana/rear_scan" + to="/ana/sensors/scan" /> + <remap from="/ana/tracks" + to="/ana/mht/tracks" /> + <rosparam file="$(find iri_robot_aspsi)/config/ana2/move_base_params_aspsi.yaml" command="load" /> + <rosparam file="$(find iri_robot_aspsi)/config/ana2/ana/costmap_common_params.yaml" command="load" ns="global_costmap" /> + <rosparam file="$(find iri_robot_aspsi)/config/ana2/costmap_common_params.yaml" command="load" ns="local_costmap" /> + <rosparam file="$(find iri_robot_aspsi)/config/ana2/local_costmap_params.yaml" command="load" /> + <rosparam file="$(find iri_robot_aspsi)/config/ana2/ana/global_costmap_params.yaml" command="load" /> + <rosparam file="$(find iri_robot_aspsi)/config/ana2/akp_local_planner_params.yaml" command="load" /> +<rosparam file="$(find iri_ana_rosnav)/params/amcl_companion_test_ely2021_mapFME.yaml" command="load" /> + <!--<rosparam file="$(find iri_dabo_rosnav)/params/global_planner/global_planner_params.yaml" command="load" /> + <rosparam file="$(find iri_dabo_rosnav)/params/mux.yaml" command="load" />--> + <param name="AkpLocalPlanner/destination_map_path" value="$(find iri_robot_aspsi)/maps/master_big_destinations_Gazebo_sim_more_dest.txt" /> + <param name="AkpLocalPlanner/results_filename_Zanlungo" value="name_file.txt" /> + <param name="AkpLocalPlanner/robot" value="ana"/> + <param name="AkpLocalPlanner/person_goal_id" value="200"/> + <param name="AkpLocalPlanner/speed_k" value="1"/> + <param name="AkpLocalPlanner/person_radi_amp" value="0.2"/> + <param name="AkpLocalPlanner/change_treshold_distance_between_steps" value="0.3"/> + <!-- for SIM--> + <param name="AkpLocalPlanner/change_sim" value="False"/> + <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/Ana_launchs/BRL/sim_sample_companion_with_person_brl.launch b/launch/Ana_launchs/BRL/sim_sample_companion_with_person_brl.launch new file mode 100644 index 0000000000000000000000000000000000000000..c7d7b43b56d0d6ea4976027c34114db17e0f3291 --- /dev/null +++ b/launch/Ana_launchs/BRL/sim_sample_companion_with_person_brl.launch @@ -0,0 +1,90 @@ +<?xml version="1.0" encoding="UTF-8"?> +<launch> + + <arg name="ns" default="ana"/> + <arg name="world_name" default="master_big"/> + <arg name="world_description" default="true"/> + <arg name="cmd_vel_mux_config" default="$(find iri_ana_gazebo)/config/mux.yaml"/> + <arg name="rviz" default="false"/> + <arg name="gui" default="true"/> + + <arg name="pan_tilt_controller_config_file" default="$(find iri_ana_gazebo)/config/ana_pan_tilt_sim_config.yaml" /> + <arg name="joint_trajectory_config_file" default="$(find iri_ana_gazebo)/config/ana_joint_trajectory_gazebo_config.yaml"/> + <arg name="pan_tilt_effort_controller_ns" default="effort_controller" /> + <arg name="pan_tilt_position_controller_ns" default="position_controller" /> + + <include file="$(find iri_gazebo_worlds)/launch/test.launch"> + <arg name="gui" value="$(arg gui)"/> + <arg name="world_name" value="$(arg world_name)"/> + <arg name="description" value="$(arg world_description)"/> + <arg name="rviz" value="false"/> + </include> + + <include file="$(find iri_objects_description)/launch/spawn_person.launch"> + <arg name="name" value="person1"/> + <arg name="type" value="person"/> + <arg name="x" value="12.5"/> <!--changes x component of the initial person2's position in the environment (in map coord)--> + <arg name="y" value="20.0"/> <!--changes y component of the initial person2's position inside the environment (in map coord)--> + <arg name="yaw" value="0.0"/> <!--changes the initial person2's orientation in the environment (it is in map coordinates)--> + </include> + + <include file="$(find iri_ana_gazebo)/launch/spawn.launch"> + <arg name="ns" value="$(arg ns)"/> + <arg name="model" value="ana"/> + <arg name="x" value="12.5"/> + <arg name="y" value="22.0"/> + <arg name="yaw" value="0.0"/> + <arg name="pan_tilt_controller_config_file" value="$(arg pan_tilt_controller_config_file)"/> + <arg name="joint_trajectory_config_file" value="$(arg joint_trajectory_config_file)"/> + <arg name="pan_tilt_effort_controller_ns" value="$(arg pan_tilt_effort_controller_ns)"/> + <arg name="pan_tilt_position_controller_ns" value="$(arg pan_tilt_position_controller_ns)"/> + </include> + +<!-- +<include file="$(find iri_objects_description)/launch/spawn_person.launch"> + <arg name="name" value="person2"/> + <arg name="type" value="person"/> + <arg name="x" value="27.0"/> + <arg name="y" value="22.0"/> + <arg name="yaw" value="3.14"/> +</include> + +<include file="$(find iri_objects_description)/launch/spawn_person.launch"> + <arg name="name" value="person3"/> + <arg name="type" value="person"/> + <arg name="x" value="40.5"/> + <arg name="y" value="17.5"/> + <arg name="yaw" value="3.14"/> +</include> + +<include file="$(find iri_objects_description)/launch/spawn_person.launch"> + <arg name="name" value="person4"/> + <arg name="type" value="person"/> + <arg name="x" value="40.0"/> + <arg name="y" value="21.5"/> + <arg name="yaw" value="3.14"/> + </include> +--> + + <include file="$(find iri_rosnav)/launch/include/cmd_vel_mux.launch"> + <arg name="ns" value="$(arg ns)"/> + <arg name="nodelet_manager_name" value="mux_nodelet_manager"/> + <arg name="config" value="$(arg cmd_vel_mux_config)"/> + <arg name="output" value="log" /> + <arg name="launch_prefix" value="" /> + </include> + + <node name="cmd_vel_relay" + pkg="topic_tools" + type="relay" + args="/ana/ana_odom/cmd_vel_in /ana/cmd_vel"> + </node> + + <node name="rviz" + pkg="rviz" + type="rviz" + if="$(arg rviz)" + args="-d $(find iri_ana_gazebo)/rviz/$(arg ns).rviz"> + </node> + +</launch> diff --git a/launch/Ana_launchs/BRL/tracker_nav_and_detection_for_ASPSI_brl.launch b/launch/Ana_launchs/BRL/tracker_nav_and_detection_for_ASPSI_brl.launch new file mode 100644 index 0000000000000000000000000000000000000000..ced6e81d321a159e31a8bbda78765d51c95469e0 --- /dev/null +++ b/launch/Ana_launchs/BRL/tracker_nav_and_detection_for_ASPSI_brl.launch @@ -0,0 +1,191 @@ +<!-- --> +<launch> + + <arg name="rviz" default="true"/> + <arg name="rviz_file" default="$(find iri_ana_rosnav)/rviz/ana.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"/> + <arg name="costmap_global_params" default="global_params.yaml"/> + <arg name="map_name" default="master_big"/> + <arg name="initial_x" default="20.5"/> + <arg name="initial_y" default="39.0"/> + <arg name="initial_yaw" default="0.0"/> + <arg name="local_planner" default="dwa"/> + <arg name="global_planner" default="global_planner"/> + <arg name="output" default="screen" /> + <arg name="launch_prefix" default="" /> + + <include file="$(find iri_rosnav)/launch/nav.launch"> + <arg name="ns" value="ana"/> + <arg name="path" value="$(find iri_ana_rosnav)/params"/> + <arg name="move_base_params" value="$(arg move_base_params)"/> + <arg name="costmap_common_params" value="$(arg costmap_common_params)"/> + <arg name="costmap_local_params" value="$(arg costmap_local_params)"/> + <arg name="costmap_global_params" value="$(arg costmap_global_params)"/> + <arg name="map_frame_id" value="map"/> + <arg name="odom_frame_id" value="ana/odom"/> + <arg name="base_frame_id" value="ana/base_footprint"/> + <arg name="map_topic" value="/ana/map"/> + <arg name="map_service" value="/ana/static_map"/> + <arg name="odom_topic" value="/ana/odom"/> + <arg name="cmd_vel_topic" value="/ana/navigation/cmd_vel"/> + <arg name="scan_topic" value="/ana/sensors/scan"/> + <arg name="use_map" value="true"/> + <arg name="use_map_server" value="true"/> + <arg name="map_name" value="$(arg map_name)"/> + <arg name="use_amcl" value="true"/> + <arg name="amcl_config" value="$(find iri_ana_rosnav)/params/amcl_companion_test_ely2021_mapFME.yaml"/> + <arg name="initial_x" value="$(arg initial_x)"/> + <arg name="initial_y" value="$(arg initial_y)"/> + <arg name="initial_yaw" value="$(arg initial_yaw)"/> + <arg name="use_fake_loc" value="false"/> + <arg name="use_gmapping" value="false"/> + <arg name="local_planner" value="$(arg local_planner)"/> + <arg name="global_planner" value="$(arg global_planner)"/> + <arg name="output" value="$(arg output)" /> + <arg name="launch_prefix" value="$(arg launch_prefix)" /> + </include> + + <include file="$(find iri_rosnav)/launch/include/pointcloud_to_laserscan.launch"> + <arg name="ns" value="ana"/> + <arg name="node_name" value="pcl_to_laserscan"/> + <arg name="scan_topic" value="/ana/sensors/scan"/> + <arg name="cloud_topic" value="/ana/sensors/velodyne_points"/> + <arg name="config_file" value="$(find iri_ana_rosnav)/params/pointcloud_to_laserscan.yaml" /> + <arg name="output" value="$(arg output)" /> + <arg name="launch_prefix" value="$(arg launch_prefix)" /> + </include> + +<group ns="$(optenv ROBOT ana)"> + <!-- People Tracking MHT (remap) --> + <!-- published topics: /$(env ROBOT)/mht/tracks --> + <!-- subscribed topics: /$(env ROBOT)/detections --> + <!-- service clients: --> + <!-- service servers: --> + <!-- action clients: --> + <!-- action servers: --> + <node pkg ="iri_people_tracking_mht" + type="iri_people_tracking_mht" + name="mht" + output="screen"> + <remap from="/ana/mht/detections" + to="/ana/lpd/peopleMapFiltered" /> + <remap from="/ana/mht/odom" + to="/ana/odom" /> <!-- /topic-odometry-of-the-robot --> + <remap from="/ana/mht/odom_for_medium_velocity" + to="/ana/odom" /> <!-- /topic-odometry-of-the-robot --> + + <!-- People Tracking MHT parameters for outdoor environments with few false alarms--> + <param name="~/frame_id" type="string" value="map" /> <!-- tf of the map for the tracker --> + <param name="local_tracker" type="bool" value="false"/> + <param name="~/association_threshold" type="double" value="1.0" /> + <param name="~/confirmation_threshold" type="double" value="0.9" /> + <param name="~/deletion_threshold" type="double" value="0.4" /> + <param name="~/laser_probability" type="double" value="0.9" /> + <param name="~/laser_false_alarm" type="double" value="0.1" /> + <param name="~/laser_new_track" type="double" value="0.11" /> + <param name="~/laser_no_detection" type="double" value="0.97" /> + <param name="~/laser_no_confirmed" type="double" value="0.02" /> + <param name="~/laser_no_confirmed_iteration" type="double" value="0.01" /> + <param name="~/augment_covariance_track" type="double" value="0.5" /> + <param name="~/mht_velocity_margin" type="double" value="1.0" /> + <param name="~/covariance_no_track" type="double" value="5.0" /> + </node> + + <!-- Laser People Detector (remap) --> + <!-- published topics: /$(env ROBOT)/people --> + <!-- subscribed topics: /$(env ROBOT)/sensors/front_laser_scan --> + <!-- service clients: --> + <!-- service servers: --> + <!-- action clients: --> + <!-- action servers: --> + <node pkg ="iri_laser_people_detection" + type="iri_laser_people_detection" + name="lpd" + output="screen" + > + + <param name="~/rangeThreshold" type="double" value="0.0" /> + <param name="~/detectionThreshold" type="double" value="-0.07" /> + <param name="~/personRadius" type="double" value="0.7" /> + <!-- load different trained detectors (to switch at rangeThreshold) --> + <param name="~/boostFilePath" type="string" value="/usr/local/include/iri/laser_people_detection/boostData/boostFile.txt" /> + <param name="~/boostFilePath2" type="string" value="/usr/local/include/iri/laser_people_detection/boostData/boostFile2.txt" /> + <!-- transform detections to base_link frame --> + <param name="~/selectPosesFrame" type="bool" value="False" /> + <param name="~/posesFrame" type="string" value="/scan" /> <!-- frame where you want the poses --> + <param name="~/selectScanFrame" type="bool" value="False" /> + <param name="~/scanFrame" type="string" value="/scan" /> <!-- frame of the scan --> + <!-- visualization --> + <param name="~/markerWidth" type="double" value="0.7" /> + <param name="~/markerHeight" type="double" value="0.4" /> + <param name="~/markerR" type="double" value="0.0" /> + <param name="~/markerG" type="double" value="0.0" /> + <param name="~/markerB" type="double" value="0.5" /> + <param name="~/markerA" type="double" value="0.5" /> + <!-- filter detections by range/x/y --> + <param name="~/filterPosesMode" type="bool" value="False" /> + <param name="~/filterR" type="double" value="0.0" /> + <param name="~/filterX" type="double" value="0.0" /> + <param name="~/filterY" type="double" value="0.0" /> + + <remap from="/ana/lpd/scan" + to="/ana/sensors/scan" /> <!-- topic: scan del robot --> + <remap from="/ana/lpd/people" + to="/ana/lpd/people" /> + </node> + + + + <!-- Laser People map Filter --> + <!-- published topics: /$(env ROBOT)/peopleFiltered --> + <!-- subscribed topics: /$(env ROBOT)/people --> + <!-- /$(env ROBOT)map --> + <!-- service clients: --> + <!-- service servers: --> + <!-- action clients: --> + <!-- action servers: --> + <node pkg ="iri_laser_people_map_filter" + type="iri_laser_people_map_filter" + name="lpmf"> + + <param name="~/filter" type="bool" value="True" /> <!-- true to enable filtering when we have a map --> + <param name="~/neighborRadius" type="double" value="0.3" /> + <param name="~/markerWidth" type="double" value="0.7" /> + <param name="~/markerHeight" type="double" value="1.5" /> + <param name="~/markerR" type="double" value="1.0" /> + <param name="~/markerG" type="double" value="1.0" /> + <param name="~/markerB" type="double" value="0.0" /> + <param name="~/markerA" type="double" value="0.9" /> + <param name="~/markerLifeTime" type="double" value="0.1" /> + + + <remap from="/ana/lpmf/people" + to="/ana/lpd/people" /> + <remap from="/ana/lpmf/people_out" + to="/ana/lpd/peopleMapFiltered" /> + <remap from="/ana/lpmf/map" + to="/ana/map" /> <!-- topic of the map --> + + </node> + </group> + + <!-- Ini Navigation Companion Nodes --> + + + + <!-- <node name="rviz" + pkg="rviz" + type="rviz" + if="$(arg rviz)" + args="-d $(arg rviz_file)"> + </node>--> + +<node name="rviz" + pkg="rviz" + type="rviz" + if="$(arg rviz)" + args="-d $(find iri_robot_aspsi)/rviz/ana.rviz"> + </node> +</launch> diff --git a/launch/Ana_launchs/FME/gazebo_ASPSI_OK_ana.launch b/launch/Ana_launchs/FME/gazebo_ASPSI_OK_ana.launch new file mode 100644 index 0000000000000000000000000000000000000000..3db1b61dbf31bcfab7709dcdf75f9b1ed3af0bcf --- /dev/null +++ b/launch/Ana_launchs/FME/gazebo_ASPSI_OK_ana.launch @@ -0,0 +1,73 @@ +<!-- --> +<launch> + + + <!-- Ini Navigation Companion Nodes --> + <!--launch-prefix="xterm -e ddd -args" --> + + <group ns="$(optenv ROBOT ana)"> + <rosparam command="delete" ns="/ana/move_base" /> + + <!--launch-prefix="xterm -e ddd -args" machine="visio"--> + <node pkg ="move_base" + type="move_base" + name="move_base" + output="screen"> + <remap from="/ana/move_base_ana/cmd_vel" + to="/ana/navigation/cmd_vel" /> + <remap from="/ana/move_base_ana/odom" + to="/ana/odom" /> + <remap from="/ana/front_scan" + to="/ana/sensors/scan" /> + <remap from="/ana/rear_scan" + to="/ana/sensors/scan" /> + <remap from="/ana/tracks" + to="/ana/mht/tracks" /> + <rosparam file="$(find iri_robot_aspsi)/config/ana2/move_base_params_aspsi.yaml" command="load" /> + <rosparam file="$(find iri_robot_aspsi)/config/ana2/ana/costmap_common_params.yaml" command="load" ns="global_costmap" /> + <rosparam file="$(find iri_robot_aspsi)/config/ana2/costmap_common_params.yaml" command="load" ns="local_costmap" /> + <rosparam file="$(find iri_robot_aspsi)/config/ana2/local_costmap_params.yaml" command="load" /> + <rosparam file="$(find iri_robot_aspsi)/config/ana2/ana/global_costmap_params.yaml" command="load" /> + <rosparam file="$(find iri_robot_aspsi)/config/ana2/akp_local_planner_params.yaml" command="load" /> +<rosparam file="$(find iri_ana_rosnav)/params/amcl_companion_test_ely2021_mapFME.yaml" command="load" /> + <!--<rosparam file="$(find iri_dabo_rosnav)/params/global_planner/global_planner_params.yaml" command="load" /> + <rosparam file="$(find iri_dabo_rosnav)/params/mux.yaml" command="load" />--> + <param name="AkpLocalPlanner/destination_map_path" value="$(find iri_robot_aspsi)/maps/destinaciones_ana_FME_ubuntu18_ros_melodic.txt" /> + <param name="AkpLocalPlanner/results_filename_Zanlungo" value="name_file.txt" /> + <param name="AkpLocalPlanner/robot" value="ana"/> + <param name="AkpLocalPlanner/person_goal_id" value="200"/> + <param name="AkpLocalPlanner/speed_k" value="1"/> + <param name="AkpLocalPlanner/person_radi_amp" value="0.2"/> + <param name="AkpLocalPlanner/change_treshold_distance_between_steps" value="0.3"/> + <!-- for SIM--> + <param name="AkpLocalPlanner/change_sim" value="False"/> + <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/Ana_launchs/FME/sim_sample_companion_with_person.launch b/launch/Ana_launchs/FME/sim_sample_companion_with_person.launch new file mode 100644 index 0000000000000000000000000000000000000000..097a8641028fb2aedd7b19382b96cb1f321931b9 --- /dev/null +++ b/launch/Ana_launchs/FME/sim_sample_companion_with_person.launch @@ -0,0 +1,80 @@ +<?xml version="1.0" encoding="UTF-8"?> +<launch> + + <arg name="ns" default="ana"/> + <arg name="world_name" default="fme_opened_door"/> <!-- Possible worlds: fme_opened_door; fme_door_open_with_obst; master_big; etc. --> + <arg name="world_description" default="true"/> + <arg name="cmd_vel_mux_config" default="$(find iri_ana_gazebo)/config/mux.yaml"/> + <arg name="rviz" default="false"/> + <arg name="gui" default="true"/> + + <arg name="pan_tilt_controller_config_file" default="$(find iri_ana_gazebo)/config/ana_pan_tilt_sim_config.yaml" /> + <arg name="joint_trajectory_config_file" default="$(find iri_ana_gazebo)/config/ana_joint_trajectory_gazebo_config.yaml"/> + <arg name="pan_tilt_effort_controller_ns" default="effort_controller" /> + <arg name="pan_tilt_position_controller_ns" default="position_controller" /> + + <include file="$(find iri_gazebo_worlds)/launch/test.launch"> + <arg name="gui" value="$(arg gui)"/> + <arg name="world_name" value="$(arg world_name)"/> + <arg name="description" value="$(arg world_description)"/> + <arg name="rviz" value="false"/> + </include> + + <include file="$(find iri_objects_description)/launch/spawn_person.launch"> + <arg name="name" value="person1"/> + <arg name="type" value="person"/> + <arg name="x" value="0.0"/> + <arg name="y" value="6.0"/> + <arg name="yaw" value="-1.57"/> + </include> + + <include file="$(find iri_ana_gazebo)/launch/spawn.launch"> + <arg name="ns" value="$(arg ns)"/> + <arg name="model" value="ana"/> + <arg name="x" value="-1.5"/> + <arg name="y" value="6.0"/> + <arg name="yaw" value="-1.57"/> + <arg name="pan_tilt_controller_config_file" value="$(arg pan_tilt_controller_config_file)"/> + <arg name="joint_trajectory_config_file" value="$(arg joint_trajectory_config_file)"/> + <arg name="pan_tilt_effort_controller_ns" value="$(arg pan_tilt_effort_controller_ns)"/> + <arg name="pan_tilt_position_controller_ns" value="$(arg pan_tilt_position_controller_ns)"/> + </include> +<!-- + <include file="$(find iri_objects_description)/launch/spawn_person.launch"> + <arg name="name" value="person2"/> + <arg name="type" value="person"/> + <arg name="x" value="-2.0"/> + <arg name="y" value="-6.0"/> + <arg name="yaw" value="1.57"/> + </include> + + <include file="$(find iri_objects_description)/launch/spawn_person.launch"> + <arg name="name" value="person3"/> + <arg name="type" value="person"/> + <arg name="x" value="1.5"/> + <arg name="y" value="-6.0"/> + <arg name="yaw" value="1.57"/> + </include> +--> + <include file="$(find iri_rosnav)/launch/include/cmd_vel_mux.launch"> + <arg name="ns" value="$(arg ns)"/> + <arg name="nodelet_manager_name" value="mux_nodelet_manager"/> + <arg name="config" value="$(arg cmd_vel_mux_config)"/> + <arg name="output" value="log" /> + <arg name="launch_prefix" value="" /> + </include> + + <node name="cmd_vel_relay" + pkg="topic_tools" + type="relay" + args="/ana/ana_odom/cmd_vel_in /ana/cmd_vel"> + </node> + + <node name="rviz" + pkg="rviz" + type="rviz" + if="$(arg rviz)" + args="-d $(find iri_ana_gazebo)/rviz/$(arg ns).rviz"> + </node> + +</launch> diff --git a/launch/Ana_launchs/FME/tracker_nav_and_detection_for_ASPSI.launch b/launch/Ana_launchs/FME/tracker_nav_and_detection_for_ASPSI.launch new file mode 100644 index 0000000000000000000000000000000000000000..a687b2833f090317422f5bf0242a7e9a9ad4eb2a --- /dev/null +++ b/launch/Ana_launchs/FME/tracker_nav_and_detection_for_ASPSI.launch @@ -0,0 +1,191 @@ +<!-- --> +<launch> + + <arg name="rviz" default="true"/> + <arg name="rviz_file" default="$(find iri_ana_rosnav)/rviz/ana.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"/> + <arg name="costmap_global_params" default="global_params.yaml"/> + <arg name="map_name" default="fme"/> + <arg name="initial_x" default="-1.5"/> + <arg name="initial_y" default="6.0"/> + <arg name="initial_yaw" default="-1.57"/> + <arg name="local_planner" default="dwa"/> + <arg name="global_planner" default="global_planner"/> + <arg name="output" default="screen" /> + <arg name="launch_prefix" default="" /> + + <include file="$(find iri_rosnav)/launch/nav.launch"> + <arg name="ns" value="ana"/> + <arg name="path" value="$(find iri_ana_rosnav)/params"/> + <arg name="move_base_params" value="$(arg move_base_params)"/> + <arg name="costmap_common_params" value="$(arg costmap_common_params)"/> + <arg name="costmap_local_params" value="$(arg costmap_local_params)"/> + <arg name="costmap_global_params" value="$(arg costmap_global_params)"/> + <arg name="map_frame_id" value="map"/> + <arg name="odom_frame_id" value="ana/odom"/> + <arg name="base_frame_id" value="ana/base_footprint"/> + <arg name="map_topic" value="/ana/map"/> + <arg name="map_service" value="/ana/static_map"/> + <arg name="odom_topic" value="/ana/odom"/> + <arg name="cmd_vel_topic" value="/ana/navigation/cmd_vel"/> + <arg name="scan_topic" value="/ana/sensors/scan"/> + <arg name="use_map" value="true"/> + <arg name="use_map_server" value="true"/> + <arg name="map_name" value="$(arg map_name)"/> + <arg name="use_amcl" value="true"/> + <arg name="amcl_config" value="$(find iri_ana_rosnav)/params/amcl_companion_test_ely2021_mapFME.yaml"/> + <arg name="initial_x" value="$(arg initial_x)"/> + <arg name="initial_y" value="$(arg initial_y)"/> + <arg name="initial_yaw" value="$(arg initial_yaw)"/> + <arg name="use_fake_loc" value="false"/> + <arg name="use_gmapping" value="false"/> + <arg name="local_planner" value="$(arg local_planner)"/> + <arg name="global_planner" value="$(arg global_planner)"/> + <arg name="output" value="$(arg output)" /> + <arg name="launch_prefix" value="$(arg launch_prefix)" /> + </include> + + <include file="$(find iri_rosnav)/launch/include/pointcloud_to_laserscan.launch"> + <arg name="ns" value="ana"/> + <arg name="node_name" value="pcl_to_laserscan"/> + <arg name="scan_topic" value="/ana/sensors/scan"/> + <arg name="cloud_topic" value="/ana/sensors/velodyne_points"/> + <arg name="config_file" value="$(find iri_ana_rosnav)/params/pointcloud_to_laserscan.yaml" /> + <arg name="output" value="$(arg output)" /> + <arg name="launch_prefix" value="$(arg launch_prefix)" /> + </include> + +<group ns="$(optenv ROBOT ana)"> + <!-- People Tracking MHT (remap) --> + <!-- published topics: /$(env ROBOT)/mht/tracks --> + <!-- subscribed topics: /$(env ROBOT)/detections --> + <!-- service clients: --> + <!-- service servers: --> + <!-- action clients: --> + <!-- action servers: --> + <node pkg ="iri_people_tracking_mht" + type="iri_people_tracking_mht" + name="mht" + output="screen"> + <remap from="/ana/mht/detections" + to="/ana/lpd/peopleMapFiltered" /> + <remap from="/ana/mht/odom" + to="/ana/odom" /> <!-- /topic-odometry-of-the-robot --> + <remap from="/ana/mht/odom_for_medium_velocity" + to="/ana/odom" /> <!-- /topic-odometry-of-the-robot --> + + <!-- People Tracking MHT parameters for outdoor environments with few false alarms--> + <param name="~/frame_id" type="string" value="map" /> <!-- tf of the map for the tracker --> + <param name="local_tracker" type="bool" value="false"/> + <param name="~/association_threshold" type="double" value="1.0" /> + <param name="~/confirmation_threshold" type="double" value="0.9" /> + <param name="~/deletion_threshold" type="double" value="0.4" /> + <param name="~/laser_probability" type="double" value="0.9" /> + <param name="~/laser_false_alarm" type="double" value="0.1" /> + <param name="~/laser_new_track" type="double" value="0.11" /> + <param name="~/laser_no_detection" type="double" value="0.97" /> + <param name="~/laser_no_confirmed" type="double" value="0.02" /> + <param name="~/laser_no_confirmed_iteration" type="double" value="0.01" /> + <param name="~/augment_covariance_track" type="double" value="0.5" /> + <param name="~/mht_velocity_margin" type="double" value="1.0" /> + <param name="~/covariance_no_track" type="double" value="5.0" /> + </node> + + <!-- Laser People Detector (remap) --> + <!-- published topics: /$(env ROBOT)/people --> + <!-- subscribed topics: /$(env ROBOT)/sensors/front_laser_scan --> + <!-- service clients: --> + <!-- service servers: --> + <!-- action clients: --> + <!-- action servers: --> + <node pkg ="iri_laser_people_detection" + type="iri_laser_people_detection" + name="lpd" + output="screen" + > + + <param name="~/rangeThreshold" type="double" value="0.0" /> + <param name="~/detectionThreshold" type="double" value="-0.07" /> + <param name="~/personRadius" type="double" value="0.7" /> + <!-- load different trained detectors (to switch at rangeThreshold) --> + <param name="~/boostFilePath" type="string" value="/usr/local/include/iri/laser_people_detection/boostData/boostFile.txt" /> + <param name="~/boostFilePath2" type="string" value="/usr/local/include/iri/laser_people_detection/boostData/boostFile2.txt" /> + <!-- transform detections to base_link frame --> + <param name="~/selectPosesFrame" type="bool" value="False" /> + <param name="~/posesFrame" type="string" value="/scan" /> <!-- frame where you want the poses --> + <param name="~/selectScanFrame" type="bool" value="False" /> + <param name="~/scanFrame" type="string" value="/scan" /> <!-- frame of the scan --> + <!-- visualization --> + <param name="~/markerWidth" type="double" value="0.7" /> + <param name="~/markerHeight" type="double" value="0.4" /> + <param name="~/markerR" type="double" value="0.0" /> + <param name="~/markerG" type="double" value="0.0" /> + <param name="~/markerB" type="double" value="0.5" /> + <param name="~/markerA" type="double" value="0.5" /> + <!-- filter detections by range/x/y --> + <param name="~/filterPosesMode" type="bool" value="False" /> + <param name="~/filterR" type="double" value="0.0" /> + <param name="~/filterX" type="double" value="0.0" /> + <param name="~/filterY" type="double" value="0.0" /> + + <remap from="/ana/lpd/scan" + to="/ana/sensors/scan" /> <!-- topic: scan del robot --> + <remap from="/ana/lpd/people" + to="/ana/lpd/people" /> + </node> + + + + <!-- Laser People map Filter --> + <!-- published topics: /$(env ROBOT)/peopleFiltered --> + <!-- subscribed topics: /$(env ROBOT)/people --> + <!-- /$(env ROBOT)map --> + <!-- service clients: --> + <!-- service servers: --> + <!-- action clients: --> + <!-- action servers: --> + <node pkg ="iri_laser_people_map_filter" + type="iri_laser_people_map_filter" + name="lpmf"> + + <param name="~/filter" type="bool" value="True" /> <!-- true to enable filtering when we have a map --> + <param name="~/neighborRadius" type="double" value="0.3" /> + <param name="~/markerWidth" type="double" value="0.7" /> + <param name="~/markerHeight" type="double" value="1.5" /> + <param name="~/markerR" type="double" value="1.0" /> + <param name="~/markerG" type="double" value="1.0" /> + <param name="~/markerB" type="double" value="0.0" /> + <param name="~/markerA" type="double" value="0.9" /> + <param name="~/markerLifeTime" type="double" value="0.1" /> + + + <remap from="/ana/lpmf/people" + to="/ana/lpd/people" /> + <remap from="/ana/lpmf/people_out" + to="/ana/lpd/peopleMapFiltered" /> + <remap from="/ana/lpmf/map" + to="/ana/map" /> <!-- topic of the map --> + + </node> + </group> + + <!-- Ini Navigation Companion Nodes --> + + + + <!-- <node name="rviz" + pkg="rviz" + type="rviz" + if="$(arg rviz)" + args="-d $(arg rviz_file)"> + </node>--> + +<node name="rviz" + pkg="rviz" + type="rviz" + if="$(arg rviz)" + args="-d $(find iri_robot_aspsi)/rviz/ana.rviz"> + </node> +</launch> diff --git a/launch/Dabo_launchs/BRL/gazebo_ASPSI_BRL_OK.launch b/launch/Dabo_launchs/BRL/gazebo_ASPSI_BRL_OK.launch new file mode 100644 index 0000000000000000000000000000000000000000..30a1915d1c8eb3daf29056216c92c60c2868a477 --- /dev/null +++ b/launch/Dabo_launchs/BRL/gazebo_ASPSI_BRL_OK.launch @@ -0,0 +1,50 @@ +<!-- --> +<launch> + + + <!-- Ini Navigation Companion Nodes --> + <!--launch-prefix="xterm -e ddd -args" --> + + <group ns="dabo"> + <rosparam command="delete" ns="/dabo/move_base" /> + + <node pkg ="move_base" + type="move_base" + name="move_base" + output="screen"> + <remap from="/dabo/move_base/cmd_vel" + to="/dabo/navigation/cmd_vel" /> + <remap from="/dabo/move_base/odom" + to="/dabo/odom" /> + <remap from="/dabo/front_scan" + to="/dabo/sensors/front_scan" /> + <remap from="/dabo/rear_scan" + to="/dabo/sensors/rear_scan" /> + <remap from="/dabo/tracks" + to="/mht_2/tracks" /> + <rosparam file="$(find iri_robot_aspsi)/config/dabo/move_base_params.yaml" command="load" /> + <rosparam file="$(find iri_robot_aspsi)/config/ana/costmap_common_params.yaml" command="load" ns="global_costmap" /> + <rosparam file="$(find iri_robot_aspsi)/config/dabo/costmap_common_params.yaml" command="load" ns="local_costmap" /> + <rosparam file="$(find iri_robot_aspsi)/config/dabo/local_costmap_params.yaml" command="load" /> + <rosparam file="$(find iri_robot_aspsi)/config/ana/global_costmap_params.yaml" command="load" /> + <rosparam file="$(find iri_robot_aspsi)/config/dabo/akp_local_planner_params.yaml" command="load" /> + <rosparam file="$(find iri_dabo_rosnav)/params/amcl_companion_test_ely2021_mapFME.yaml" command="load" /> + <!--<rosparam file="$(find iri_dabo_rosnav)/params/global_planner/global_planner_params.yaml" command="load" /> + <rosparam file="$(find iri_dabo_rosnav)/params/mux.yaml" command="load" />--> + <param name="AkpLocalPlanner/destination_map_path" value="$(find iri_robot_aspsi)/maps/master_big_destinations_Gazebo_sim.txt" /> + <param name="AkpLocalPlanner/results_filename_Zanlungo" value="name_file.txt" /> + <param name="AkpLocalPlanner/robot" value="dabo"/> + <param name="AkpLocalPlanner/person_goal_id" value="200"/> + <param name="AkpLocalPlanner/speed_k" value="1"/> + <param name="AkpLocalPlanner/person_radi_amp" value="0.2"/> + <param name="AkpLocalPlanner/change_treshold_distance_between_steps" value="0.3"/> + <!-- for SIM--> + <param name="AkpLocalPlanner/change_sim" value="False"/> + <param name="AkpLocalPlanner/sim_target_per" value="False"/> + <param name="AkpLocalPlanner/frame_robot_footprint" value="dabo/base_link"/> + </node> + </group> + + <node name="dr" pkg ="rqt_reconfigure" type="rqt_reconfigure" /> + +</launch> diff --git a/launch/Dabo_launchs/BRL/sim_gazebo_dabo_companion.launch b/launch/Dabo_launchs/BRL/sim_gazebo_dabo_companion.launch new file mode 100644 index 0000000000000000000000000000000000000000..936be4a08122cb4e010f628f0841570786b24337 --- /dev/null +++ b/launch/Dabo_launchs/BRL/sim_gazebo_dabo_companion.launch @@ -0,0 +1,251 @@ +<?xml version="1.0" encoding="UTF-8"?> +<launch> + + <arg name="ns" default="dabo"/> + <arg name="world_name" default="master_big"/> <!-- other world_name:empty, master_big, fme_opened_door (in: /iri_navigation/iri_gazebo_worlds/models/fme_opened_door)--> + <arg name="world" default="$(find iri_gazebo_worlds)/worlds/$(arg world_name).world"/> + <arg name="world_description" default="true"/> + <arg name="robot_model" default="dabo_fixed"/> + <arg name="rviz" default="true"/> + <arg name="gui" default="true"/> +<!-- Ini variables nav--> + <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"/> + <arg name="costmap_global_params" default="global_params.yaml"/> + <arg name="map_name" default="master_big"/> + <arg name="initial_x" default="11.0"/> + <arg name="initial_y" default="22.0"/> + <arg name="initial_yaw" default="0.0"/> + <arg name="local_planner" default="dwa"/> + <arg name="global_planner" default="global_planner"/> + <arg name="output" default="screen" /> + <arg name="launch_prefix" default="" /> + + <include file="$(find iri_gazebo_worlds)/launch/test.launch"> + <arg name="gui" value="$(arg gui)"/> + <arg name="world_name" value="$(arg world_name)"/> + <arg name="description" value="$(arg world_description)"/> + <arg name="rviz" value="false"/> + </include> + + <include file="$(find iri_dabo_gazebo)/launch/spawn.launch"> + <arg name="ns" value="$(arg ns)"/> + <arg name="model" value="$(arg robot_model)"/> + <arg name="x" value="11.0"/> + <arg name="y" value="22.0"/> + <arg name="yaw" value="0.0"/> + </include> + +<!-- person1 is the nearest person to the robot. This person will be the accompanied one, if you use exactly our instructions included in the read-me of the node. --> + <include file="$(find iri_objects_description)/launch/spawn_person.launch"> + <arg name="name" value="person1"/> + <arg name="type" value="person"/> + <arg name="x" value="11.5"/> <!-- changes the x component of the initial person1's position inside the environment (it is in map coordinates). --> + <arg name="y" value="20.0"/> <!-- changes the y component of the initial person1's position inside the environment (it is in map coordinates)--> + <arg name="yaw" value="0.0"/> <!-- changes the initial person1's orientation inside the environment (it is in map coordinates)--> + </include> + + + +<!-- YOU can include more people in a similar way --> + <!-- <include file="$(find iri_objects_description)/launch/spawn_person.launch"> + <arg name="name" value="person2"/> + <arg name="type" value="person"/> + <arg name="x" value="27.0"/> + <arg name="y" value="22.0"/> + <arg name="yaw" value="3.14"/> + + </include> + +<include file="$(find iri_objects_description)/launch/spawn_person.launch"> + <arg name="name" value="person3"/> + <arg name="type" value="person"/> + <arg name="x" value="40.5"/> + <arg name="y" value="17.5"/> + <arg name="yaw" value="3.14"/> + </include> + +<include file="$(find iri_objects_description)/launch/spawn_person.launch"> + <arg name="name" value="person4"/> + <arg name="type" value="person"/> + <arg name="x" value="40.0"/> + <arg name="y" value="21.5"/> + <arg name="yaw" value="3.14"/> + </include> +--> + + + +<!-- DETECTION AND TRACKING --> +<!-- DETECTION AND TRACKING --> +<!-- DETECTION AND TRACKING --> + + <!-- People Tracking MHT (remap) --> + <!-- published topics: /$(env ROBOT)/mht/tracks --> + <!-- subscribed topics: /$(env ROBOT)/detections --> + <!-- service clients: --> + <!-- service servers: --> + <!-- action clients: --> + <!-- action servers: --> + <node pkg ="iri_people_tracking_mht" + type="iri_people_tracking_mht" + name="mht_2" + output="screen"> + <remap from="/mht_2/detections" + to="/lpd_2/peopleMapFiltered" /> + <remap from="/mht_2/odom" + to="/dabo/odom" /> <!-- /topic-odometry-of-the-robot --> + <remap from="/mht_2/odom_for_medium_velocity" + to="/dabo/odom" /> <!-- /topic-odometry-of-the-robot --> + + <!-- People Tracking MHT parameters for outdoor environments with few false alarms--> + <param name="~/frame_id" type="string" value="map" /> <!-- tf of the map for the tracker --> + <param name="local_tracker" type="bool" value="false"/> + <param name="~/association_threshold" type="double" value="1.0" /> + <param name="~/confirmation_threshold" type="double" value="0.9" /> + <param name="~/deletion_threshold" type="double" value="0.4" /> + <param name="~/laser_probability" type="double" value="0.9" /> + <param name="~/laser_false_alarm" type="double" value="0.1" /> + <param name="~/laser_new_track" type="double" value="0.11" /> + <param name="~/laser_no_detection" type="double" value="0.97" /> + <param name="~/laser_no_confirmed" type="double" value="0.02" /> + <param name="~/laser_no_confirmed_iteration" type="double" value="0.01" /> + <param name="~/augment_covariance_track" type="double" value="0.5" /> + <param name="~/mht_velocity_margin" type="double" value="1.0" /> + <param name="~/covariance_no_track" type="double" value="5.0" /> + </node> + + <!-- Laser People Detector (remap) --> + <!-- published topics: /$(env ROBOT)/people --> + <!-- subscribed topics: /$(env ROBOT)/sensors/front_laser_scan --> + <!-- service clients: --> + <!-- service servers: --> + <!-- action clients: --> + <!-- action servers: --> + <node pkg ="iri_laser_people_detection" + type="iri_laser_people_detection" + name="lpd_2" + output="screen" + > + + <param name="~/rangeThreshold" type="double" value="0.0" /> + <param name="~/detectionThreshold" type="double" value="-0.07" /> + <param name="~/personRadius" type="double" value="0.7" /> + <!-- load different trained detectors (to switch at rangeThreshold) --> + <param name="~/boostFilePath" type="string" value="/usr/local/include/iri/laser_people_detection/boostData/boostFile.txt" /> + <param name="~/boostFilePath2" type="string" value="/usr/local/include/iri/laser_people_detection/boostData/boostFile2.txt" /> + <!-- transform detections to base_link frame --> + <param name="~/selectPosesFrame" type="bool" value="False" /> + <param name="~/posesFrame" type="string" value="/scan" /> <!-- frame where you want the poses --> + <param name="~/selectScanFrame" type="bool" value="False" /> + <param name="~/scanFrame" type="string" value="/scan" /> <!-- frame of the scan --> + <!-- visualization --> + <param name="~/markerWidth" type="double" value="0.7" /> + <param name="~/markerHeight" type="double" value="0.4" /> + <param name="~/markerR" type="double" value="0.0" /> + <param name="~/markerG" type="double" value="0.0" /> + <param name="~/markerB" type="double" value="0.5" /> + <param name="~/markerA" type="double" value="0.5" /> + <!-- filter detections by range/x/y --> + <param name="~/filterPosesMode" type="bool" value="False" /> + <param name="~/filterR" type="double" value="0.0" /> + <param name="~/filterX" type="double" value="0.0" /> + <param name="~/filterY" type="double" value="0.0" /> + + <remap from="/lpd_2/scan" + to="/dabo/sensors/front_scan" /> <!-- topic: scan del robot --> + <remap from="/lpd_2/people" + to="/dabo/people" /> + </node> + + <node pkg ="iri_laser_people_detection" + type="iri_laser_people_detection" + name="lpd" + output="screen" + > + + <param name="~/rangeThreshold" type="double" value="0.0" /> + <param name="~/detectionThreshold" type="double" value="-0.07" /> + <param name="~/personRadius" type="double" value="0.7" /> + <!-- load different trained detectors (to switch at rangeThreshold) --> + <param name="~/boostFilePath" type="string" value="/usr/local/include/iri/laser_people_detection/boostData/boostFile.txt" /> + <param name="~/boostFilePath2" type="string" value="/usr/local/include/iri/laser_people_detection/boostData/boostFile2.txt" /> + <!-- transform detections to base_link frame --> + <param name="~/selectPosesFrame" type="bool" value="False" /> + <param name="~/posesFrame" type="string" value="/scan" /> <!-- frame where you want the poses --> + <param name="~/selectScanFrame" type="bool" value="False" /> + <param name="~/scanFrame" type="string" value="/scan" /> <!-- frame of the scan --> + <!-- visualization --> + <param name="~/markerWidth" type="double" value="0.7" /> + <param name="~/markerHeight" type="double" value="0.4" /> + <param name="~/markerR" type="double" value="0.0" /> + <param name="~/markerG" type="double" value="0.0" /> + <param name="~/markerB" type="double" value="0.5" /> + <param name="~/markerA" type="double" value="0.5" /> + <!-- filter detections by range/x/y --> + <param name="~/filterPosesMode" type="bool" value="False" /> + <param name="~/filterR" type="double" value="0.0" /> + <param name="~/filterX" type="double" value="0.0" /> + <param name="~/filterY" type="double" value="0.0" /> + + <remap from="/lpd/scan" + to="/dabo/sensors/rear_scan" /> <!-- topic: scan del robot --> + <remap from="/lpd/people" + to="/dabo/people" /> + </node> + + + <!-- Laser People map Filter --> + <!-- published topics: /$(env ROBOT)/peopleFiltered --> + <!-- subscribed topics: /$(env ROBOT)/people --> + <!-- /$(env ROBOT)map --> + <!-- service clients: --> + <!-- service servers: --> + <!-- action clients: --> + <!-- action servers: --> + <node pkg ="iri_laser_people_map_filter" + type="iri_laser_people_map_filter" + name="laser_people_map_filter"> + + <param name="~/filter" type="bool" value="True" /> <!-- true to enable filtering when we have a map --> + <param name="~/neighborRadius" type="double" value="0.3" /> + <param name="~/markerWidth" type="double" value="0.7" /> + <param name="~/markerHeight" type="double" value="1.5" /> + <param name="~/markerR" type="double" value="1.0" /> + <param name="~/markerG" type="double" value="1.0" /> + <param name="~/markerB" type="double" value="0.0" /> + <param name="~/markerA" type="double" value="0.9" /> + <param name="~/markerLifeTime" type="double" value="0.1" /> + + + <remap from="/laser_people_map_filter/people" + to="/dabo/people" /> + <remap from="/laser_people_map_filter/people_out" + to="/lpd_2/peopleMapFiltered" /> + <remap from="/laser_people_map_filter/map" + to="/dabo/map" /> <!-- topic of the map --> + + </node> + + +<!-- INI nav nodes _companion--> + <include file="$(find iri_dabo_rosnav)/launch/nav_map_companion.launch"> + <arg name="map_name" value="master_big" /> + <arg name="initial_x" value="19.0"/> + <arg name="initial_y" value="39.0"/> + <arg name="initial_yaw" value="0.0"/> + </include> + + + + + <node name="rviz" + pkg ="rviz" + type="rviz" + if="$(arg rviz)" + args="-d $(find iri_robot_aspsi)/rviz/dabo_companion_ok2.rviz" /> + + + +</launch> diff --git a/launch/Dabo_launchs/BRL/sim_gazebo_dabo_companion_all.launch b/launch/Dabo_launchs/BRL/sim_gazebo_dabo_companion_all.launch new file mode 100644 index 0000000000000000000000000000000000000000..3cd938bba03a00f6058903d9174e54ee2155409a --- /dev/null +++ b/launch/Dabo_launchs/BRL/sim_gazebo_dabo_companion_all.launch @@ -0,0 +1,322 @@ +<?xml version="1.0" encoding="UTF-8"?> +<launch> + + <arg name="ns" default="dabo"/> + <arg name="world_name" default="master_big"/> <!-- other world_name:empty, master_big, fme_opened_door (in: /iri_navigation/iri_gazebo_worlds/models/fme_opened_door)--> + <arg name="world" default="$(find iri_gazebo_worlds)/worlds/$(arg world_name).world"/> + <arg name="world_description" default="true"/> + <arg name="robot_model" default="dabo_fixed"/> + <arg name="rviz" default="true"/> + <arg name="gui" default="true"/> +<!-- Ini variables nav, mias --> + <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"/> + <arg name="costmap_global_params" default="global_params.yaml"/> + <arg name="map_name" default="master_big"/> + <arg name="initial_x" default="11.0"/> + <arg name="initial_y" default="22.0"/> + <arg name="initial_yaw" default="0.0"/> + <arg name="local_planner" default="dwa"/> + <arg name="global_planner" default="global_planner"/> + <arg name="output" default="screen" /> + <arg name="launch_prefix" default="" /> + + + + + <include file="$(find iri_gazebo_worlds)/launch/test.launch"> + <arg name="gui" value="$(arg gui)"/> + <arg name="world_name" value="$(arg world_name)"/> + <arg name="description" value="$(arg world_description)"/> + <arg name="rviz" value="false"/> + </include> + + <include file="$(find iri_dabo_gazebo)/launch/spawn.launch"> + <arg name="ns" value="$(arg ns)"/> + <arg name="model" value="$(arg robot_model)"/> + <arg name="x" value="11.0"/> + <arg name="y" value="22.0"/> + <arg name="yaw" value="0.0"/> + </include> + +<!-- person1 is the nearest person to the robot. This person will be the accompanied one, if you use exactly our instructions included in the read-me of the node. --> + <include file="$(find iri_objects_description)/launch/spawn_person.launch"> + <arg name="name" value="person1"/> + <arg name="type" value="person"/> + <arg name="x" value="11.5"/> <!-- changes the x component of the initial person1's position inside the environment (it is in map coordinates). --> + <arg name="y" value="20.0"/> <!-- changes the y component of the initial person1's position inside the environment (it is in map coordinates)--> + <arg name="yaw" value="0.0"/> <!-- changes the initial person1's orientation inside the environment (it is in map coordinates)--> + </include> + + + +<!-- YOU can include more people in a similar way --> + <include file="$(find iri_objects_description)/launch/spawn_person.launch"> + <arg name="name" value="person2"/> + <arg name="type" value="person"/> + <arg name="x" value="27.0"/> <!-- changes the x component of the initial person2's position inside the environment (it is in map coordinates). --> + <arg name="y" value="22.0"/> <!-- changes the y component of the initial person2's position inside the environment (it is in map coordinates)--> + <arg name="yaw" value="0.0"/> <!-- changes the initial person2's orientation inside the environment (it is in map coordinates)--> + + </include> + +<!--<include file="$(find iri_objects_description)/launch/spawn_person.launch"> + <arg name="name" value="person3"/> + <arg name="type" value="person"/> + <arg name="x" value="40.5"/> + <arg name="y" value="17.5"/> + <arg name="yaw" value="1.57"/> + </include> + +<include file="$(find iri_objects_description)/launch/spawn_person.launch"> + <arg name="name" value="person4"/> + <arg name="type" value="person"/> + <arg name="x" value="40.0"/> + <arg name="y" value="21.5"/> + <arg name="yaw" value="1.57"/> + </include>--> + + + + +<!-- DETECTION AND TRACKING --> +<!-- DETECTION AND TRACKING --> +<!-- DETECTION AND TRACKING --> + + <!-- People Tracking MHT (remap) --> + <!-- published topics: /$(env ROBOT)/mht/tracks --> + <!-- subscribed topics: /$(env ROBOT)/detections --> + <!-- service clients: --> + <!-- service servers: --> + <!-- action clients: --> + <!-- action servers: --> + <node pkg ="iri_people_tracking_mht" + type="iri_people_tracking_mht" + name="mht_2" + output="screen"> + <remap from="/mht_2/detections" + to="/lpd_2/peopleMapFiltered" /> + <remap from="/mht_2/odom" + to="/dabo/odom" /> <!-- /topic-odometry-of-the-robot --> + <remap from="/mht_2/odom_for_medium_velocity" + to="/dabo/odom" /> <!-- /topic-odometry-of-the-robot --> + + <!-- People Tracking MHT parameters for outdoor environments with few false alarms--> + <param name="~/frame_id" type="string" value="map" /> <!-- tf of the map for the tracker --> + <param name="local_tracker" type="bool" value="false"/> + <param name="~/association_threshold" type="double" value="1.0" /> + <param name="~/confirmation_threshold" type="double" value="0.9" /> + <param name="~/deletion_threshold" type="double" value="0.4" /> + <param name="~/laser_probability" type="double" value="0.9" /> + <param name="~/laser_false_alarm" type="double" value="0.1" /> + <param name="~/laser_new_track" type="double" value="0.11" /> + <param name="~/laser_no_detection" type="double" value="0.97" /> + <param name="~/laser_no_confirmed" type="double" value="0.02" /> + <param name="~/laser_no_confirmed_iteration" type="double" value="0.01" /> + <param name="~/augment_covariance_track" type="double" value="0.5" /> + <param name="~/mht_velocity_margin" type="double" value="1.0" /> + <param name="~/covariance_no_track" type="double" value="5.0" /> + </node> + + <!-- Laser People Detector (remap) --> + <!-- published topics: /$(env ROBOT)/people --> + <!-- subscribed topics: /$(env ROBOT)/sensors/front_laser_scan --> + <!-- service clients: --> + <!-- service servers: --> + <!-- action clients: --> + <!-- action servers: --> + <node pkg ="iri_laser_people_detection" + type="iri_laser_people_detection" + name="lpd_2" + output="screen" + > + + <param name="~/rangeThreshold" type="double" value="0.0" /> + <param name="~/detectionThreshold" type="double" value="-0.07" /> + <param name="~/personRadius" type="double" value="0.7" /> + <!-- load different trained detectors (to switch at rangeThreshold) --> + <param name="~/boostFilePath" type="string" value="/usr/local/include/iri/laser_people_detection/boostData/boostFile.txt" /> + <param name="~/boostFilePath2" type="string" value="/usr/local/include/iri/laser_people_detection/boostData/boostFile2.txt" /> + <!-- transform detections to base_link frame --> + <param name="~/selectPosesFrame" type="bool" value="False" /> + <param name="~/posesFrame" type="string" value="/scan" /> <!-- frame where you want the poses --> + <param name="~/selectScanFrame" type="bool" value="False" /> + <param name="~/scanFrame" type="string" value="/scan" /> <!-- frame of the scan --> + <!-- visualization --> + <param name="~/markerWidth" type="double" value="0.7" /> + <param name="~/markerHeight" type="double" value="0.4" /> + <param name="~/markerR" type="double" value="0.0" /> + <param name="~/markerG" type="double" value="0.0" /> + <param name="~/markerB" type="double" value="0.5" /> + <param name="~/markerA" type="double" value="0.5" /> + <!-- filter detections by range/x/y --> + <param name="~/filterPosesMode" type="bool" value="False" /> + <param name="~/filterR" type="double" value="0.0" /> + <param name="~/filterX" type="double" value="0.0" /> + <param name="~/filterY" type="double" value="0.0" /> + + <remap from="/lpd_2/scan" + to="/dabo/sensors/front_scan" /> <!-- topic: scan del robot --> + <remap from="/lpd_2/people" + to="/dabo/people" /> + </node> + + <node pkg ="iri_laser_people_detection" + type="iri_laser_people_detection" + name="lpd" + output="screen" + > + + <param name="~/rangeThreshold" type="double" value="0.0" /> + <param name="~/detectionThreshold" type="double" value="-0.07" /> + <param name="~/personRadius" type="double" value="0.7" /> + <!-- load different trained detectors (to switch at rangeThreshold) --> + <param name="~/boostFilePath" type="string" value="/usr/local/include/iri/laser_people_detection/boostData/boostFile.txt" /> + <param name="~/boostFilePath2" type="string" value="/usr/local/include/iri/laser_people_detection/boostData/boostFile2.txt" /> + <!-- transform detections to base_link frame --> + <param name="~/selectPosesFrame" type="bool" value="False" /> + <param name="~/posesFrame" type="string" value="/scan" /> <!-- frame where you want the poses --> + <param name="~/selectScanFrame" type="bool" value="False" /> + <param name="~/scanFrame" type="string" value="/scan" /> <!-- frame of the scan --> + <!-- visualization --> + <param name="~/markerWidth" type="double" value="0.7" /> + <param name="~/markerHeight" type="double" value="0.4" /> + <param name="~/markerR" type="double" value="0.0" /> + <param name="~/markerG" type="double" value="0.0" /> + <param name="~/markerB" type="double" value="0.5" /> + <param name="~/markerA" type="double" value="0.5" /> + <!-- filter detections by range/x/y --> + <param name="~/filterPosesMode" type="bool" value="False" /> + <param name="~/filterR" type="double" value="0.0" /> + <param name="~/filterX" type="double" value="0.0" /> + <param name="~/filterY" type="double" value="0.0" /> + + <remap from="/lpd/scan" + to="/dabo/sensors/rear_scan" /> <!-- topic: scan del robot --> + <remap from="/lpd/people" + to="/dabo/people" /> + </node> + + + <!-- Laser People map Filter --> + <!-- published topics: /$(env ROBOT)/peopleFiltered --> + <!-- subscribed topics: /$(env ROBOT)/people --> + <!-- /$(env ROBOT)map --> + <!-- service clients: --> + <!-- service servers: --> + <!-- action clients: --> + <!-- action servers: --> + <node pkg ="iri_laser_people_map_filter" + type="iri_laser_people_map_filter" + name="laser_people_map_filter"> + + <param name="~/filter" type="bool" value="True" /> <!-- true to enable filtering when we have a map --> + <param name="~/neighborRadius" type="double" value="0.3" /> + <param name="~/markerWidth" type="double" value="0.7" /> + <param name="~/markerHeight" type="double" value="1.5" /> + <param name="~/markerR" type="double" value="1.0" /> + <param name="~/markerG" type="double" value="1.0" /> + <param name="~/markerB" type="double" value="0.0" /> + <param name="~/markerA" type="double" value="0.9" /> + <param name="~/markerLifeTime" type="double" value="0.1" /> + + + <remap from="/laser_people_map_filter/people" + to="/dabo/people" /> + <remap from="/laser_people_map_filter/people_out" + to="/lpd_2/peopleMapFiltered" /> + <remap from="/laser_people_map_filter/map" + to="/dabo/map" /> <!-- topic of the map --> + + </node> + + +<!-- INI nav nodes _companion--> + <include file="$(find iri_dabo_rosnav)/launch/nav_map_companion.launch"> + <arg name="map_name" value="master_big" /> + <arg name="initial_x" value="19.0"/> + <arg name="initial_y" value="39.0"/> + <arg name="initial_yaw" value="0.0"/> + </include> + + + <!-- Ini Navigation Companion Node --> + +<!--launch-prefix="xterm -e ddd -args" --> + <group ns="dabo"> + <rosparam command="delete" ns="/dabo/move_base" /> + + <node pkg ="move_base" + type="move_base" + name="move_base" + output="screen"> + <remap from="/dabo/move_base/cmd_vel" + to="/dabo/navigation/cmd_vel" /> + <remap from="/dabo/move_base/odom" + to="/dabo/odom" /> + <remap from="/dabo/front_scan" + to="/dabo/sensors/front_scan" /> + <remap from="/dabo/rear_scan" + to="/dabo/sensors/rear_scan" /> + <remap from="/dabo/tracks" + to="/mht_2/tracks" /> + <rosparam file="$(find iri_robot_aspsi)/config/dabo/move_base_params.yaml" command="load" /> + <rosparam file="$(find iri_robot_aspsi)/config/ana/costmap_common_params.yaml" command="load" ns="global_costmap" /> + <rosparam file="$(find iri_robot_aspsi)/config/dabo/costmap_common_params.yaml" command="load" ns="local_costmap" /> + <rosparam file="$(find iri_robot_aspsi)/config/dabo/local_costmap_params.yaml" command="load" /> + <rosparam file="$(find iri_robot_aspsi)/config/ana/global_costmap_params.yaml" command="load" /> + <rosparam file="$(find iri_robot_aspsi)/config/dabo/akp_local_planner_params.yaml" command="load" /> + <rosparam file="$(find iri_dabo_rosnav)/params/amcl_companion_test_ely2021_mapFME.yaml" command="load" /> + <!--<rosparam file="$(find iri_dabo_rosnav)/params/global_planner/global_planner_params.yaml" command="load" /> + <rosparam file="$(find iri_dabo_rosnav)/params/mux.yaml" command="load" />--> + <param name="AkpLocalPlanner/destination_map_path" value="$(find iri_robot_aspsi)/maps/master_big_destinations_Gazebo_sim.txt" /> + <param name="AkpLocalPlanner/results_filename_Zanlungo" value="name_file.txt" /> + <param name="AkpLocalPlanner/robot" value="dabo"/> + <param name="AkpLocalPlanner/person_goal_id" value="200"/> + <param name="AkpLocalPlanner/speed_k" value="1"/> + <param name="AkpLocalPlanner/person_radi_amp" value="0.2"/> + <param name="AkpLocalPlanner/change_treshold_distance_between_steps" value="0.3"/> + <!-- for SIM--> + <param name="AkpLocalPlanner/change_sim" value="False"/> + <param name="AkpLocalPlanner/sim_target_per" value="False"/> + <param name="AkpLocalPlanner/frame_robot_footprint" value="dabo/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="/dabo/state_mach_comp/people_tracks" + to="/dabo/mht_2/tracks" /> + <remap from="/dabo/state_mach_comp/odom" + to="/dabo/odom" /> + <remap from="/dabo/state_mach_comp/companionState" + to="/dabo/companionState" /> + <remap from="/dabo/tibi_akp_status" + to="/dabo/dabo_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="dabo/base_link" /> + <param name="~/robot" value="dabo" /> + <param name="terrinet_project_conf" value="true"/> + </node> + + </group> + + + + <node name="dr" pkg ="rqt_reconfigure" type="rqt_reconfigure" /> + + + + <node name="rviz" + pkg ="rviz" + type="rviz" + if="$(arg rviz)" + args="-d $(find iri_robot_aspsi)/rviz/dabo_companion_ok2.rviz" /> + + + +</launch> diff --git a/launch/ejemplo_rosbag_launch/Example_relaunch_all_with_rosbag_to_obtain_results.launch b/launch/ejemplo_rosbag_launch/Example_relaunch_all_with_rosbag_to_obtain_results.launch new file mode 100644 index 0000000000000000000000000000000000000000..d5bcbafa68e3427cc54e7d470697f8aa03373f3f --- /dev/null +++ b/launch/ejemplo_rosbag_launch/Example_relaunch_all_with_rosbag_to_obtain_results.launch @@ -0,0 +1,242 @@ +<!-- EXAMPLE FOR Dabo-ROBOT --> +<!-- Note: This is just an example of what you can do to get results from the rosbags, but it may be necessary to change some topics or add something else from the robot model or map. --> +<launch> + +<param name="use_sim_time" value="true"/> + + <node pkg="rosbag" + type="play" + name="rosbag" + args=" --clock --pause -l -r 4 /home/erepiso/results_tibi/1_rosbags_teleop_comparacion_SideBySide_Vform/17_12_2018/17_12_2018_tibi/subset_2018-12-17-16-59-15.bag"/> + +<!-- Rosbags Comparación Side-by-Side y Vform !!! --> +<!-- "Help": If you want, for comfortablenes include the launch's names here to only copy names and change in the previous node the rosbag. + +Example: 29_11_2018/29_11_2018_teo/2018-11-29-15-08-39.bag ; real-video-asociated to launch: Video_NAME.mp4 // + briev description of experiment. + --> + + + +<!-- Launch Dabo robot model: --> + <include file="$(find iri_dabo_gazebo)/launch/spawn.launch"> + <arg name="ns" value="$(arg ns)"/> + <arg name="model" value="$(arg robot_model)"/> + <arg name="x" value="11.0"/> + <arg name="y" value="22.0"/> + <arg name="yaw" value="0.0"/> + </include> + + + <!--re-launch Launch detector and tracker (option 2: comment these ones and change the topics to use the tracks of the recorded rosbag. --> + + <!-- People Tracking MHT (remap) --> + <!-- published topics: /$(env ROBOT)/mht/tracks --> + <!-- subscribed topics: /$(env ROBOT)/detections --> + <!-- service clients: --> + <!-- service servers: --> + <!-- action clients: --> + <!-- action servers: --> + <node pkg ="iri_people_tracking_mht" + type="iri_people_tracking_mht" + name="mht_2" + output="screen"> + <remap from="/mht_2/detections" + to="/lpd_2/peopleMapFiltered" /> + <remap from="/mht_2/odom" + to="/dabo/odom" /> <!-- /topic-odometry-of-the-robot --> + <remap from="/mht_2/odom_for_medium_velocity" + to="/dabo/odom" /> <!-- /topic-odometry-of-the-robot --> + + <!-- People Tracking MHT parameters for outdoor environments with few false alarms--> + <param name="~/frame_id" type="string" value="map" /> <!-- tf of the map for the tracker --> + <param name="local_tracker" type="bool" value="false"/> + <param name="~/association_threshold" type="double" value="1.0" /> + <param name="~/confirmation_threshold" type="double" value="0.9" /> + <param name="~/deletion_threshold" type="double" value="0.4" /> + <param name="~/laser_probability" type="double" value="0.9" /> + <param name="~/laser_false_alarm" type="double" value="0.1" /> + <param name="~/laser_new_track" type="double" value="0.11" /> + <param name="~/laser_no_detection" type="double" value="0.97" /> + <param name="~/laser_no_confirmed" type="double" value="0.02" /> + <param name="~/laser_no_confirmed_iteration" type="double" value="0.01" /> + <param name="~/augment_covariance_track" type="double" value="0.5" /> + <param name="~/mht_velocity_margin" type="double" value="1.0" /> + <param name="~/covariance_no_track" type="double" value="5.0" /> + </node> + + <!-- Laser People Detector (remap) --> + <!-- published topics: /$(env ROBOT)/people --> + <!-- subscribed topics: /$(env ROBOT)/sensors/front_laser_scan --> + <!-- service clients: --> + <!-- service servers: --> + <!-- action clients: --> + <!-- action servers: --> + <node pkg ="iri_laser_people_detection" + type="iri_laser_people_detection" + name="lpd_2" + output="screen" + > + + <param name="~/rangeThreshold" type="double" value="0.0" /> + <param name="~/detectionThreshold" type="double" value="-0.07" /> + <param name="~/personRadius" type="double" value="0.7" /> + <!-- load different trained detectors (to switch at rangeThreshold) --> + <param name="~/boostFilePath" type="string" value="/usr/local/include/iri/laser_people_detection/boostData/boostFile.txt" /> + <param name="~/boostFilePath2" type="string" value="/usr/local/include/iri/laser_people_detection/boostData/boostFile2.txt" /> + <!-- transform detections to base_link frame --> + <param name="~/selectPosesFrame" type="bool" value="False" /> + <param name="~/posesFrame" type="string" value="/scan" /> <!-- frame where you want the poses --> + <param name="~/selectScanFrame" type="bool" value="False" /> + <param name="~/scanFrame" type="string" value="/scan" /> <!-- frame of the scan --> + <!-- visualization --> + <param name="~/markerWidth" type="double" value="0.7" /> + <param name="~/markerHeight" type="double" value="0.4" /> + <param name="~/markerR" type="double" value="0.0" /> + <param name="~/markerG" type="double" value="0.0" /> + <param name="~/markerB" type="double" value="0.5" /> + <param name="~/markerA" type="double" value="0.5" /> + <!-- filter detections by range/x/y --> + <param name="~/filterPosesMode" type="bool" value="False" /> + <param name="~/filterR" type="double" value="0.0" /> + <param name="~/filterX" type="double" value="0.0" /> + <param name="~/filterY" type="double" value="0.0" /> + + <remap from="/lpd_2/scan" + to="/dabo/sensors/front_scan" /> <!-- topic: scan del robot --> + <remap from="/lpd_2/people" + to="/dabo/people" /> + </node> + + <node pkg ="iri_laser_people_detection" + type="iri_laser_people_detection" + name="lpd" + output="screen" + > + + <param name="~/rangeThreshold" type="double" value="0.0" /> + <param name="~/detectionThreshold" type="double" value="-0.07" /> + <param name="~/personRadius" type="double" value="0.7" /> + <!-- load different trained detectors (to switch at rangeThreshold) --> + <param name="~/boostFilePath" type="string" value="/usr/local/include/iri/laser_people_detection/boostData/boostFile.txt" /> + <param name="~/boostFilePath2" type="string" value="/usr/local/include/iri/laser_people_detection/boostData/boostFile2.txt" /> + <!-- transform detections to base_link frame --> + <param name="~/selectPosesFrame" type="bool" value="False" /> + <param name="~/posesFrame" type="string" value="/scan" /> <!-- frame where you want the poses --> + <param name="~/selectScanFrame" type="bool" value="False" /> + <param name="~/scanFrame" type="string" value="/scan" /> <!-- frame of the scan --> + <!-- visualization --> + <param name="~/markerWidth" type="double" value="0.7" /> + <param name="~/markerHeight" type="double" value="0.4" /> + <param name="~/markerR" type="double" value="0.0" /> + <param name="~/markerG" type="double" value="0.0" /> + <param name="~/markerB" type="double" value="0.5" /> + <param name="~/markerA" type="double" value="0.5" /> + <!-- filter detections by range/x/y --> + <param name="~/filterPosesMode" type="bool" value="False" /> + <param name="~/filterR" type="double" value="0.0" /> + <param name="~/filterX" type="double" value="0.0" /> + <param name="~/filterY" type="double" value="0.0" /> + + <remap from="/lpd/scan" + to="/dabo/sensors/rear_scan" /> <!-- topic: scan del robot --> + <remap from="/lpd/people" + to="/dabo/people" /> + </node> + + + <!-- Laser People map Filter --> + <!-- published topics: /$(env ROBOT)/peopleFiltered --> + <!-- subscribed topics: /$(env ROBOT)/people --> + <!-- /$(env ROBOT)map --> + <!-- service clients: --> + <!-- service servers: --> + <!-- action clients: --> + <!-- action servers: --> + <node pkg ="iri_laser_people_map_filter" + type="iri_laser_people_map_filter" + name="laser_people_map_filter"> + + <param name="~/filter" type="bool" value="True" /> <!-- true to enable filtering when we have a map --> + <param name="~/neighborRadius" type="double" value="0.3" /> + <param name="~/markerWidth" type="double" value="0.7" /> + <param name="~/markerHeight" type="double" value="1.5" /> + <param name="~/markerR" type="double" value="1.0" /> + <param name="~/markerG" type="double" value="1.0" /> + <param name="~/markerB" type="double" value="0.0" /> + <param name="~/markerA" type="double" value="0.9" /> + <param name="~/markerLifeTime" type="double" value="0.1" /> + + + <remap from="/laser_people_map_filter/people" + to="/dabo/people" /> + <remap from="/laser_people_map_filter/people_out" + to="/lpd_2/peopleMapFiltered" /> + <remap from="/laser_people_map_filter/map" + to="/dabo/map" /> <!-- topic of the map --> + + </node> + + +<!-- INI nav nodes _companion--> + <include file="$(find iri_dabo_rosnav)/launch/nav_map_companion.launch"> + <arg name="map_name" value="master_big" /> + <arg name="initial_x" value="19.0"/> + <arg name="initial_y" value="39.0"/> + <arg name="initial_yaw" value="0.0"/> + </include> + + +<!-- Launch the companion planner: --> + <group ns="dabo"> + <rosparam command="delete" ns="/dabo/move_base" /> + + <node pkg ="move_base" + type="move_base" + name="move_base2" + output="screen"> + <remap from="/dabo/move_base/cmd_vel" + to="/dabo/navigation/cmd_vel" /> + <remap from="/dabo/move_base/odom" + to="/dabo/odom" /> + <remap from="/dabo/front_scan" + to="/dabo/sensors/front_scan" /> + <remap from="/dabo/rear_scan" + to="/dabo/sensors/rear_scan" /> + <remap from="/dabo/tracks" + to="/mht_2/tracks" /> + <remap from="/dabo/move_base2/goal" + to="/dabo/move_base/goal" /> + <remap from="/dabo/planner_markers" + to="/dabo/planner_markers2" /> + <rosparam file="$(find iri_robot_aspsi)/config/dabo/move_base_params.yaml" command="load" /> + <rosparam file="$(find iri_robot_aspsi)/config/ana/costmap_common_params.yaml" command="load" ns="global_costmap" /> + <rosparam file="$(find iri_robot_aspsi)/config/dabo/costmap_common_params.yaml" command="load" ns="local_costmap" /> + <rosparam file="$(find iri_robot_aspsi)/config/dabo/local_costmap_params.yaml" command="load" /> + <rosparam file="$(find iri_robot_aspsi)/config/ana/global_costmap_params.yaml" command="load" /> + <rosparam file="$(find iri_robot_aspsi)/config/dabo/akp_local_planner_params.yaml" command="load" /> + <rosparam file="$(find iri_dabo_rosnav)/params/amcl_companion_test_ely2021_mapFME.yaml" command="load" /> + <!--<rosparam file="$(find iri_dabo_rosnav)/params/global_planner/global_planner_params.yaml" command="load" /> + <rosparam file="$(find iri_dabo_rosnav)/params/mux.yaml" command="load" />--> + <param name="AkpLocalPlanner/destination_map_path" value="$(find iri_robot_aspsi)/maps/master_big_destinations_Gazebo_sim.txt" /> + <param name="AkpLocalPlanner/results_filename_Zanlungo" value="name_file.txt" /> + <param name="AkpLocalPlanner/robot" value="dabo"/> + <param name="AkpLocalPlanner/person_goal_id" value="200"/> + <param name="AkpLocalPlanner/speed_k" value="1"/> + <param name="AkpLocalPlanner/person_radi_amp" value="0.2"/> + <param name="AkpLocalPlanner/change_treshold_distance_between_steps" value="0.3"/> + <!-- for SIM--> + <param name="AkpLocalPlanner/change_sim" value="False"/> + <param name="AkpLocalPlanner/sim_target_per" value="False"/> + <param name="AkpLocalPlanner/frame_robot_footprint" value="dabo/base_link"/> + </node> + </group> + + <node name="dr" pkg ="rqt_reconfigure" type="rqt_reconfigure" /> + + + + + <node name="rviz" pkg="rviz" type="rviz" args="-d $(find iri_akp_local_planner_companion)/config/tibi_comparacion_Vform_sideBySide.rviz" /> + + +</launch>