Merge pull request #21 from garyservin/master

Add move_base simple example

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'

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)

files/move_base_app/AndroidManifest.xml.in

@@ -0,0 +1,22 @@
+<?xml version="1.0" encoding="utf-8"?>
+<manifest xmlns:android="http://schemas.android.com/apk/res/android"
+    package="com.ros.move_base_app"
+    android:versionCode="1"
+    android:versionName="1.0">
+
+    <uses-sdk android:minSdkVersion="14" />
+    <uses-permission
+        android:name="android.permission.WRITE_EXTERNAL_STORAGE" />
+    <uses-permission android:name="android.permission.INTERNET" />
+    <application android:label="@string/app_name" android:icon="@drawable/ic_launcher"
+        android:hasCode="false">
+        <activity android:name="android.app.NativeActivity"
+            android:label="@string/app_name">
+            <meta-data android:name="android.app.lib_name" android:value="move_base_app" />
+            <intent-filter>
+                <action android:name="android.intent.action.MAIN" />
+                <category android:name="android.intent.category.LAUNCHER" />
+            </intent-filter>
+        </activity>
+    </application>
+</manifest>

files/move_base_app/custom_rules.xml.in

@@ -0,0 +1,7 @@
+<project name="custom_rules" default="debug">
+    <target name="-pre-build">
+        <exec executable=":{ndk_path}" failonerror="true">
+            <env key="NDK_MODULE_PATH" path=":{module_path}" />
+        </exec>
+    </target>
+</project>

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)

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

files/move_base_app/jni/src/test_move_base.cpp

@@ -0,0 +1,48 @@
+#include "ros/ros.h"
+#include <move_base/move_base.h>
+
+#include <android_native_app_glue.h>
+#include <android/log.h>
+
+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();
+    }
+}

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 <costmap_2d/footprint.h>
+ #include <geometry_msgs/Polygon.h>
+ #include <dynamic_reconfigure/server.h>
+-#include <pluginlib/class_loader.h>
++#include <costmap_2d/footprint_layer.h>
++#include <costmap_2d/inflation_layer.h>
++#include <costmap_2d/obstacle_layer.h>
++#include <costmap_2d/static_layer.h>
+
+ class SuperValue : public XmlRpc::XmlRpcValue
+ {
+@@ -280,7 +283,6 @@
+   ros::Timer timer_;
+   ros::Time last_publish_;
+   ros::Duration publish_cycle;
+-  pluginlib::ClassLoader<Layer> plugin_loader_;
+   tf::Stamped<tf::Pose> old_pose_;
+   Costmap2DPublisher* publisher_;
+   dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig> *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<std::string>(my_list[i]["name"]);
+-      std::string type = static_cast<std::string>(my_list[i]["type"]);
+-      ROS_INFO("Using plugin \"%s\"", pname.c_str());
+-
+-      boost::shared_ptr<Layer> 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<Layer> 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<Layer> 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<Layer> 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<Layer> obstacle (new costmap_2d::ObstacleLayer());
++    layered_costmap_->addPlugin(obstacle);
++    obstacle->initialize(layered_costmap_, name + "/" + std::string("obstacle_layer"), &tf_);
+   }
+
+   // subscribe to the footprint topic