diff --git a/do_everything.sh b/do_everything.sh
index dfba9b1..9541a1c 100755
--- a/do_everything.sh
+++ b/do_everything.sh
@@ -272,6 +272,12 @@ if [[ $skip -ne 1 ]] ; then
# TODO: The correct way to handle this would be to create .cmake files for fcl and do a findpackage(fcl)
patch -p0 -N -d $prefix < /opt/roscpp_android/patches/moveit_core.patch
+ # Patch move_base - Remove pluginlib
+ patch -p0 -N -d $prefix < /opt/roscpp_android/patches/move_base.patch
+
+ # Patch costmap_2d - Remove pluginlib
+ patch -p0 -N -d $prefix < /opt/roscpp_android/patches/costmap_2d.patch
+
fi
echo
@@ -348,6 +354,19 @@ echo
(cd $prefix/sample_app && ant debug)
+echo
+echo -e '\e[34mCreating move_base sample app.\e[39m'
+echo
+
+cp $my_loc/files/Android.mk.move_base $prefix/roscpp_android_ndk/Android.mk
+( cd $prefix && run_cmd sample_app move_base_app $prefix/roscpp_android_ndk )
+
+echo
+echo -e '\e[34mBuilding move_base apk.\e[39m'
+echo
+
+(cd $prefix/move_base_app && ant debug)
+
echo
echo 'done.'
echo 'summary of what just happened:'
@@ -356,4 +375,6 @@ echo ' include/ contains headers'
echo ' lib/ contains static libraries'
echo ' roscpp_android_ndk/ is a NDK sub-project that can be imported into an NDK app'
echo ' sample_app/ is an example of such an app, a native activity'
-echo ' sample_app/bin/sample_app-debug.apk is the built apk'
+echo ' sample_app/bin/sample_app-debug.apk is the built apk, it implements a subscriber and a publisher'
+echo ' move_base_sample_app/ is an example app that implements the move_base node'
+echo ' move_base_app/bin/move_base_app-debug.apk is the built apk for the move_base example'
diff --git a/files/Android.mk.algron b/files/Android.mk.algron
deleted file mode 100644
index 926ceaf..0000000
--- a/files/Android.mk.algron
+++ /dev/null
@@ -1,46 +0,0 @@
-LOCAL_PATH := $(call my-dir)
-
-# These must go in some sort of order like include flags, otherwise they are dropped
-# Oh no, need to automate this with catkin somehow....
-stlibs := topic_tools roscpp boost_signals boost_filesystem rosconsole rosconsole_print rosconsole_backend_interface boost_regex roscpp_serialization boost_date_time boost_system boost_thread xmlrpcpp cpp_common console_bridge move_base dynamic_reconfigure_config_init_mutex global_planner navfn costmap_2d ardrone_sdk ardrone_driver rostime pc_ardrone avcodec swscale avutil vlib sdk image_transport camera_info_manager roslib rospack tinyxml class_loader PocoFoundation nav_to_odom pose_viz utm_transform tf tf2_ros actionlib message_filters tf2 straight_line_planner camera_calibration_parsers boost_program_options yaml-cpp utm_odometry ros_filter filter_base ekf base_local_planner altitude_controller
-
-#shlibs :=
-
-define include_shlib
-$(eval include $$(CLEAR_VARS))
-$(eval LOCAL_MODULE := $(1))
-$(eval LOCAL_SRC_FILES := $$(LOCAL_PATH)/lib/lib$(1).so)
-$(eval include $$(PREBUILT_SHARED_LIBRARY))
-endef
-define include_stlib
-$(eval include $$(CLEAR_VARS))
-$(eval LOCAL_MODULE := $(1))
-$(eval LOCAL_SRC_FILES := $$(LOCAL_PATH)/lib/lib$(1).a)
-$(eval include $$(PREBUILT_STATIC_LIBRARY))
-endef
-
-#$(foreach shlib,$(shlibs),$(eval $(call include_shlib,$(shlib))))
-$(foreach stlib,$(stlibs),$(eval $(call include_stlib,$(stlib))))
-
-include $(CLEAR_VARS)
-LOCAL_MODULE := roscpp_android_ndk
-LOCAL_EXPORT_C_INCLUDES := $(LOCAL_PATH)/include
-LOCAL_EXPORT_C_INCLUDES += $(LOCAL_PATH)/include/ardrone
-LOCAL_EXPORT_C_INCLUDES += $(LOCAL_PATH)/include/ardrone/Common
-LOCAL_EXPORT_C_INCLUDES += $(LOCAL_PATH)/include/ardrone/VP_SDK
-LOCAL_EXPORT_C_INCLUDES += $(LOCAL_PATH)/include/ardrone/linux
-LOCAL_EXPORT_C_INCLUDES += $(LOCAL_PATH)/include/ardrone/Lib
-LOCAL_EXPORT_C_INCLUDES += $(LOCAL_PATH)/include/ardrone/FFMPEG/Includes
-LOCAL_EXPORT_CPPFLAGS := -fexceptions -frtti
-
-# Do not change this next line as is it commented / uncommented by build_ndk.sh depending on the debugging requirements
-# This variable makes all the dependent modules inherit the flags
-#LOCAL_EXPORT_CFLAGS += -g
-
-#LOCAL_SRC_FILES := dummy.cpp
-#LOCAL_EXPORT_LDLIBS := $(foreach l,$(shlibs),-l$(l)) -L$(LOCAL_PATH)/lib
-#LOCAL_EXPORT_LDLIBS := -lstdc++ #-L$(LOCAL_PATH)/lib
-#LOCAL_SHARED_LIBRARIES := $(shlibs)
-LOCAL_STATIC_LIBRARIES := $(stlibs) gnustl_static
-
-include $(BUILD_STATIC_LIBRARY)
diff --git a/files/Android.mk.move_base b/files/Android.mk.move_base
new file mode 100644
index 0000000..62180a3
--- /dev/null
+++ b/files/Android.mk.move_base
@@ -0,0 +1,36 @@
+LOCAL_PATH := $(call my-dir)
+
+# These must go in some sort of order like include flags, otherwise they are dropped
+# Oh no, need to automate this with catkin somehow....
+stlibs := roscpp boost_signals boost_filesystem rosconsole rosconsole_print rosconsole_backend_interface boost_regex xmlrpcpp roscpp_serialization rostime boost_date_time cpp_common boost_system boost_thread console_bridge move_base rotate_recovery global_planner navfn layers boost_iostreams qhullstatic flann_cpp_s flann_cpp_s-gd nodeletlib bondcpp uuid rosbag rosbag_storage roslz4 lz4 topic_tools voxel_grid tf tf2_ros actionlib tf2 move_slow_and_clear dwa_local_planner clear_costmap_recovery carrot_planner base_local_planner trajectory_planner_ros urdfdom_sensor urdfdom_model_state urdfdom_model urdfdom_world rosconsole_bridge pointcloud_filters laser_scan_filters mean params increment median transfer_function compressed_image_transport cv_bridge opencv_core opencv_androidcamera opencv_flann opencv_imgproc opencv_highgui opencv_features2d opencv_calib3d opencv_ml opencv_video opencv_legacy opencv_objdetect opencv_photo opencv_gpu opencv_videostab opencv_ocl opencv_superres opencv_nonfree opencv_stitching opencv_contrib image_transport compressed_depth_image_transport amcl_sensors amcl_map amcl_pf stereo_image_proc image_proc image_geometry polled_camera camera_info_manager pluginlib_tutorials nodelet_math collada_parser geometric_shapes octomap octomath shape_tools random_numbers camera_calibration_parsers costmap_2d laser_geometry message_filters resource_retriever dynamic_reconfigure_config_init_mutex tinyxml class_loader PocoFoundation roslib rospack boost_program_options pcl_ros_filters pcl_ros_io pcl_ros_tf pcl_common pcl_octree pcl_kdtree pcl_search pcl_sample_consensus pcl_filters pcl_io pcl_features pcl_registration pcl_keypoints pcl_ml pcl_segmentation pcl_stereo pcl_tracking pcl_recognition pcl_surface
+
+
+#shlibs :=
+
+define include_shlib
+$(eval include $$(CLEAR_VARS))
+$(eval LOCAL_MODULE := $(1))
+$(eval LOCAL_SRC_FILES := $$(LOCAL_PATH)/lib/lib$(1).so)
+$(eval include $$(PREBUILT_SHARED_LIBRARY))
+endef
+define include_stlib
+$(eval include $$(CLEAR_VARS))
+$(eval LOCAL_MODULE := $(1))
+$(eval LOCAL_SRC_FILES := $$(LOCAL_PATH)/lib/lib$(1).a)
+$(eval include $$(PREBUILT_STATIC_LIBRARY))
+endef
+
+#$(foreach shlib,$(shlibs),$(eval $(call include_shlib,$(shlib))))
+$(foreach stlib,$(stlibs),$(eval $(call include_stlib,$(stlib))))
+
+include $(CLEAR_VARS)
+LOCAL_MODULE := roscpp_android_ndk
+LOCAL_EXPORT_C_INCLUDES := $(LOCAL_PATH)/include
+LOCAL_EXPORT_CPPFLAGS := -fexceptions -frtti
+#LOCAL_SRC_FILES := dummy.cpp
+#LOCAL_EXPORT_LDLIBS := $(foreach l,$(shlibs),-l$(l)) -L$(LOCAL_PATH)/lib
+#LOCAL_EXPORT_LDLIBS := -lstdc++ #-L$(LOCAL_PATH)/lib
+#LOCAL_SHARED_LIBRARIES := $(shlibs)
+LOCAL_STATIC_LIBRARIES := $(stlibs) gnustl_static
+
+include $(BUILD_STATIC_LIBRARY)
diff --git a/files/move_base_app/AndroidManifest.xml.in b/files/move_base_app/AndroidManifest.xml.in
new file mode 100644
index 0000000..197b735
--- /dev/null
+++ b/files/move_base_app/AndroidManifest.xml.in
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/files/move_base_app/custom_rules.xml.in b/files/move_base_app/custom_rules.xml.in
new file mode 100644
index 0000000..189a837
--- /dev/null
+++ b/files/move_base_app/custom_rules.xml.in
@@ -0,0 +1,7 @@
+
+
+
+
+
+
+
diff --git a/files/move_base_app/ic_launcher.png b/files/move_base_app/ic_launcher.png
new file mode 100644
index 0000000..c7956d1
Binary files /dev/null and b/files/move_base_app/ic_launcher.png differ
diff --git a/files/move_base_app/jni/Android.mk.in b/files/move_base_app/jni/Android.mk.in
new file mode 100644
index 0000000..780e4e3
--- /dev/null
+++ b/files/move_base_app/jni/Android.mk.in
@@ -0,0 +1,14 @@
+LOCAL_PATH := $(call my-dir)
+
+include $(CLEAR_VARS)
+
+LOCAL_MODULE := move_base_app
+LOCAL_SRC_FILES := src/test_move_base.cpp
+LOCAL_C_INCLUDES := $(LOCAL_PATH)/include
+LOCAL_LDLIBS := -landroid
+LOCAL_STATIC_LIBRARIES := android_native_app_glue roscpp_android_ndk
+
+include $(BUILD_SHARED_LIBRARY)
+
+$(call import-module,android/native_app_glue)
+$(call import-module,roscpp_android_ndk)
diff --git a/files/move_base_app/jni/Application.mk b/files/move_base_app/jni/Application.mk
new file mode 100644
index 0000000..2a5d55c
--- /dev/null
+++ b/files/move_base_app/jni/Application.mk
@@ -0,0 +1,4 @@
+#NDK_TOOLCHAIN_VERSION=4.4.3
+APP_STL := gnustl_static
+APP_PLATFORM := android-14
+APP_ABI := armeabi-v7a
\ No newline at end of file
diff --git a/files/move_base_app/jni/src/test_move_base.cpp b/files/move_base_app/jni/src/test_move_base.cpp
new file mode 100644
index 0000000..4ff67ac
--- /dev/null
+++ b/files/move_base_app/jni/src/test_move_base.cpp
@@ -0,0 +1,48 @@
+#include "ros/ros.h"
+#include
+
+#include
+#include
+
+void log(const char *msg, ...) {
+ va_list args;
+ va_start(args, msg);
+ __android_log_vprint(ANDROID_LOG_INFO, "MOVE_BASE_NDK_EXAMPLE", msg, args);
+ va_end(args);
+}
+
+void android_main(android_app *papp) {
+ // Make sure glue isn't stripped
+ app_dummy();
+
+ int argc = 4;
+ // TODO: don't hardcode ip addresses
+ char *argv[] = {"nothing_important" , "__master:=http://192.168.1.100:11311",
+ "__ip:=192.168.1.101", "cmd_vel:=navigation_velocity_smoother/raw_cmd_vel"};
+
+ for(int i = 0; i < argc; i++){
+ log(argv[i]);
+ }
+
+ ros::init(argc, &argv[0], "move_base");
+
+ std::string master_uri = ros::master::getURI();
+
+ if(ros::master::check()){
+ log("ROS MASTER IS UP!");
+ } else {
+ log("NO ROS MASTER.");
+ }
+ log(master_uri.c_str());
+
+ ros::NodeHandle n;
+
+ tf::TransformListener tf(ros::Duration(10));
+ move_base::MoveBase move_base(tf);
+
+ ros::WallRate loop_rate(100);
+ while(ros::ok() && !papp->destroyRequested){
+ ros::spinOnce();
+ loop_rate.sleep();
+ }
+}
diff --git a/patches/costmap_2d.patch b/patches/costmap_2d.patch
new file mode 100644
index 0000000..855cb22
--- /dev/null
+++ b/patches/costmap_2d.patch
@@ -0,0 +1,90 @@
+diff -ur catkin_ws/src/navigation/costmap_2d/CMakeLists.txt catkin_ws/src/navigation/costmap_2d/CMakeLists.txt
+--- catkin_ws/src/navigation/costmap_2d/CMakeLists.txt 2014-12-06 03:49:50.000000000 -0300
++++ catkin_ws/src/navigation/costmap_2d/CMakeLists.txt 2015-02-20 19:51:25.355809604 -0300
+@@ -95,6 +95,11 @@
+ src/costmap_math.cpp
+ src/footprint.cpp
+ src/costmap_layer.cpp
++ plugins/footprint_layer.cpp
++ plugins/inflation_layer.cpp
++ plugins/obstacle_layer.cpp
++ plugins/static_layer.cpp
++ src/observation_buffer.cpp
+ )
+ add_dependencies(costmap_2d geometry_msgs_gencpp)
+ target_link_libraries(costmap_2d
+diff -ur catkin_ws/src/navigation/costmap_2d/include/costmap_2d/costmap_2d_ros.h catkin_ws/src/navigation/costmap_2d/include/costmap_2d/costmap_2d_ros.h
+--- catkin_ws/src/navigation/costmap_2d/include/costmap_2d/costmap_2d_ros.h 2014-12-06 03:49:50.000000000 -0300
++++ catkin_ws/src/navigation/costmap_2d/include/costmap_2d/costmap_2d_ros.h 2015-02-20 19:54:14.015805735 -0300
+@@ -45,7 +45,10 @@
+ #include
+ #include
+ #include
+-#include
++#include
++#include
++#include
++#include
+
+ class SuperValue : public XmlRpc::XmlRpcValue
+ {
+@@ -280,7 +283,6 @@
+ ros::Timer timer_;
+ ros::Time last_publish_;
+ ros::Duration publish_cycle;
+- pluginlib::ClassLoader plugin_loader_;
+ tf::Stamped old_pose_;
+ Costmap2DPublisher* publisher_;
+ dynamic_reconfigure::Server *dsrv_;
+diff -ur catkin_ws/src/navigation/costmap_2d/src/costmap_2d_ros.cpp catkin_ws/src/navigation/costmap_2d/src/costmap_2d_ros.cpp
+--- catkin_ws/src/navigation/costmap_2d/src/costmap_2d_ros.cpp 2014-12-06 03:49:50.000000000 -0300
++++ catkin_ws/src/navigation/costmap_2d/src/costmap_2d_ros.cpp 2015-02-20 19:23:38.511847844 -0300
+@@ -62,9 +62,7 @@
+
+ Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf) :
+ layered_costmap_(NULL), name_(name), tf_(tf), stop_updates_(false), initialized_(true), stopped_(false), robot_stopped_(
+- false), map_update_thread_(NULL), last_publish_(0), plugin_loader_("costmap_2d",
+- "costmap_2d::Layer"), publisher_(
+- NULL)
++ false), map_update_thread_(NULL), last_publish_(0), publisher_(NULL)
+ {
+ ros::NodeHandle private_nh("~/" + name);
+ ros::NodeHandle g_nh;
+@@ -114,16 +112,28 @@
+ {
+ XmlRpc::XmlRpcValue my_list;
+ private_nh.getParam("plugins", my_list);
+- for (int32_t i = 0; i < my_list.size(); ++i)
+- {
+- std::string pname = static_cast(my_list[i]["name"]);
+- std::string type = static_cast(my_list[i]["type"]);
+- ROS_INFO("Using plugin \"%s\"", pname.c_str());
+-
+- boost::shared_ptr plugin = plugin_loader_.createInstance(type);
+- layered_costmap_->addPlugin(plugin);
+- plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
++
++ ROS_INFO("Using plugin \"footprint_layer\"");
++ boost::shared_ptr footprint (new costmap_2d::FootprintLayer());
++ layered_costmap_->addPlugin(footprint);
++ footprint->initialize(layered_costmap_, name + "/" + std::string("footprint_layer"), &tf_);
++
++ ROS_INFO("Using plugin \"inflation_layer\"");
++ boost::shared_ptr inflation (new costmap_2d::InflationLayer());
++ layered_costmap_->addPlugin(inflation);
++ inflation->initialize(layered_costmap_, name + "/" + std::string("inflation_layer"), &tf_);
++
++ if(name == "global_costmap"){
++ ROS_INFO("Using plugin \"static_layer\"");
++ boost::shared_ptr static_ (new costmap_2d::StaticLayer());
++ layered_costmap_->addPlugin(static_);
++ static_->initialize(layered_costmap_, name + "/" + std::string("static_layer"), &tf_);
+ }
++
++ ROS_INFO("Using plugin \"obstacle_layer\"");
++ boost::shared_ptr obstacle (new costmap_2d::ObstacleLayer());
++ layered_costmap_->addPlugin(obstacle);
++ obstacle->initialize(layered_costmap_, name + "/" + std::string("obstacle_layer"), &tf_);
+ }
+
+ // subscribe to the footprint topic
diff --git a/patches/move_base.patch b/patches/move_base.patch
new file mode 100644
index 0000000..6edeab8
--- /dev/null
+++ b/patches/move_base.patch
@@ -0,0 +1,392 @@
+diff -ur catkin_ws/src/navigation/move_base/CMakeLists.txt catkin_ws/src/navigation/move_base/CMakeLists.txt
+--- catkin_ws/src/navigation/move_base/CMakeLists.txt 2014-12-06 03:49:40.000000000 -0300
++++ catkin_ws/src/navigation/move_base/CMakeLists.txt 2015-02-04 18:22:06.292433561 -0300
+@@ -8,6 +8,10 @@
+ pluginlib
+ actionlib
+ dynamic_reconfigure
++ navfn
++ base_local_planner
++ dwa_local_planner
++ clear_costmap_recovery
+ message_generation
+ nav_core
+ tf
+@@ -24,6 +28,10 @@
+ CATKIN_DEPENDS
+ roscpp
+ dynamic_reconfigure
++ navfn
++ base_local_planner
++ dwa_local_planner
++ clear_costmap_recovery
+ )
+
+ include_directories(
+diff -ur catkin_ws/src/navigation/move_base/include/move_base/move_base.h catkin_ws/src/navigation/move_base/include/move_base/move_base.h
+--- catkin_ws/src/navigation/move_base/include/move_base/move_base.h 2014-12-06 03:49:40.000000000 -0300
++++ catkin_ws/src/navigation/move_base/include/move_base/move_base.h 2015-02-04 19:08:33.244369624 -0300
+@@ -56,6 +56,10 @@
+ #include
+ #include
+
++#include
++#include
++#include
++
+ #include
+ #include "move_base/MoveBaseConfig.h"
+
+@@ -201,9 +205,6 @@
+
+ ros::Time last_valid_plan_, last_valid_control_, last_oscillation_reset_;
+ geometry_msgs::PoseStamped oscillation_pose_;
+- pluginlib::ClassLoader bgp_loader_;
+- pluginlib::ClassLoader blp_loader_;
+- pluginlib::ClassLoader recovery_loader_;
+
+ //set up plan triple buffer
+ std::vector* planner_plan_;
+diff -ur catkin_ws/src/navigation/move_base/package.xml catkin_ws/src/navigation/move_base/package.xml
+--- catkin_ws/src/navigation/move_base/package.xml 2014-12-06 03:49:40.000000000 -0300
++++ catkin_ws/src/navigation/move_base/package.xml 2015-02-04 18:20:17.544436056 -0300
+@@ -35,6 +35,7 @@
+
+ base_local_planner
+ clear_costmap_recovery
++ dwa_local_planner
+ navfn
+ rotate_recovery
+ base_local_planner
+@@ -46,6 +47,7 @@
+
+ actionlib
+ costmap_2d
++ dwa_local_planner
+ dynamic_reconfigure
+ geometry_msgs
+ message_runtime
+diff -ur catkin_ws/src/navigation/move_base/src/move_base.cpp catkin_ws/src/navigation/move_base/src/move_base.cpp
+--- catkin_ws/src/navigation/move_base/src/move_base.cpp 2014-12-06 03:49:40.000000000 -0300
++++ catkin_ws/src/navigation/move_base/src/move_base.cpp 2015-02-04 18:19:05.148437717 -0300
+@@ -49,9 +49,6 @@
+ tf_(tf),
+ as_(NULL),
+ planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
+- bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
+- blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),
+- recovery_loader_("nav_core", "nav_core::RecoveryBehavior"),
+ planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
+ runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false) {
+
+@@ -112,57 +109,17 @@
+ planner_costmap_ros_->pause();
+
+ //initialize the global planner
+- try {
+- //check if a non fully qualified name has potentially been passed in
+- if(!bgp_loader_.isClassAvailable(global_planner)){
+- std::vector classes = bgp_loader_.getDeclaredClasses();
+- for(unsigned int i = 0; i < classes.size(); ++i){
+- if(global_planner == bgp_loader_.getName(classes[i])){
+- //if we've found a match... we'll get the fully qualified name and break out of the loop
+- ROS_WARN("Planner specifications should now include the package name. You are using a deprecated API. Please switch from %s to %s in your yaml file.",
+- global_planner.c_str(), classes[i].c_str());
+- global_planner = classes[i];
+- break;
+- }
+- }
+- }
+-
+- planner_ = bgp_loader_.createInstance(global_planner);
+- planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
+- } catch (const pluginlib::PluginlibException& ex)
+- {
+- ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
+- exit(1);
+- }
++ planner_.reset(new navfn::NavfnROS());
++ planner_->initialize(std::string("navfn"), planner_costmap_ros_);
+
+ //create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
+ controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
+ controller_costmap_ros_->pause();
+
+ //create a local planner
+- try {
+- //check if a non fully qualified name has potentially been passed in
+- if(!blp_loader_.isClassAvailable(local_planner)){
+- std::vector classes = blp_loader_.getDeclaredClasses();
+- for(unsigned int i = 0; i < classes.size(); ++i){
+- if(local_planner == blp_loader_.getName(classes[i])){
+- //if we've found a match... we'll get the fully qualified name and break out of the loop
+- ROS_WARN("Planner specifications should now include the package name. You are using a deprecated API. Please switch from %s to %s in your yaml file.",
+- local_planner.c_str(), classes[i].c_str());
+- local_planner = classes[i];
+- break;
+- }
+- }
+- }
+-
+- tc_ = blp_loader_.createInstance(local_planner);
+- ROS_INFO("Created local_planner %s", local_planner.c_str());
+- tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
+- } catch (const pluginlib::PluginlibException& ex)
+- {
+- ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
+- exit(1);
+- }
++ tc_.reset(new dwa_local_planner::DWAPlannerROS());
++ ROS_INFO("Created local_planner %s", "dwa_local_planner");
++ tc_->initialize(std::string("DWAPlannerROS"), &tf_, controller_costmap_ros_);
+
+ // Start actively updating costmaps based on sensor data
+ planner_costmap_ros_->start();
+@@ -181,10 +138,8 @@
+ controller_costmap_ros_->stop();
+ }
+
+- //load any user specified recovery behaviors, and if that fails load the defaults
+- if(!loadRecoveryBehaviors(private_nh)){
+- loadDefaultRecoveryBehaviors();
+- }
++ //load the default recovery behaviours
++ loadDefaultRecoveryBehaviors();
+
+ //initially, we'll need to make a plan
+ state_ = PLANNING;
+@@ -245,73 +200,38 @@
+ boost::shared_ptr old_planner = planner_;
+ //initialize the global planner
+ ROS_INFO("Loading global planner %s", config.base_global_planner.c_str());
+- try {
+- //check if a non fully qualified name has potentially been passed in
+- if(!bgp_loader_.isClassAvailable(config.base_global_planner)){
+- std::vector classes = bgp_loader_.getDeclaredClasses();
+- for(unsigned int i = 0; i < classes.size(); ++i){
+- if(config.base_global_planner == bgp_loader_.getName(classes[i])){
+- //if we've found a match... we'll get the fully qualified name and break out of the loop
+- ROS_WARN("Planner specifications should now include the package name. You are using a deprecated API. Please switch from %s to %s in your yaml file.",
+- config.base_global_planner.c_str(), classes[i].c_str());
+- config.base_global_planner = classes[i];
+- break;
+- }
+- }
+- }
+
+- planner_ = bgp_loader_.createInstance(config.base_global_planner);
++ // wait for the current planner to finish planning
++ boost::unique_lock lock(planner_mutex_);
+
+- // wait for the current planner to finish planning
+- boost::unique_lock lock(planner_mutex_);
++ planner_.reset(new navfn::NavfnROS());
+
+- // Clean up before initializing the new planner
+- planner_plan_->clear();
+- latest_plan_->clear();
+- controller_plan_->clear();
+- resetState();
+- planner_->initialize(bgp_loader_.getName(config.base_global_planner), planner_costmap_ros_);
++ // Clean up before initializing the new planner
++ planner_plan_->clear();
++ latest_plan_->clear();
++ controller_plan_->clear();
++ resetState();
+
+- lock.unlock();
+- } catch (const pluginlib::PluginlibException& ex)
+- {
+- ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", config.base_global_planner.c_str(), ex.what());
+- planner_ = old_planner;
+- config.base_global_planner = last_config_.base_global_planner;
+- }
++ lock.unlock();
+ }
+
+ if(config.base_local_planner != last_config_.base_local_planner){
+ boost::shared_ptr old_planner = tc_;
+ //create a local planner
+- try {
+- //check if a non fully qualified name has potentially been passed in
+- ROS_INFO("Loading local planner: %s", config.base_local_planner.c_str());
+- if(!blp_loader_.isClassAvailable(config.base_local_planner)){
+- std::vector classes = blp_loader_.getDeclaredClasses();
+- for(unsigned int i = 0; i < classes.size(); ++i){
+- if(config.base_local_planner == blp_loader_.getName(classes[i])){
+- //if we've found a match... we'll get the fully qualified name and break out of the loop
+- ROS_WARN("Planner specifications should now include the package name. You are using a deprecated API. Please switch from %s to %s in your yaml file.",
+- config.base_local_planner.c_str(), classes[i].c_str());
+- config.base_local_planner = classes[i];
+- break;
+- }
+- }
+- }
+- tc_ = blp_loader_.createInstance(config.base_local_planner);
+- // Clean up before initializing the new planner
+- planner_plan_->clear();
+- latest_plan_->clear();
+- controller_plan_->clear();
+- resetState();
+- tc_->initialize(blp_loader_.getName(config.base_local_planner), &tf_, controller_costmap_ros_);
+- } catch (const pluginlib::PluginlibException& ex)
+- {
+- ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", config.base_local_planner.c_str(), ex.what());
+- tc_ = old_planner;
+- config.base_local_planner = last_config_.base_local_planner;
+- }
++ // Clean up before initializing the new planner
++ planner_plan_->clear();
++ latest_plan_->clear();
++ controller_plan_->clear();
++ resetState();
++
++ ROS_INFO("Loading local planner: %s", config.base_local_planner.c_str());
++ tc_.reset(new dwa_local_planner::DWAPlannerROS());
++ // Clean up before initializing the new planner
++ planner_plan_->clear();
++ latest_plan_->clear();
++ controller_plan_->clear();
++ resetState();
++ tc_->initialize(std::string("dwa_local_planner"), &tf_, controller_costmap_ros_);
+ }
+
+ last_config_ = config;
+@@ -1030,122 +950,27 @@
+ }
+
+ bool MoveBase::loadRecoveryBehaviors(ros::NodeHandle node){
+- XmlRpc::XmlRpcValue behavior_list;
+- if(node.getParam("recovery_behaviors", behavior_list)){
+- if(behavior_list.getType() == XmlRpc::XmlRpcValue::TypeArray){
+- for(int i = 0; i < behavior_list.size(); ++i){
+- if(behavior_list[i].getType() == XmlRpc::XmlRpcValue::TypeStruct){
+- if(behavior_list[i].hasMember("name") && behavior_list[i].hasMember("type")){
+- //check for recovery behaviors with the same name
+- for(int j = i + 1; j < behavior_list.size(); j++){
+- if(behavior_list[j].getType() == XmlRpc::XmlRpcValue::TypeStruct){
+- if(behavior_list[j].hasMember("name") && behavior_list[j].hasMember("type")){
+- std::string name_i = behavior_list[i]["name"];
+- std::string name_j = behavior_list[j]["name"];
+- if(name_i == name_j){
+- ROS_ERROR("A recovery behavior with the name %s already exists, this is not allowed. Using the default recovery behaviors instead.",
+- name_i.c_str());
+- return false;
+- }
+- }
+- }
+- }
+- }
+- else{
+- ROS_ERROR("Recovery behaviors must have a name and a type and this does not. Using the default recovery behaviors instead.");
+- return false;
+- }
+- }
+- else{
+- ROS_ERROR("Recovery behaviors must be specified as maps, but they are XmlRpcType %d. We'll use the default recovery behaviors instead.",
+- behavior_list[i].getType());
+- return false;
+- }
+- }
+-
+- //if we've made it to this point, we know that the list is legal so we'll create all the recovery behaviors
+- for(int i = 0; i < behavior_list.size(); ++i){
+- try{
+- //check if a non fully qualified name has potentially been passed in
+- if(!recovery_loader_.isClassAvailable(behavior_list[i]["type"])){
+- std::vector classes = recovery_loader_.getDeclaredClasses();
+- for(unsigned int i = 0; i < classes.size(); ++i){
+- if(behavior_list[i]["type"] == recovery_loader_.getName(classes[i])){
+- //if we've found a match... we'll get the fully qualified name and break out of the loop
+- ROS_WARN("Recovery behavior specifications should now include the package name. You are using a deprecated API. Please switch from %s to %s in your yaml file.",
+- std::string(behavior_list[i]["type"]).c_str(), classes[i].c_str());
+- behavior_list[i]["type"] = classes[i];
+- break;
+- }
+- }
+- }
+-
+- boost::shared_ptr behavior(recovery_loader_.createInstance(behavior_list[i]["type"]));
+-
+- //shouldn't be possible, but it won't hurt to check
+- if(behavior.get() == NULL){
+- ROS_ERROR("The ClassLoader returned a null pointer without throwing an exception. This should not happen");
+- return false;
+- }
+-
+- //initialize the recovery behavior with its name
+- behavior->initialize(behavior_list[i]["name"], &tf_, planner_costmap_ros_, controller_costmap_ros_);
+- recovery_behaviors_.push_back(behavior);
+- }
+- catch(pluginlib::PluginlibException& ex){
+- ROS_ERROR("Failed to load a plugin. Using default recovery behaviors. Error: %s", ex.what());
+- return false;
+- }
+- }
+- }
+- else{
+- ROS_ERROR("The recovery behavior specification must be a list, but is of XmlRpcType %d. We'll use the default recovery behaviors instead.",
+- behavior_list.getType());
+- return false;
+- }
+- }
+- else{
+- //if no recovery_behaviors are specified, we'll just load the defaults
+- return false;
+- }
+-
+- //if we've made it here... we've constructed a recovery behavior list successfully
+- return true;
++ return false;
+ }
+
+ //we'll load our default recovery behaviors here
+ void MoveBase::loadDefaultRecoveryBehaviors(){
+ recovery_behaviors_.clear();
+- try{
+- //we need to set some parameters based on what's been passed in to us to maintain backwards compatibility
+- ros::NodeHandle n("~");
+- n.setParam("conservative_reset/reset_distance", conservative_reset_dist_);
+- n.setParam("aggressive_reset/reset_distance", circumscribed_radius_ * 4);
+-
+- //first, we'll load a recovery behavior to clear the costmap
+- boost::shared_ptr cons_clear(recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));
+- cons_clear->initialize("conservative_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
+- recovery_behaviors_.push_back(cons_clear);
+-
+- //next, we'll load a recovery behavior to rotate in place
+- boost::shared_ptr rotate(recovery_loader_.createInstance("rotate_recovery/RotateRecovery"));
+- if(clearing_rotation_allowed_){
+- rotate->initialize("rotate_recovery", &tf_, planner_costmap_ros_, controller_costmap_ros_);
+- recovery_behaviors_.push_back(rotate);
+- }
+
+- //next, we'll load a recovery behavior that will do an aggressive reset of the costmap
+- boost::shared_ptr ags_clear(recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));
+- ags_clear->initialize("aggressive_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
+- recovery_behaviors_.push_back(ags_clear);
+-
+- //we'll rotate in-place one more time
+- if(clearing_rotation_allowed_)
+- recovery_behaviors_.push_back(rotate);
+- }
+- catch(pluginlib::PluginlibException& ex){
+- ROS_FATAL("Failed to load a plugin. This should not happen on default recovery behaviors. Error: %s", ex.what());
+- }
++ //we need to set some parameters based on what's been passed in to us to maintain backwards compatibility
++ ros::NodeHandle n("~");
++ n.setParam("conservative_reset/reset_distance", conservative_reset_dist_);
++ n.setParam("aggressive_reset/reset_distance", circumscribed_radius_ * 4);
++
++ //first, we'll load a recovery behavior to clear the costmap
++ boost::shared_ptr cons_clear(new clear_costmap_recovery::ClearCostmapRecovery());
++ cons_clear->initialize(std::string("conservative_reset"), &tf_, planner_costmap_ros_, controller_costmap_ros_);
++ recovery_behaviors_.push_back(cons_clear);
++
++ //next, we'll load a recovery behavior that will do an aggressive reset of the costmap
++ boost::shared_ptr ags_clear(new clear_costmap_recovery::ClearCostmapRecovery());
++ ags_clear->initialize(std::string("aggressive_reset"), &tf_, planner_costmap_ros_, controller_costmap_ros_);
++ recovery_behaviors_.push_back(ags_clear);
+
+ return;
+ }
diff --git a/sample_app.sh b/sample_app.sh
index 6c51f5e..3a946c7 100755
--- a/sample_app.sh
+++ b/sample_app.sh
@@ -35,20 +35,20 @@ sedcmd="sed -e "$(eval echo $sedcmd)
subst() {
target=$(echo $1 | sed 's,.in$,,')
- $sedcmd $my_loc/files/sample_app/$1 > $2/$target
+ $sedcmd $my_loc/files/$3/$1 > $2/$target
}
-files=$(cd $my_loc/files/sample_app && find . -name \*.in)
+files=$(cd $my_loc/files/$lib_name && find . -name \*.in)
for f in $files; do
- subst $f $prefix
+ subst $f $prefix $lib_name
done
-rest=$(cd $my_loc/files/sample_app && find . -type f | grep -v '.in$')
+rest=$(cd $my_loc/files/$lib_name && find . -type f | grep -v '.in$')
for f in $rest; do
- cp $my_loc/files/sample_app/$f $prefix/$f
+ cp $my_loc/files/$lib_name/$f $prefix/$f
done
# Copy app icon
mkdir -p $prefix/res/drawable
-cp $my_loc/files/sample_app/ic_launcher.png $prefix/res/drawable/ic_launcher.png
+cp $my_loc/files/$lib_name/ic_launcher.png $prefix/res/drawable/ic_launcher.png