RoboticaUtnFrba · eborghi10 · Jul 7, 2019 · Jul 7, 2019 · Jul 7, 2019 · Jul 7, 2019
@@ -16,12 +16,13 @@ install:
   - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu xenial main" > /etc/apt/sources.list.d/ros-latest.list'
   - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
   - sudo apt-get update -qq
-  - sudo apt-get install -qq -y python-rosdep python-catkin-tools
+  - sudo apt-get install -qq -y python-rosdep python-catkin-tools python3-vcstool
   - sudo rosdep init
   - rosdep update
   # Use rosdep to install all dependencies (including ROS itself)
   - git clone https://github.com/RoboticaUtnFrba/i2c_imu.git $TRAVIS_BUILD_DIR/i2c_imu
   - git clone https://github.com/RoboticaUtnFrba/libcreate.git $TRAVIS_BUILD_DIR/libcreate
+  - git clone https://github.com/RoboticaUtnFrba/mbf_tools.git $TRAVIS_BUILD_DIR/mbf_tools
   - rosdep install --from-paths ./ -i -y --rosdistro $CI_ROS_DISTRO
   # Install and compile RTIMULib2
   - git clone https://github.com/RoboticaUtnFrba/RTIMULib2.git $TRAVIS_BUILD_DIR/RTIMULib2

@@ -15,7 +15,23 @@ ENV NVIDIA_VISIBLE_DEVICES=all NVIDIA_DRIVER_CAPABILITIES=all
 USER root
 RUN apt-get update
 
+# Added dependencies
 RUN apt-get install -y \
-  ros-$ROS1_DISTRO-image-proc
+  libopencv-dev \
+  ros-$ROS1_DISTRO-dwa-local-planner \
+  ros-$ROS1_DISTRO-eband-local-planner \
+  ros-$ROS1_DISTRO-global-planner \
+  ros-$ROS1_DISTRO-image-proc \
+  ros-$ROS1_DISTRO-joy \
+  ros-$ROS1_DISTRO-joy-teleop \
+  ros-$ROS1_DISTRO-libdlib \
+  ros-$ROS1_DISTRO-map-server \
+  ros-$ROS1_DISTRO-mbf-costmap-core \
+  ros-$ROS1_DISTRO-mbf-msgs \
+  ros-$ROS1_DISTRO-move-base-flex \
+  ros-$ROS1_DISTRO-opengm \
+  ros-$ROS1_DISTRO-sbpl-lattice-planner \
+  ros-$ROS1_DISTRO-slam-gmapping \
+  ros-$ROS1_DISTRO-smach-viewer
 
 USER create
@@ -15,7 +15,23 @@ ENV NVIDIA_VISIBLE_DEVICES=all NVIDIA_DRIVER_CAPABILITIES=all
 USER root
 RUN apt-get update
 
+# Added dependencies
 RUN apt-get install -y \
-  ros-$ROS1_DISTRO-image-proc
+  libopencv-dev \
+  ros-$ROS1_DISTRO-dwa-local-planner \
+  # ros-$ROS1_DISTRO-eband-local-planner \
+  ros-$ROS1_DISTRO-global-planner \
+  ros-$ROS1_DISTRO-image-proc \
+  ros-$ROS1_DISTRO-joy \
+  ros-$ROS1_DISTRO-joy-teleop \
+  # ros-$ROS1_DISTRO-libdlib \
+  ros-$ROS1_DISTRO-map-server \
+  ros-$ROS1_DISTRO-mbf-costmap-core \
+  ros-$ROS1_DISTRO-mbf-msgs \
+  ros-$ROS1_DISTRO-move-base-flex \
+  # ros-$ROS1_DISTRO-opengm \
+  ros-$ROS1_DISTRO-sbpl-lattice-planner \
+  # ros-$ROS1_DISTRO-slam-gmapping \
+  ros-$ROS1_DISTRO-smach-viewer
 
 USER create
@@ -16,9 +16,6 @@ find_package(catkin REQUIRED COMPONENTS
   tf
 )
 
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
 ###################################
 ## catkin specific configuration ##
 ###################################
@@ -28,12 +25,7 @@ find_package(catkin REQUIRED COMPONENTS
 ## LIBRARIES: libraries you create in this project that dependent projects also need
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES ca_move_base
-#  CATKIN_DEPENDS actionlib move_base move_base_msgs roscpp tf
-#  DEPENDS system_lib
-)
+catkin_package()
 
 ###########
 ## Build ##
