<launch>
<!--...................................
- Load URDF to param server
- Robot State Publisher
- Rviz with description from file.rviz
- Joint State Publisher with GUI
.......................................-->
<arg name="model" default="$(find visbot)/urdf/visbot.urdf"/>
<arg name="gui" default="True" />
<param name="robot_description" textfile="$(arg model)" />
<param name="use_gui" value="$(arg gui)"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find visbot)/urdf.rviz" respawn="true"/>
<!--<node name="rviz" pkg="rviz" type="rviz" args="-d $(find visbot)/urdf.rviz" required="true" />-->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="/zeros/YAW" value="1.570796327"/>
<param name="/zeros/PITCH" value="0.785398163"/>
<rosparam param="/source_list"> [/WheelsState] </rosparam>
</node>
<!--.......................................................................-->
<!--........................................................................
- jointrepnode Replublicador de la info de los servos
- rosserial para comunicarnos con el arduino de la cabeza
- ros arduino para comunicarnos con el arduino de la base
............................................................................-->
<node name="jointrepnode" pkg="visbot" type="jointrepnode"/> <!--nombre de nodo y ejecutable identicos-->
<node name="serial_node" pkg="rosserial_python" type="serial_node.py" args="/dev/ttyACM3" respawn="true"/>
<!--respawn para reiniciar el nodo automaticamente si se cierra-->
<node name="ros_arduino_node" pkg="ros_arduino_python" type="arduino_node.py" output="screen" respawn="true"
respawn_delay="10">
<rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
<rosparam param="/ros_arduino_node/port"> /dev/ttyUSB0 </rosparam>
<!--para poder manipular mas rapidamente el parametro del puerto-->
</node>
<!--.......................................................................-->
<!--
- Llamada a los servicios que encienden las camaras
- Republicadores de imagenes de camara izquierda y derecha
- stereo image proc
-->
<node name="ss" args="call /left/camera/start_capture" pkg="rosservice" type="rosservice"/>
<node name="sss" args="call /right/camera/start_capture" pkg="rosservice" type="rosservice"/>
<node name="republishLeft" args="compressed in:=/left/camera/image/ raw out:=/stereo/left/image_raw" pkg="image_transport" type="republish"/>
<node name="republishRight" args="compressed in:=/right/camera/image/ raw out:=/stereo/right/image_raw" pkg="image_transport" type="republish"/>
<node ns="stereo" name="stereo_image_proc" args="_approximate_sync:=True _queue_size:=5 _disparity_range:=200" pkg="stereo_image_proc" type="stereo_image_proc"/>
<!--.........................................................................-->
<!-- Choose visualization -->
<arg name="rviz" default="true" />
<arg name="rtabmapviz" default="true" />
<param name="use_sim_time" type="bool" value="False"/>
<!--<arg name="pi/2" value="1.5707963267948966" />-->
<!--<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />-->
<!--<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf" args="$(arg optical_rotate) /base_link /camera 100" />-->
<!--<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf" args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera 100" />-->
<!-- Stereo Odometry -->
<!--<node pkg="viso2_ros" type="stereo_odometer" name="stereo_odometer" output="screen">-->
<!--<remap from="stereo" to="/stereo"/>-->
<!--<remap from="image" to="image_rect"/>-->
<!--<remap from="stereo_odometer/odometry" to="/odometry"/>-->
<!--<param name="base_link_frame_id" value="base_link"/>-->
<!--<param name="odom_frame_id" value="odom"/>-->
<!--<param name="ref_frame_change_method" value="1"/>-->
<!--<param name="queue_size" value="5"/>-->
<!--<param name="approximate_sync" type="bool" value="True"/>-->
<!--</node>-->
<!-- ROS navigation stack move_base -->
<!--<group ns="planner">-->
<!--<remap from="openni_points" to="/planner_cloud"/>-->
<!--<remap from="base_scan" to="/base_scan"/>-->
<!--<remap from="map" to="/map"/>-->
<!--<remap from="move_base_simple/goal" to="/planner_goal"/>-->
<!--<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">-->
<!--<rosparam file="/home/ubuntu/catkin_ws/src/VisBot/ConfigFiles/costmap_common_params.yaml" command="load" ns="global_costmap" />-->
<!--<rosparam file="/home/ubuntu/catkin_ws/src/VisBot/ConfigFiles/costmap_common_params.yaml" command="load" ns="local_costmap" />-->
<!--<rosparam file="/home/ubuntu/catkin_ws/src/VisBot/ConfigFiles/local_costmap_params_3d.yaml" command="load" />-->
<!--<rosparam file="/home/ubuntu/catkin_ws/src/VisBot/ConfigFiles/global_costmap_params.yaml" command="load" />-->
<!--<rosparam file="/home/ubuntu/catkin_ws/src/VisBot/ConfigFiles/base_local_planner_params.yaml" command="load" />-->
<!--</node>-->
<!--<param name="cmd_vel/abtr_priority" value="10"/>-->
<!--</group>-->
<!-- Create point cloud for the planner -->
<node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap_ros/point_cloud_xyz stereo_nodelet">
<remap from="disparity/image" to="disparity"/>
<remap from="disparity/camera_info" to="right/camera_info_throttle"/>
<remap from="cloud" to="cloudXYZ"/>
<param name="voxel_size" type="double" value="0.05"/>
<param name="decimation" type="int" value="4"/>
<param name="max_depth" type="double" value="4"/>
</node>
<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection stereo_nodelet">
<remap from="cloud" to="cloudXYZ"/>
<remap from="obstacles" to="/planner_cloud"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="map_frame_id" type="string" value="map"/>
<param name="wait_for_transform" type="bool" value="true"/>
<param name="min_cluster_size" type="int" value="20"/>
<param name="max_obstacles_height" type="double" value="0.0"/>
</node>
<group ns="rtabmap">
<!-- Visual SLAM: args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start --udebug">
<param name="frame_id" type="string" value="/base_link"/>
<param name="subscribe_stereo" type="bool" value="true"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="wait_for_transform" type="bool" value="true"/><!--para los warnings de tf-->
<remap from="left/image_rect" to="/stereo/left/image_rect_color"/>
<remap from="right/image_rect" to="/stereo/right/image_rect_color"/>
<!--<remap from="right/image_rect" to="/stereo/right/image_rect"/>-->
<remap from="left/camera_info" to="/stereo/left/camera_info"/>
<remap from="right/camera_info" to="/stereo/right/camera_info"/>
<!--<remap from="odom" to="/odometry"/> -->
<remap from="odom" to="/odom"/>
<param name="queue_size" type="int" value="5"/>
<param name="stereo_approx_sync" type="bool" value="True"/>
<!-- RTAB-Map's parameters -->
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="Kp/WordsPerImage" type="string" value="200"/>
<param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree -->
<param name="SURF/HessianThreshold" type="string" value="1000"/>
<param name="LccBow/MaxDepth" type="string" value="5"/>
<param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="0.02"/>
<param name="LccReextract/Activated" type="string" value="true"/>
<param name="LccReextract/MaxWords" type="string" value="500"/>
<param name="Kp/MaxDepth" type="string" value="8.0"/>
<!-- Disable graph optimization because we use map_optimizer node below -->
<param name="RGBD/ToroIterations" type="string" value="0"/>
</node>
<!-- Optimizing outside rtabmap node makes it able to optimize always the global map -->
<node pkg="rtabmap_ros" type="map_optimizer" name="map_optimizer"/>
<node if="$(arg rviz)" pkg="rtabmap_ros" type="map_assembler" name="map_assembler">
<param name="occupancy_grid" type="bool" value="true"/>
<remap from="mapData" to="mapData_optimized"/>
<remap from="grid_projection_map" to="/map"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_stereo" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="false"/><!--true solo para uso de odometria rtabmap-->
<param name="queue_size" type="int" value="5"/>
<param name="frame_id" type="string" value="/base_link"/>
<param name="wait_for_transform" type="bool" value="true"/><!--para los warnings de tf-->
<remap from="left/image_rect" to="/stereo/left/image_rect_color"/>
<remap from="right/image_rect" to="/stereo/right/image_rect_color"/>
<!--<remap from="right/image_rect" to="/stereo/right/image_rect"/>-->
<remap from="left/camera_info" to="/stereo/left/camera_info"/>
<remap from="right/camera_info" to="/stereo/right/camera_info"/>
<remap from="odom_info" to="/odom_info"/>
<!--<remap from="odom" to="/odometry"/>-->
<remap from="odom" to="/odom"/>
<remap from="mapData" to="mapData_optimized"/>
</node>
</group>
<!-- Visualisation RVIZ -->
<!--<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_stereo_outdoor.rviz"/>-->
</launch>