@@ -46,80 +38,12 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/ca_move_base.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
 add_executable(send_goal src/send_robot_goal.cpp)
 
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
 ## Specify libraries to link a library or executable target against
 target_link_libraries(send_goal
   ${catkin_LIBRARIES}
 )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables and/or libraries for installation
-# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_ca_move_base.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
@@ -0,0 +1,101 @@
+# controllers:
+#    - name: 'eband'
+#      type: 'eband_local_planner/EBandPlannerROS'
+#    - name: 'dwa'
+#      type: 'dwa_local_planner/DWAPlannerROS'
+# eband:
+#   Ctrl_Rate: 20.0
+#   bubble_velocity_multiplier: 2.0
+#   costmap_weight: 10.0
+#   differential_drive: true
+#   disallow_hysteresis: false
+#   eband_equilibrium_approx_max_recursion_depth: 3
+#   eband_equilibrium_relative_overshoot: 0.75
+#   eband_external_force_gain: 3.0
+#   eband_internal_force_gain: 2.0
+#   eband_min_relative_overlap: 0.7
+#   eband_significant_force_lower_bound: 0.15
+#   eband_tiny_bubble_distance: 0.01
+#   eband_tiny_bubble_expansion: 0.01
+#   in_place_trans_vel: 0.0
+#   k_damp: 3.5
+#   k_prop: 4.0
+#   marker_lifetime: 0.5
+#   max_acceleration: 0.2
+#   max_rotational_acceleration: 0.3
+#   max_translational_acceleration: 0.4
+#   # Lower values for testing with the actual robot.
+#   max_vel_lin: 1.1 #0.5 #1.45
+#   max_vel_th: 1.2 #0.5 #2.0
+#   min_in_place_vel_th: 0.05
+#   min_vel_lin: 0.0
+#   min_vel_th: 0.0
+#   num_iterations_eband_optimization: 4
+#   rot_stopped_vel: 0.01
+#   rotation_correction_threshold: 0.5
+#   rotation_threshold_multiplier: 1.0
+#   trans_stopped_vel: 0.01
+#   virtual_mass: 100.0
+#   xy_goal_tolerance: 0.2
+#   yaw_goal_tolerance: 0.2
+
+# dwa:
+#   max_vel_x: 2.0
+#   min_vel_x: 0.0
+
+#   max_vel_y: 0.0
+#   min_vel_y: 0.0
+
+#   max_trans_vel: 2.12
+#   min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
+#   trans_stopped_vel: 0.1
+
+#   # Warning!
+#   #   do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities
+#   #   are non-negligible and small in place rotational velocities will be created.
+
+#   max_rot_vel: 2.0  # choose slightly less than the base's capability
+#   min_rot_vel: 0.4  # this is the min angular velocity when there is negligible translational velocity
+#   rot_stopped_vel: 0.1
+
+#   acc_lim_x: 0.7 # maximum is theoretically 2.0, but we
+#   acc_lim_theta: 10.0
+#   acc_lim_y: 0.1      # diff drive robot
+
+#   # Goal Tolerance Parameters
+#   yaw_goal_tolerance: 0.4  # 0.05
+#   xy_goal_tolerance: 0.2   # 0.10
+#   # latch_xy_goal_tolerance: false
+
+#   # Forward Simulation Parameters
+#   sim_time: 2.0       # 1.7
+#   sim_granularity: 0.025
+#   angular_sim_granularity: 0.025
+#   vx_samples: 20      # 3
+#   vy_samples: 1       # diff drive robot, there is only one sample
+#   vtheta_samples: 20  # 20
+
+#   # Trajectory Scoring Parameters
+#   path_distance_bias: 0.1      # 32.0   - weighting for how much it should stick to the global path plan -- if this value is too high, won't avoid dynamic obstacles
+#   goal_distance_bias: 40.0     # 24.0   - wighting for how much it should attempt to reach its goal
+#   occdist_scale: 0.2           # 0.01   - weighting for how much the controller should avoid obstacles
+#   forward_point_distance: 0.0  # 0.325  - how far along to place an additional scoring point
+#   stop_time_buffer: 0.3        # 0.2    - amount of time a robot must stop in before colliding for a valid traj.
+#   scaling_speed: 1.0           # 0.25   - absolute velocity at which to start scaling the robot's footprint
+#   max_scaling_factor: 0.2      # 0.2    - how much to scale the robot's footprint when at speed.
+
+#   # Oscillation Prevention Parameters
+#   oscillation_reset_dist: 0.1  # 0.05   - how far to travel before resetting oscillation flags
+
+#   holonomic_robot: false
+
+#     # Debugging
+#   publish_traj_pc: true
+#   publish_cost_grid_pc: true
+#   global_frame_id: map
+
+controllers:
+  - name: 'controller'
+    type: 'base_local_planner/TrajectoryPlannerROS'
+controller:
+  holonomic_robot: false
@@ -0,0 +1,38 @@
+# planners:
+#   - name: 'GlobalPlanner'
+#     type: 'global_planner/GlobalPlanner'
+#   - name: 'SBPLLatticePlanner'
+#     type: 'SBPLLatticePlanner'
+# planner_patience: 10.0
+# GlobalPlanner:
+#   allow_unknown: false
+#   default_tolerance: 0.0
+#   visualize_potential: true
+#   use_dijkstra: true
+#   use_quadratic: false
+#   use_grid_path: false
+#   old_navfn_behaviour: false
+#   lethal_cost: 253
+#   neutral_cost: 66
+#   cost_factor: 0.55
+# SBPLLatticePlanner:
+#   planner_type: 'ARAPlanner'
+#   allocated_time: 20.0 # default: 10.0
+#   initial_epsilon: 3.0
+#   environment_type: 'XYThetaLattice' # The only environment supported
+#   forward_search: true # default is false
+#   nominalvel_mpersecs: 0.7
+#   timetoturn45degsinplace_secs: 0.6
+#   lethal_obstacle: 250
+
+planners: 
+  - name: 'planner'
+    type: 'global_planner/GlobalPlanner'
+planner:
+  allow_unknown: false
+  default_tolerance: 0.0
+  visualize_potential: false
+  use_dijkstra: true
+  use_quadratic: false
+  use_grid_path: false
+  old_navfn_behaviour: false
@@ -0,0 +1,12 @@
+recovery_behaviors:
+  - name: 'rotate_recovery'
+    type: 'rotate_recovery/RotateRecovery'
+  - name: 'clear_costmap'
+    type: 'clear_costmap_recovery/ClearCostmapRecovery'
+  - name: 'moveback_recovery'
+    type: 'moveback_recovery/MoveBackRecovery'
+
+moveback_recovery:
+  linear_vel_back: -0.1 # default -0.3
+  step_back_length: 0.3  # default 1.0
+  step_back_timeout: 5.0 # default 15.0
@@ -0,0 +1,15 @@
+<launch>
+  <!-- Move Base Flex -->
+  <node pkg="mbf_costmap_nav" type="mbf_costmap_nav" respawn="false" name="move_base_flex" output="screen">
+    <param name="tf_timeout" value="1.5"/>
+    <param name="planner_max_retries" value="3"/>
+    <param name="SBPLLatticePlanner/primitive_filename" value="$(find ca_move_base)/mprim/ceres_5cm.mprim" />
+    <rosparam file="$(find ca_move_base)/config/planners.yaml" command="load" />
+    <rosparam file="$(find ca_move_base)/config/controllers.yaml" command="load" />
+    <rosparam file="$(find ca_move_base)/config/recovery_behaviors.yaml" command="load" />
+    <rosparam file="$(find ca_move_base)/config/costmap_common_params.yaml" command="load" ns="global_costmap"/>
+    <rosparam file="$(find ca_move_base)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find ca_move_base)/config/global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find ca_move_base)/config/local_costmap_params.yaml" command="load" />
+  </node>
+</launch>
@@ -9,7 +9,10 @@
   <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)"/>
 
 	<!-- move_base -->
-	<include file="$(find ca_move_base)/launch/move_base.launch" />
+	<include file="$(find ca_move_base)/launch/move_base_flex.launch" />
+
+  <!-- SMACH Navigation State Machine -->
+  <node pkg="ca_move_base" type="mbf_move_base_action.py" name="mbf_move_base_action" output="screen"/>
 
 	<!-- AMCL -->
   <arg name="initial_pose_x"  default="0.0"/>