update

Controllers/Packages/amr_control/CMakeLists.txt

@@ -11,7 +11,6 @@ find_package(catkin REQUIRED COMPONENTS
   geometry_msgs
   nav_2d_utils
   roscpp
-  rospy
   std_msgs
   angles
   delta_modbus
@@ -19,89 +18,18 @@ find_package(catkin REQUIRED COMPONENTS
   loc_core
   amr_comunication
   vda5050_msgs
+  nav_msgs
+
+  robot_nav_2d_utils
+  move_base_core
+  robot_cpp
+  robot_nav_msgs
 )
 
+find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
 ## System dependencies are found with CMake's conventions
 find_package(Boost REQUIRED COMPONENTS system thread)
 find_package(PkgConfig)
-find_package(robot_cpp REQUIRED)
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   geometry_msgs#   std_msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
 
 ###################################
 ## catkin specific configuration ##
@@ -114,8 +42,16 @@ find_package(robot_cpp REQUIRED)
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
   INCLUDE_DIRS include
-  LIBRARIES amr_control 
-  CATKIN_DEPENDS geometry_msgs move_base_core loc_core nav_2d_utils roscpp rospy std_msgs
+  LIBRARIES amr_control amr_control_converter amr_control_publisher amr_control_subscriber
+  CATKIN_DEPENDS 
+    geometry_msgs  
+    loc_core 
+    nav_2d_utils 
+    robot_nav_2d_utils
+    move_base_core 
+    robot_cpp
+    roscpp 
+    std_msgs 
   DEPENDS Boost 
 )
 
@@ -129,25 +65,39 @@ include_directories(
   include
   ${catkin_INCLUDE_DIRS}
   ${Boost_INCLUDE_DIRS}
+  ${TF3_INCLUDE_DIR}
 )
 
 ## Declare a C++ library
+add_library(${PROJECT_NAME}_converter src/tf_converter.cpp src/sensor_converter.cpp)
+add_dependencies(${PROJECT_NAME}_converter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME}_converter ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${TF3_LIBRARY})
+
+add_library(${PROJECT_NAME}_publisher src/amr_publiser.cpp)
+add_dependencies(${PROJECT_NAME}_publisher ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME}_publisher ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
+add_library(${PROJECT_NAME}_subscriber src/amr_subcriber.cpp)
+add_dependencies(${PROJECT_NAME}_subscriber ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME}_subscriber ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
 add_library(${PROJECT_NAME}
   src/amr_control.cpp
   src/amr_monitor.cpp
   src/amr_safety.cpp
   src/amr_opc_ua_server_api.cpp
-  # src/amr_vda_5050_client_api.cpp
-  # src/amr_make_plan_with_order.cpp
+  src/amr_vda_5050_client_api.cpp
+  src/amr_make_plan_with_order.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})
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_converter_EXPORTED_TARGETS} ${${PROJECT_NAME}_publisher_EXPORTED_TARGETS} ${${PROJECT_NAME}_subscriber_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
 # Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings
 # Libraries will be found via LD_LIBRARY_PATH or system paths
-set_target_properties(${PROJECT_NAME} PROPERTIES
+set_target_properties(${PROJECT_NAME} ${PROJECT_NAME}_converter ${PROJECT_NAME}_publisher ${PROJECT_NAME}_subscriber PROPERTIES
     SKIP_BUILD_RPATH TRUE
     BUILD_WITH_INSTALL_RPATH FALSE
     INSTALL_RPATH_USE_LINK_PATH FALSE
@@ -156,62 +106,47 @@ set_target_properties(${PROJECT_NAME} PROPERTIES
 ## 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(${PROJECT_NAME}_node src/amr_control_node.cpp)
+add_executable(${PROJECT_NAME}_node src/amr_control_node.cpp)
 # add_executable(vda_5050_api_test test/vda_5050_api.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 ${PROJECT_NAME})
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME ${PROJECT_NAME})
 
 ## Add cmake target dependencies of the executable
 ## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node 
-#   ${PROJECT_NAME}
-#   ${${PROJECT_NAME}_EXPORTED_TARGETS} 
-#   ${catkin_EXPORTED_TARGETS}
-# )
-# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings
-# Libraries will be found via LD_LIBRARY_PATH or system paths
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES
-#     SKIP_BUILD_RPATH TRUE
-#     BUILD_WITH_INSTALL_RPATH FALSE
-#     INSTALL_RPATH_USE_LINK_PATH FALSE
-# )
 
-# add_dependencies(vda_5050_api_test
-#   ${PROJECT_NAME}
-#   ${${PROJECT_NAME}_EXPORTED_TARGETS} 
-#   ${catkin_EXPORTED_TARGETS}
-# )
-# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings
-# Libraries will be found via LD_LIBRARY_PATH or system paths
-# set_target_properties(vda_5050_api_test PROPERTIES
-#     SKIP_BUILD_RPATH TRUE
-#     BUILD_WITH_INSTALL_RPATH FALSE
-#     INSTALL_RPATH_USE_LINK_PATH FALSE
-# )
+add_dependencies(${PROJECT_NAME}_node 
+  ${PROJECT_NAME}
+  ${${PROJECT_NAME}_EXPORTED_TARGETS} 
+  ${catkin_EXPORTED_TARGETS}
+)
+# Configure RPATH to find libraries in devel space
+# Use ORIGIN to find libraries relative to executable location
+set_target_properties(${PROJECT_NAME}_node PROPERTIES
+    BUILD_WITH_INSTALL_RPATH FALSE
+    INSTALL_RPATH_USE_LINK_PATH TRUE
+    BUILD_RPATH "${CATKIN_DEVEL_PREFIX}/lib"
+)
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(${PROJECT_NAME}
+  ${PROJECT_NAME}_converter
+  ${PROJECT_NAME}_publisher
+  ${PROJECT_NAME}_subscriber
   ${FreeOpcUa_LIBRARIES}
   ${catkin_LIBRARIES}
   ${Boost_LIBRARIES}
-  robot_cpp
+  ${TF3_LIBRARY}
 )
 
-# target_link_libraries(${PROJECT_NAME}_node
-#   ${PROJECT_NAME}
-#   ${catkin_LIBRARIES}
-#   ${Boost_LIBRARIES}
-# )
-
-# target_link_libraries(vda_5050_api_test
-#   ${PROJECT_NAME}
-#   ${catkin_LIBRARIES}
-#   ${Boost_LIBRARIES}
-# )
+target_link_libraries(${PROJECT_NAME}_node
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+)
 
 
 #############
@@ -230,13 +165,13 @@ target_link_libraries(${PROJECT_NAME}
 
 ## Mark executables for installation
 ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node vda_5050_api_test
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
+install(TARGETS ${PROJECT_NAME}_node 
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
 
 ## Mark libraries for installation
 ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-install(TARGETS ${PROJECT_NAME} 
+install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_converter ${PROJECT_NAME}_publisher ${PROJECT_NAME}_subscriber
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}

Controllers/Packages/amr_control/include/amr_control/amr_control.h

@@ -10,6 +10,8 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <tf2_ros/transform_listener.h>
 #include <std_msgs/Bool.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <sensor_msgs/LaserScan.h>
 
 #include "delta_modbus/delta_modbus_tcp.h"
 
@@ -24,10 +26,11 @@
 #include <amr_control/amr_monitor.h>
 #include <nova5_control/imr_nova_control.h>
 #include <amr_control/amr_safety.h>
+#include <amr_control/tf_converter.h>
+#include <amr_control/sensor_converter.h>
+#include <amr_control/amr_publiser.h>
+#include <amr_control/amr_subcriber.h>
 
-// #include <robot/node_handle.h>
-// Plugin Loader
-// #include <robot/plugin_loader_helper.h>
 #include <boost/dll/import.hpp>
 
 namespace amr_control
@@ -35,10 +38,28 @@ namespace amr_control
   class AmrController
   {
   public:
+    /**
+     * @brief Constructor with NodeHandle and TF buffer
+     * @param nh ROS NodeHandle for communication
+     * @param buffer Shared pointer to tf2_ros::Buffer for coordinate transforms
+     */
     AmrController(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer);
+    
+    /**
+     * @brief Default constructor
+     */
     AmrController();
+    
+    /**
+     * @brief Destructor - cleans up threads and resources
+     */
     virtual ~AmrController();
 
+    /**
+     * @brief Initialize the AMR controller with NodeHandle and TF buffer
+     * @param nh ROS NodeHandle for communication
+     * @param buffer Shared pointer to tf2_ros::Buffer for coordinate transforms
+     */
     void init(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer);
 
     /**
@@ -47,37 +68,90 @@ namespace amr_control
     virtual void stop(void);
 
   protected:
+    /**
+     * @brief Initialize communication handles (OPC UA server and VDA5050 client)
+     * @param nh ROS NodeHandle for communication setup
+     */
     virtual void initalizingComunicationHandle(ros::NodeHandle &nh);
+    
+    /**
+     * @brief Initialize move_base navigation system and subscribe to sensor topics
+     * @param nh ROS NodeHandle for move_base configuration
+     */
     virtual void initalizingMoveBaseHandle(ros::NodeHandle &nh);
+    
+    /**
+     * @brief Initialize localization system (loc_base)
+     * @param nh ROS NodeHandle for localization configuration
+     */
     virtual void initalizingLocalizationHandle(ros::NodeHandle &nh);
+    
+    /**
+     * @brief Initialize monitoring system (AmrMonitor)
+     * @param nh ROS NodeHandle for monitor configuration
+     */
     virtual void initalizingMonitorHandle(ros::NodeHandle &nh);
 
   private:  
+
+    /**
+     * @brief Callback for arm control - starts arm thread for robot arm operations
+     */
     void ArmCallBack();
 
-    void ArmDotuff();
-
-    void unLoadCallBack();
-
-    void conveyorBeltsShipping(amr_control::State &state);
-
+    /**
+     * @brief Callback for load operation - starts receiving conveyor belt thread
+     */
     void loadCallBack();
 
-    void conveyorBeltsReceiving(amr_control::State &state);
-
-    void controllerDotuff();
+    /**
+     * @brief Callback for unload operation - starts shipping conveyor belt thread
+     */
+    void unLoadCallBack();
 
+    /**
+     * @brief Callback for marker detection status
+     * @param msg Boolean message indicating if marker is detected
+     */
     void isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg);
 
+    /**
+     * @brief Control shipping conveyor belt via PLC communication
+     * @param state Reference to state variable that tracks belt operation status
+     */
+    void conveyorBeltsShipping(amr_control::State &state);
+
+    /**
+     * @brief Control receiving conveyor belt via PLC communication
+     * @param state Reference to state variable that tracks belt operation status
+     */
+    void conveyorBeltsReceiving(amr_control::State &state);
+
+    /**
+     * @brief Main controller loop - handles PLC communication, safety checks, and velocity control
+     */
+    void controllerDotuff();
+
+    /**
+     * @brief Arm control thread function - manages robot arm operations (home, mode control)
+     */
+    void ArmDotuff();
+
+    /**
+     * @brief Main thread handler - coordinates initialization and main control loop
+     */
     void threadHandle();
 
     bool initalized_;
     ros::NodeHandle nh_;
     std::shared_ptr<tf2_ros::Buffer> tf_;
-    // robot::TFListenerPtr tf_core_ptr_;
+    robot::TFListenerPtr tf_core_ptr_;
+    std::shared_ptr<amr_control::TfConverter> tf_converter_ptr_;
+    std::shared_ptr<amr_control::SensorConverter> sensor_converter_ptr_;
+    std::shared_ptr<amr_control::AmrPublisher> amr_publisher_ptr_;
+    std::shared_ptr<amr_control::AmrSubscriber> amr_subscriber_ptr_;
 
     ros::Subscriber is_detected_maker_sub_;
-
     std::shared_ptr<amr_control::OpcUAServerAPI> opc_ua_server_api_ptr_;
     std::shared_ptr<amr_control::VDA5050ClientAPI> vda_5050_client_api_ptr_;
     std::shared_ptr<std::thread> server_thr_;

Controllers/Packages/amr_control/include/amr_control/amr_publiser.h

@@ -0,0 +1,245 @@
+#ifndef __AMR_PUBLISHER_H_INCLUDED_
+#define __AMR_PUBLISHER_H_INCLUDED_
+
+#include <ros/ros.h>
+#include <robot/robot.h>
+
+#include <geometry_msgs/PolygonStamped.h>
+#include <geometry_msgs/Twist.h>
+#include <robot_nav_2d_msgs/Twist2DStamped.h>
+#include <robot_nav_2d_msgs/Path2D.h>
+#include <robot_nav_2d_utils/conversions.h>
+#include <nav_2d_utils/conversions.h>
+
+#include <nav_2d_msgs/Path2D.h>
+#include <nav_msgs/Path.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <robot_nav_msgs/OccupancyGrid.h>
+#include <robot_map_msgs/OccupancyGridUpdate.h>
+#include <map_msgs/OccupancyGridUpdate.h>
+#include <move_base_core/navigation.h>
+#include <memory>
+#include <string>
+
+namespace amr_control
+{
+  struct PlannerObject
+  {
+    // ROS publishers
+    ros::Publisher pub_;
+    ros::Publisher update_pub_;
+    ros::Publisher footprint_pub_;
+    ros::Publisher plan_pub_;
+    ros::Timer timer_;
+    std::string topic_;
+    bool active_;
+  };
+
+  /**
+   * @class AmrPublisher
+   * @brief Publisher class for publishing costmap data from move_base_core to RViz
+   * 
+   * This class converts robot_nav_msgs::OccupancyGrid to nav_msgs::OccupancyGrid
+   * and publishes global and local costmaps to ROS topics that RViz can visualize.
+   */
+  class AmrPublisher
+  {
+  public:
+    /**
+     * @brief Constructor
+     * @param nh ROS NodeHandle for creating publishers
+     * @param move_base_ptr Shared pointer to BaseNavigation instance
+     */
+    AmrPublisher(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr);
+    
+    /**
+     * @brief Destructor
+     */
+    virtual ~AmrPublisher();
+
+    /**
+     * @brief Publish global plan to RViz
+     */
+    void publishGlobalPlan();
+
+    /**
+     * @brief Publish local plan to RViz
+     */
+    void publishLocalPlan();
+
+    /**
+     * @brief Publish global costmap to RViz
+     * @param publish_rate Rate at which to publish (Hz). If 0, publish immediately once.
+     */
+    void publishGlobalCostmap(double publish_rate = 0.0);
+
+    /**
+     * @brief Publish local costmap to RViz
+     * @param publish_rate Rate at which to publish (Hz). If 0, publish immediately once.
+     */
+    void publishLocalCostmap(double publish_rate = 0.0);
+
+    /**
+     * @brief Publish both global and local costmaps
+     * @param publish_rate Rate at which to publish (Hz). If 0, publish immediately once.
+     */
+    void publishCostmaps(double publish_rate = 0.0);
+
+    /**
+     * @brief Publish global footprint to RViz
+     */
+    void publishGlobalFootprint();
+
+    /**
+     * @brief Publish local footprint to RViz
+     */
+    void publishLocalFootprint();
+
+    /**
+     * @brief Publish both global and local footprints
+     */
+    void publishFootprints();
+
+    /**
+     * @brief Start publishing costmaps at a fixed rate
+     * @param global_rate Publishing rate for global costmap (Hz). If 0, don't publish global.
+     * @param local_rate Publishing rate for local costmap (Hz). If 0, don't publish local.
+     */
+    void startPublishing(double global_rate = 1.0, double local_rate = 5.0);
+
+    /**
+     * @brief Stop publishing costmaps
+     */
+    void stopPublishing();
+
+    /**
+     * @brief Check if publishing is active
+     * @return True if publishing is active (either global or local)
+     */
+    bool isPublishing() const { return global_planner_obj_.active_ || local_planner_obj_.active_ || cmd_vel_publishing_active_; }
+
+    /**
+     * @brief Start publishing cmd_vel at a fixed rate using ros::WallTimer
+     * @param rate Publishing rate in Hz (default: 20.0 Hz)
+     */
+     void startCmdVelPublishing(double rate = 20.0);
+
+     /**
+      * @brief Stop publishing cmd_vel
+      */
+     void stopCmdVelPublishing();
+
+    /**
+     * @brief Start publishing plans at a fixed rate
+     * @param global_rate Publishing rate for global plan (Hz). If 0, don't publish global.
+     * @param local_rate Publishing rate for local plan (Hz). If 0, don't publish local.
+     */
+    void startPlanPublishing(double global_rate = 10.0, double local_rate = 10.0);
+
+    /**
+     * @brief Stop publishing plans
+     */
+    void stopPlanPublishing();
+
+  private:
+
+    /**
+     * @brief Convert robot_nav_2d_msgs::Path2D to nav_msgs::Path
+     * @param robot_path Input robot_nav_2d_msgs::Path2D
+     * @param nav_path Output nav_msgs::Path
+     */
+    void convertToNavPath(const robot_nav_2d_msgs::Path2D &robot_path, 
+                          nav_msgs::Path &nav_path);
+
+    /**
+     * @brief Callback for new subscription to global costmap
+     * @param pub SingleSubscriberPublisher for the new subscription
+     */
+    void onNewSubscriptionGlobal(const ros::SingleSubscriberPublisher& pub);
+
+    /**
+     * @brief Callback for new subscription to local costmap
+     * @param pub SingleSubscriberPublisher for the new subscription
+     */
+    void onNewSubscriptionLocal(const ros::SingleSubscriberPublisher& pub);
+
+    /**
+     * @brief Convert robot_nav_msgs::OccupancyGrid to nav_msgs::OccupancyGrid
+     * @param robot_grid Input robot_nav_msgs::OccupancyGrid
+     * @param nav_grid Output nav_msgs::OccupancyGrid
+     */
+    void convertToNavOccupancyGrid(const robot_nav_msgs::OccupancyGrid &robot_grid, 
+                                    nav_msgs::OccupancyGrid &nav_grid);
+
+    void convertToRobotOccupancyGridUpdate(const robot_map_msgs::OccupancyGridUpdate &robot_grid_update, 
+                                           map_msgs::OccupancyGridUpdate &grid_update);
+
+    /**
+     * @brief Convert robot_geometry_msgs::PolygonStamped to geometry_msgs::PolygonStamped
+     * @param robot_footprint Input robot_geometry_msgs::PolygonStamped
+     * @param nav_footprint Output geometry_msgs::PolygonStamped
+     */
+    void convertToNavPolygonStamped(const robot_geometry_msgs::PolygonStamped &robot_footprint, 
+                                     geometry_msgs::PolygonStamped &nav_footprint);
+
+    /**
+     * @brief Timer callback for publishing global costmap
+     * @param event Timer event
+     */
+    void globalCostmapTimerCallback(const ros::TimerEvent &event);
+
+    /**
+     * @brief Timer callback for publishing local costmap
+     * @param event Timer event
+     */
+    void localCostmapTimerCallback(const ros::TimerEvent &event);
+
+    /**
+     * @brief Publish cmd_vel command
+     */
+    void publishCmdVel();
+
+    /**
+     * @brief WallTimer callback for publishing cmd_vel
+     * @param event WallTimer event
+     */
+    void cmdVelTimerCallback(const ros::WallTimerEvent &event);
+
+    /**
+     * @brief Timer callback for updating global plan
+     * @param event Timer event
+     */
+    void globalPlanTimerCallback(const ros::TimerEvent &event);
+
+    /**
+     * @brief Timer callback for updating local plan
+     * @param event Timer event
+     */
+    void localPlanTimerCallback(const ros::TimerEvent &event);
+
+    ros::NodeHandle nh_;
+    robot::move_base_core::BaseNavigation::Ptr move_base_ptr_;
+
+    PlannerObject global_planner_obj_;
+    PlannerObject local_planner_obj_;
+    ros::Publisher cmd_vel_pub_;
+    
+    // ros::WallTimer for cmd_vel publishing
+    ros::WallTimer cmd_vel_timer_;
+    bool cmd_vel_publishing_active_;
+
+    // Timers for plan publishing
+    ros::Timer global_plan_timer_;
+    ros::Timer local_plan_timer_;
+    bool global_plan_publishing_active_;
+    bool local_plan_publishing_active_;
+
+    // Cached plans with timestamps
+    robot_nav_2d_msgs::Path2D cached_global_plan_;
+    robot_nav_2d_msgs::Path2D cached_local_plan_;
+  };
+
+} // namespace amr_control
+
+#endif // __AMR_PUBLISHER_H_INCLUDED_
+

Controllers/Packages/amr_control/include/amr_control/amr_subcriber.h

@@ -0,0 +1,31 @@
+#ifndef __AMR_SUBCRIBER_H_INCLUDED_
+#define __AMR_SUBCRIBER_H_INCLUDED_
+
+#include <ros/ros.h>
+#include <robot/robot.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <nav_msgs/Odometry.h>
+#include <move_base_core/navigation.h>
+#include <memory>
+#include <string>
+
+namespace amr_control
+{
+  class AmrSubscriber
+  {
+  public:
+    AmrSubscriber(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr);
+    virtual ~AmrSubscriber();
+
+  private:
+    void moveBaseSimpleCallback(const geometry_msgs::PoseStamped::ConstPtr &msg);
+    void odometryCallback(const nav_msgs::Odometry::ConstPtr &msg);
+    
+    ros::NodeHandle nh_;
+    ros::Subscriber move_base_simple_sub_;
+    ros::Subscriber odometry_sub_;
+    robot::move_base_core::BaseNavigation::Ptr move_base_ptr_;
+  };
+} // namespace amr_control
+
+#endif // __AMR_SUBCRIBER_H_INCLUDED_

Controllers/Packages/amr_control/include/amr_control/sensor_converter.h

@@ -0,0 +1,63 @@
+#ifndef __SENSOR_CONVERTER_H_INCLUDED_
+#define __SENSOR_CONVERTER_H_INCLUDED_
+#include <ros/ros.h>
+#include <robot/robot.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/GetMap.h>
+#include <sensor_msgs/LaserScan.h>
+#include <robot_nav_msgs/OccupancyGrid.h>
+#include <robot_sensor_msgs/LaserScan.h>
+#include <robot_sensor_msgs/PointCloud.h>
+#include <robot_sensor_msgs/PointCloud2.h>
+#include <robot_geometry_msgs/Point.h>
+#include <robot_geometry_msgs/PoseStamped.h>
+#include <move_base_core/navigation.h>
+#include <memory>
+
+namespace amr_control
+{
+  class SensorConverter
+  {
+  public:
+    SensorConverter(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr);
+    virtual ~SensorConverter();
+
+    /**
+     * @brief Request map from map server service or wait for message
+     * @param nh NodeHandle to use for service client and topic wait
+     */
+     void requestMapFromServer(ros::NodeHandle &nh);
+     
+  private:
+    /**
+     * @brief Callback for map server topic - receives static map and adds it to move_base
+     * @param msg OccupancyGrid message from map server
+     */
+    void mapServerCallback(const nav_msgs::OccupancyGrid::ConstPtr &msg);
+
+    /**
+     * @brief Callback for front laser scan topic - receives and processes front laser data
+     * @param msg LaserScan message from front laser sensor
+     */
+    void laserFScanCallback(const sensor_msgs::LaserScan::ConstPtr &msg);
+
+    /**
+     * @brief Callback for back laser scan topic - receives and processes back laser data
+     * @param msg LaserScan message from back laser sensor
+     */
+    void laserBScanCallback(const sensor_msgs::LaserScan::ConstPtr &msg);
+
+    /**
+     * @brief Convert nav_msgs::OccupancyGrid to robot_nav_msgs::OccupancyGrid and add to move_base
+     * @param nav_map The nav_msgs map to convert
+     */
+    void convertAndAddMap(const nav_msgs::OccupancyGrid &nav_map);
+
+    ros::NodeHandle nh_;
+    std::shared_ptr<robot::move_base_core::BaseNavigation> move_base_ptr_;
+    ros::Subscriber map_server_sub_;
+    ros::Subscriber laser_f_scan_sub_;
+    ros::Subscriber laser_b_scan_sub_;
+  };
+}
+#endif

Controllers/Packages/amr_control/include/amr_control/tf_converter.h

@@ -0,0 +1,45 @@
+#ifndef __TF_CONVERTER_H_INCLUDED_
+#define __TF_CONVERTER_H_INCLUDED_
+
+#include <ros/ros.h>
+#include <robot/robot.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf3/buffer_core.h>
+#include <tf3/exceptions.h>
+#include <move_base_core/common.h>
+
+namespace amr_control
+{
+  class TfConverter
+  {
+  public:
+    TfConverter(std::shared_ptr<tf2_ros::Buffer> tf);
+    ~TfConverter();
+
+    /**
+     * @brief Lấy buffer tf3 từ tf_core_ptr_
+     * @param tf_buffer Shared pointer to tf3::BufferCore
+     */
+    void TfBuffer(std::shared_ptr<tf3::BufferCore> &tf_buffer);
+
+    /**
+     * @brief Kiểm tra và in ra thông tin về tf_core_ptr_ để verify dữ liệu cập nhật
+     * @param check_frames Các frame pairs cần kiểm tra (ví dụ: {"map", "base_footprint"})
+     */
+    void checkTfCoreData(const std::vector<std::pair<std::string, std::string>> &check_frames, robot::TFListenerPtr &tf_core_ptr);
+
+  private:
+    /**
+     * @brief Thread worker for tf conversion
+     */
+    void tfWorker();
+
+    std::shared_ptr<tf2_ros::Buffer> tf_;
+    std::shared_ptr<tf3::BufferCore> tf3_buffer_;
+    std::thread tf_thread_;
+    std::atomic<bool> stop_tf_thread_{false};
+    std::mutex tf3_mutex_;
+  };
+}
+
+#endif

Controllers/Packages/amr_control/package.xml

@@ -49,10 +49,13 @@
   <!-- Use doc_depend for packages you need only for building documentation: -->
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+
   <build_depend>geometry_msgs</build_depend>
   <build_depend>nav_2d_utils</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>angles</build_depend>
   <build_depend>delta_modbus</build_depend>
@@ -60,11 +63,10 @@
   <build_depend>loc_core</build_depend>
   <build_depend>amr_comunication</build_depend>
   <build_depend>vda5050_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
 
   <build_export_depend>geometry_msgs</build_export_depend>
   <build_export_depend>nav_2d_utils</build_export_depend>
-  <build_export_depend>roscpp</build_export_depend>
-  <build_export_depend>rospy</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
   <build_export_depend>angles</build_export_depend>
   <build_export_depend>delta_modbus</build_export_depend>
@@ -72,12 +74,10 @@
   <build_export_depend>loc_core</build_export_depend>
   <build_export_depend>amr_comunication</build_export_depend>
   <build_export_depend>vda5050_msgs</build_export_depend>
+  <build_export_depend>nav_msgs</build_export_depend>
 
   <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>move_base_core</exec_depend>
   <exec_depend>nav_2d_utils</exec_depend>
-  <exec_depend>roscpp</exec_depend>
-  <exec_depend>rospy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>angles</exec_depend>
   <exec_depend>delta_modbus</exec_depend>
@@ -85,7 +85,21 @@
   <exec_depend>loc_core</exec_depend>
   <exec_depend>amr_comunication</exec_depend>
   <exec_depend>vda5050_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
   
+<!-- PNKX core  -->
+  <build_depend>robot_cpp</build_depend>
+  <build_depend>robot_nav_2d_utils</build_depend>
+  <build_depend>move_base_core</build_depend>
+  <build_depend>robot_nav_msgs</build_depend>
+  <build_export_depend>robot_cpp</build_export_depend>
+  <build_export_depend>robot_nav_2d_utils</build_export_depend>
+  <build_export_depend>move_base_core</build_export_depend>
+  <build_export_depend>robot_nav_msgs</build_export_depend>
+  <exec_depend>robot_cpp</exec_depend>
+  <exec_depend>robot_nav_2d_utils</exec_depend>
+  <exec_depend>move_base_core</exec_depend>
+  <exec_depend>robot_nav_msgs</exec_depend>
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->

Controllers/Packages/amr_control/src/amr_control.cpp

@@ -1,5 +1,10 @@
 #include "amr_control/amr_control.h"
+
+#include <robot/robot.h>
+#include <tf3/buffer_core.h>
 #include <geometry_msgs/Vector3.h>
+#include <xmlrpcpp/XmlRpcValue.h>
+#include <robot_geometry_msgs/Point.h>
 
 namespace amr_control
 {
@@ -13,6 +18,7 @@ namespace amr_control
         loc_base_ptr_(nullptr),
         move_base_ptr_(nullptr),
         amr_safety_ptr_(nullptr),
+        tf_core_ptr_(nullptr),
         loc_base_loader_("loc_core", "loc_core::BaseLocalization")
   {
     this->init(nh, buffer);
@@ -28,6 +34,8 @@ namespace amr_control
         loc_base_ptr_(nullptr),
         move_base_ptr_(nullptr),
         amr_safety_ptr_(nullptr),
+        tf_converter_ptr_(nullptr),
+        tf_core_ptr_(nullptr),
         loc_base_loader_("loc_core", "loc_core::BaseLocalization")
   {
   }
@@ -68,6 +76,15 @@ namespace amr_control
     {
       vda_5050_client_api_ptr_.reset();
     }
+
+    // Stop costmap, cmd_vel and plan publishing
+    if (amr_publisher_ptr_)
+    {
+      amr_publisher_ptr_->stopPublishing();
+      amr_publisher_ptr_->stopCmdVelPublishing();
+      amr_publisher_ptr_->stopPlanPublishing();
+      amr_publisher_ptr_.reset();
+    }
   }
 
   void AmrController::init(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer)
@@ -217,368 +234,455 @@ namespace amr_control
     std::string obj_name("move_base::MoveBase");
     if (tf_ == nullptr)
       throw std::runtime_error("tf2_ros::Buffer object is null");
+
+    tf_converter_ptr_ = std::make_shared<amr_control::TfConverter>(tf_);
+    tf_converter_ptr_->TfBuffer(tf_core_ptr_);
     try
     {
-      // robot::log_info("[%s:%d]\n Loading MoveBase library...", __FILE__, __LINE__);
-      // robot::PluginLoaderHelper loader;
-      // std::string path_file_so = loader.findLibraryPath("MoveBase");
-      // move_base_loader_ = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
-      //   path_file_so,
-      //   "MoveBase", boost::dll::load_mode::append_decorations);
+      robot::log_info("[%s:%d]\n Loading MoveBase library...", __FILE__, __LINE__);
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath("MoveBase");
+      move_base_loader_ = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
+        path_file_so, "MoveBase", boost::dll::load_mode::append_decorations);
       
-      // robot::log_info("[%s:%d]\n Creating MoveBase instance...", __FILE__, __LINE__);
-      // move_base_ptr_ = create_plugin();
-      // move_base_ptr_->initialize(tf_core_ptr_);
+      robot::log_info("[%s:%d]\n Creating MoveBase instance...", __FILE__, __LINE__);
+      move_base_ptr_ = move_base_loader_();
+      sensor_converter_ptr_ = std::make_shared<amr_control::SensorConverter>(nh, move_base_ptr_);
+      move_base_ptr_->initialize(tf_core_ptr_);
 
-      // ros::Rate r(3);
-      // do
-      // {
-      //   r.sleep();
-      //   ros::spinOnce();
-      // } while (ros::ok() && !move_base_ptr_->nav_feedback_->is_ready);
+      ros::Rate r(3);
+      do
+      {
+        r.sleep();
+        ros::spinOnce();
+      } while (ros::ok() && !move_base_ptr_->getFeedback()->is_ready);
 
-      // if (move_base_ptr_ != nullptr &&
-      //     move_base_ptr_->nav_feedback_ != nullptr &&
-      //     move_base_ptr_->nav_feedback_->is_ready)
-      // {
-      //   geometry_msgs::Vector3 linear;
-      //   linear.x = 0.3;
-      //   move_base_ptr_->setTwistLinear(linear);
-      //   linear.x = -0.3;
-      //   move_base_ptr_->setTwistLinear(linear);
-      // }
+      // Initialize costmap publisher for RViz visualization
+      robot::log_info("[%s:%d]\n Initializing costmap publisher...", __FILE__, __LINE__);
+      amr_publisher_ptr_ = std::make_shared<amr_control::AmrPublisher>(nh, move_base_ptr_);
+      amr_subscriber_ptr_ = std::make_shared<amr_control::AmrSubscriber>(nh, move_base_ptr_);
+
+      if (move_base_ptr_ != nullptr &&
+          move_base_ptr_->getFeedback() != nullptr &&
+          move_base_ptr_->getFeedback()->is_ready)
+      {
+        // Read footprint from parameter server and set it
+        std::vector<robot_geometry_msgs::Point> footprint;
+        if (nh.hasParam("footprint"))
+        {
+          XmlRpc::XmlRpcValue footprint_xmlrpc;
+          nh.getParam("footprint", footprint_xmlrpc);
+          
+          if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray && footprint_xmlrpc.size() >= 3)
+          {
+            footprint.reserve(footprint_xmlrpc.size());
+            for (int i = 0; i < footprint_xmlrpc.size(); ++i)
+            {
+              if (footprint_xmlrpc[i].getType() == XmlRpc::XmlRpcValue::TypeArray && footprint_xmlrpc[i].size() == 2)
+              {
+                robot_geometry_msgs::Point pt;
+                pt.x = static_cast<double>(footprint_xmlrpc[i][0]);
+                pt.y = static_cast<double>(footprint_xmlrpc[i][1]);
+                pt.z = 0.0;
+                footprint.push_back(pt);
+              }
+            }
+            
+            if (footprint.size() >= 3)
+            {
+              move_base_ptr_->setRobotFootprint(footprint);
+              robot::log_info("[%s:%d] Set robot footprint with %zu points", __FILE__, __LINE__, footprint.size());
+              for (size_t i = 0; i < footprint.size(); ++i)
+              {
+                robot::log_info("[%s:%d] Footprint point[%zu]: (%.3f, %.3f)", 
+                              __FILE__, __LINE__, i, footprint[i].x, footprint[i].y);
+              }
+            }
+            else
+            {
+              robot::log_warning("[%s:%d] Footprint must have at least 3 points, got %zu. Using default footprint.", 
+                                __FILE__, __LINE__, footprint.size());
+            }
+          }
+          else
+          {
+            robot::log_warning("[%s:%d] Invalid footprint parameter format. Expected array of arrays with at least 3 points.", 
+                              __FILE__, __LINE__);
+          }
+        }
+        else
+        {
+          robot::log_info("[%s:%d] No footprint parameter found, using default footprint from move_base config.", 
+                        __FILE__, __LINE__);
+        }
+
+
+        robot_geometry_msgs::Vector3 linear;
+        linear.x = 0.3;
+        move_base_ptr_->setTwistLinear(linear);
+        linear.x = -0.3;
+        move_base_ptr_->setTwistLinear(linear);
+
+        // Request map từ service hoặc đợi message (vì map server chỉ publish một lần)
+        robot::log_info("[%s:%d]\n Requesting map from server...", __FILE__, __LINE__);
+        sensor_converter_ptr_->requestMapFromServer(nh);
+        
+        // Start publishing costmaps to RViz
+        // Global costmap: 1 Hz, Local costmap: 5 Hz
+
+        if (amr_publisher_ptr_)
+        {
+          robot::log_info("[%s:%d]\n Starting costmap publishing to RViz...", __FILE__, __LINE__);
+          amr_publisher_ptr_->startPublishing(1.0, 5.0);
+          
+          // Start publishing cmd_vel using WallTimer
+          double cmd_vel_rate = 20.0;  // Default: 20 Hz
+          nh.param("cmd_vel_publish_rate", cmd_vel_rate, cmd_vel_rate);
+          robot::log_info("[%s:%d]\n Starting cmd_vel publishing at %.2f Hz...", __FILE__, __LINE__, cmd_vel_rate);
+          amr_publisher_ptr_->startCmdVelPublishing(cmd_vel_rate);
+          
+          // Start publishing plans using Timer
+          double global_plan_rate = 10.0;  // Default: 10 Hz
+          double local_plan_rate = 10.0;    // Default: 10 Hz
+          nh.param("global_plan_publish_rate", global_plan_rate, global_plan_rate);
+          nh.param("local_plan_publish_rate", local_plan_rate, local_plan_rate);
+          robot::log_info("[%s:%d]\n Starting plan publishing - Global: %.2f Hz, Local: %.2f Hz...", 
+                         __FILE__, __LINE__, global_plan_rate, local_plan_rate);
+          amr_publisher_ptr_->startPlanPublishing(global_plan_rate, local_plan_rate);
+        }
+      }
     }
     catch (const std::exception &e)
     {
-      // robot::log_error("[%s:%d]\n Exception: %s", __FILE__, __LINE__, e.what());
+      robot::log_error("[%s:%d]\n Exception: %s", __FILE__, __LINE__, e.what());
       exit(1);
     }
     catch (...)
     {
-      // robot::log_error("[%s:%d]\n Unknown exception occurred", __FILE__, __LINE__);
+      robot::log_error("[%s:%d]\n Unknown exception occurred", __FILE__, __LINE__);
       exit(1);
     }
   }
 
-  // void AmrController::initalizingMonitorHandle(ros::NodeHandle &nh)
-  // {
-  //   this->monitor_ptr_ = std::make_shared<AmrMonitor>(nh);
-  // }
+  void AmrController::initalizingMonitorHandle(ros::NodeHandle &nh)
+  {
+    this->monitor_ptr_ = std::make_shared<AmrMonitor>(nh);
+  }
 
-  // void AmrController::ArmCallBack()
-  // {
-  //   ROS_INFO("Arm Calling");
-  //   std::lock_guard<std::mutex> lock(this->arm_mutex_);
-  //   {
-  //     arm_joined_ = true;
-  //     this->arm_thread_ = std::thread(&AmrController::ArmDotuff, this);
-  //   }
-  // }
+  void AmrController::ArmCallBack()
+  {
+    ROS_INFO("Arm Calling");
+    std::lock_guard<std::mutex> lock(this->arm_mutex_);
+    {
+      arm_joined_ = true;
+      this->arm_thread_ = std::thread(&AmrController::ArmDotuff, this);
+    }
+  }
 
-  // void AmrController::ArmDotuff()
-  // {
-  //   std::shared_ptr<imr_nova_control> arm_control_ptr;
-  //   arm_control_ptr = std::make_shared<imr_nova_control>();
-  //   arm_control_ptr->enable_ = &this->enable_;
-  //   arm_control_ptr->go_home_flag_ = &this->arm_go_home_;
-  //   arm_control_ptr->continue_flag_ = &this->arm_continue_;
-  //   arm_control_ptr->power_on_flag_ = &this->arm_power_on_;
-  //   OpcUAServerAPI::mode_ptr_ = &arm_control_ptr->mode_;
-  //   this->status_code_ptr_ = reinterpret_cast<unsigned int *>(&arm_control_ptr->statusCode_);
+  void AmrController::ArmDotuff()
+  {
+    std::shared_ptr<imr_nova_control> arm_control_ptr;
+    arm_control_ptr = std::make_shared<imr_nova_control>();
+    arm_control_ptr->enable_ = &this->enable_;
+    arm_control_ptr->go_home_flag_ = &this->arm_go_home_;
+    arm_control_ptr->continue_flag_ = &this->arm_continue_;
+    arm_control_ptr->power_on_flag_ = &this->arm_power_on_;
+    OpcUAServerAPI::mode_ptr_ = &arm_control_ptr->mode_;
+    this->status_code_ptr_ = reinterpret_cast<unsigned int *>(&arm_control_ptr->statusCode_);
 
-  //   if (!this->arm_go_home_)
-  //   {
-  //     arm_control_ptr->ok_count_max_ = &this->count_ok_max_;
-  //     arm_control_ptr->ng_count_max_ = &this->count_ng_max_;
-  //     arm_control_ptr->startModeThread();
-  //   }
-  //   else
-  //   {
-  //     arm_control_ptr->startHomeThread();
-  //   }
+    if (!this->arm_go_home_)
+    {
+      arm_control_ptr->ok_count_max_ = &this->count_ok_max_;
+      arm_control_ptr->ng_count_max_ = &this->count_ng_max_;
+      arm_control_ptr->startModeThread();
+    }
+    else
+    {
+      arm_control_ptr->startHomeThread();
+    }
 
-  //   arm_control_ptr.reset();
-  //   ROS_INFO("Arm Finished");
-  //   std::lock_guard<std::mutex> lock(this->arm_mutex_);
-  //   this->arm_joined_ = false;
-  // }
+    arm_control_ptr.reset();
+    ROS_INFO("Arm Finished");
+    std::lock_guard<std::mutex> lock(this->arm_mutex_);
+    this->arm_joined_ = false;
+  }
 
-  // void AmrController::unLoadCallBack()
-  // {
-  //   std::lock_guard<std::mutex> lock(this->arm_mutex_);
-  //   {
-  //     ROS_INFO("Shiping call");
-  //     this->belt_joined_ = true;
-  //     this->cancel_ = false;
-  //     this->cur_belt_state_en_ = amr_control::State::WAITING;
-  //     this->belt_thread_ = std::thread(&AmrController::conveyorBeltsShipping, this, std::ref(this->cur_belt_state_en_));
-  //   }
-  // }
+  void AmrController::unLoadCallBack()
+  {
+    std::lock_guard<std::mutex> lock(this->arm_mutex_);
+    {
+      ROS_INFO("Shiping call");
+      this->belt_joined_ = true;
+      this->cancel_ = false;
+      this->cur_belt_state_en_ = amr_control::State::WAITING;
+      this->belt_thread_ = std::thread(&AmrController::conveyorBeltsShipping, this, std::ref(this->cur_belt_state_en_));
+    }
+  }
 
-  // void AmrController::conveyorBeltsShipping(amr_control::State &state)
-  // {
-  //   state = amr_control::State::INITIALIZING;
-  //   std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
-  //   plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
-  //   plc_controller_ptr_->connect();
+  void AmrController::conveyorBeltsShipping(amr_control::State &state)
+  {
+    state = amr_control::State::INITIALIZING;
+    std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
+    plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
+    plc_controller_ptr_->connect();
 
-  //   if (!plc_controller_ptr_->checkConnected())
-  //   {
-  //     state = amr_control::State::FAILED;
-  //     return;
-  //   }
+    if (!plc_controller_ptr_->checkConnected())
+    {
+      state = amr_control::State::FAILED;
+      return;
+    }
 
-  //   if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
-  //   {
-  //     int shipping_regsister = 119, receiving_regsister = 116;
-  //     plc_controller_ptr_->setM(shipping_regsister);
-  //     plc_controller_ptr_->resetM(receiving_regsister);
-  //     ros::Duration(0.1).sleep();
-  //     plc_controller_ptr_->resetM(shipping_regsister);
-  //     plc_controller_ptr_->resetM(receiving_regsister);
+    if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
+    {
+      int shipping_regsister = 119, receiving_regsister = 116;
+      plc_controller_ptr_->setM(shipping_regsister);
+      plc_controller_ptr_->resetM(receiving_regsister);
+      ros::Duration(0.1).sleep();
+      plc_controller_ptr_->resetM(shipping_regsister);
+      plc_controller_ptr_->resetM(receiving_regsister);
 
-  //     ros::Rate r(5);
-  //     while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_)
-  //     {
-  //       state = amr_control::State::RUNNING;
-  //       bool output_belt[2];
-  //       plc_controller_ptr_->mulGetM(124, 125, output_belt);
+      ros::Rate r(5);
+      while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_)
+      {
+        state = amr_control::State::RUNNING;
+        bool output_belt[2];
+        plc_controller_ptr_->mulGetM(124, 125, output_belt);
 
-  //       bool enable_shipping = output_belt[0];
-  //       bool enable_receiving = output_belt[1];
-  //       if (!enable_shipping && !enable_receiving)
-  //       {
-  //         state = amr_control::State::FINISHED;
-  //         break;
-  //       }
-  //       r.sleep();
-  //     }
+        bool enable_shipping = output_belt[0];
+        bool enable_receiving = output_belt[1];
+        if (!enable_shipping && !enable_receiving)
+        {
+          state = amr_control::State::FINISHED;
+          break;
+        }
+        r.sleep();
+      }
 
-  //     if (cancel_ || !plc_controller_ptr_->checkConnected())
-  //       state = amr_control::State::FAILED;
+      if (cancel_ || !plc_controller_ptr_->checkConnected())
+        state = amr_control::State::FAILED;
 
-  //     plc_controller_ptr_->close();
-  //   }
-  //   if (plc_controller_ptr_)
-  //     plc_controller_ptr_.reset();
+      plc_controller_ptr_->close();
+    }
+    if (plc_controller_ptr_)
+      plc_controller_ptr_.reset();
 
-  //   std::lock_guard<std::mutex> lock(this->arm_mutex_);
-  //   this->belt_joined_ = false;
-  // }
+    std::lock_guard<std::mutex> lock(this->arm_mutex_);
+    this->belt_joined_ = false;
+  }
 
-  // void AmrController::loadCallBack()
-  // {
-  //   std::lock_guard<std::mutex> lock(this->arm_mutex_);
-  //   {
-  //     ROS_INFO("Receiving call");
-  //     this->belt_joined_ = true;
-  //     this->cancel_ = false;
-  //     this->cur_belt_state_en_ = amr_control::State::WAITING;
-  //     this->belt_thread_ = std::thread(&AmrController::conveyorBeltsReceiving, this, std::ref(this->cur_belt_state_en_));
-  //   }
-  // }
+  void AmrController::loadCallBack()
+  {
+    std::lock_guard<std::mutex> lock(this->arm_mutex_);
+    {
+      ROS_INFO("Receiving call");
+      this->belt_joined_ = true;
+      this->cancel_ = false;
+      this->cur_belt_state_en_ = amr_control::State::WAITING;
+      this->belt_thread_ = std::thread(&AmrController::conveyorBeltsReceiving, this, std::ref(this->cur_belt_state_en_));
+    }
+  }
 
-  // void AmrController::conveyorBeltsReceiving(amr_control::State &state)
-  // {
-  //   state = amr_control::State::INITIALIZING;
-  //   std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
-  //   plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
-  //   plc_controller_ptr_->connect();
+  void AmrController::conveyorBeltsReceiving(amr_control::State &state)
+  {
+    state = amr_control::State::INITIALIZING;
+    std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
+    plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
+    plc_controller_ptr_->connect();
 
-  //   if (!plc_controller_ptr_->checkConnected())
-  //   {
-  //     state = amr_control::State::FAILED;
-  //     return;
-  //   }
+    if (!plc_controller_ptr_->checkConnected())
+    {
+      state = amr_control::State::FAILED;
+      return;
+    }
 
-  //   if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
-  //   {
-  //     int shipping_regsister = 119, receiving_regsister = 116;
-  //     plc_controller_ptr_->resetM(shipping_regsister);
-  //     plc_controller_ptr_->setM(receiving_regsister);
-  //     ros::Duration(0.1).sleep();
-  //     plc_controller_ptr_->resetM(shipping_regsister);
-  //     plc_controller_ptr_->resetM(receiving_regsister);
+    if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
+    {
+      int shipping_regsister = 119, receiving_regsister = 116;
+      plc_controller_ptr_->resetM(shipping_regsister);
+      plc_controller_ptr_->setM(receiving_regsister);
+      ros::Duration(0.1).sleep();
+      plc_controller_ptr_->resetM(shipping_regsister);
+      plc_controller_ptr_->resetM(receiving_regsister);
 
-  //     ros::Rate r(5);
-  //     while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_)
-  //     {
-  //       state = amr_control::State::RUNNING;
-  //       bool output_belt[2];
-  //       plc_controller_ptr_->mulGetM(124, 125, output_belt);
+      ros::Rate r(5);
+      while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_)
+      {
+        state = amr_control::State::RUNNING;
+        bool output_belt[2];
+        plc_controller_ptr_->mulGetM(124, 125, output_belt);
 
-  //       bool enable_shipping = output_belt[0];
-  //       bool enable_receiving = output_belt[1];
-  //       if (!enable_shipping && !enable_receiving)
-  //       {
-  //         state = amr_control::State::FINISHED;
-  //         break;
-  //       }
-  //       r.sleep();
-  //     }
-  //     if (this->cancel_ || !plc_controller_ptr_->checkConnected())
-  //       state = amr_control::State::FAILED;
+        bool enable_shipping = output_belt[0];
+        bool enable_receiving = output_belt[1];
+        if (!enable_shipping && !enable_receiving)
+        {
+          state = amr_control::State::FINISHED;
+          break;
+        }
+        r.sleep();
+      }
+      if (this->cancel_ || !plc_controller_ptr_->checkConnected())
+        state = amr_control::State::FAILED;
 
-  //     plc_controller_ptr_->close();
-  //   }
-  //   if (plc_controller_ptr_)
-  //     plc_controller_ptr_.reset();
+      plc_controller_ptr_->close();
+    }
+    if (plc_controller_ptr_)
+      plc_controller_ptr_.reset();
 
-  //   std::lock_guard<std::mutex> lock(this->arm_mutex_);
-  //   this->belt_joined_ = false;
-  // }
+    std::lock_guard<std::mutex> lock(this->arm_mutex_);
+    this->belt_joined_ = false;
+  }
 
-  // void AmrController::controllerDotuff()
-  // {
-  //   ros::Rate r(10);
-  //   while (ros::ok())
-  //   {
-  //     std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
-  //     plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
-  //     plc_controller_ptr_->connect();
-  //     r.sleep();
-  //     ros::spinOnce();
+  void AmrController::controllerDotuff()
+  {
+    ros::Rate r(10);
+    while (ros::ok())
+    {
+      std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
+      plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
+      plc_controller_ptr_->connect();
+      r.sleep();
+      ros::spinOnce();
 
-  //     if (plc_controller_ptr_ == nullptr)
-  //       continue;
-  //     if (!plc_controller_ptr_->checkConnected())
-  //       continue;
+      if (plc_controller_ptr_ == nullptr)
+        continue;
+      if (!plc_controller_ptr_->checkConnected())
+        continue;
 
-  //     this->amr_safety_ptr_ = std::make_shared<AmrSafety>();
-  //     this->amr_safety_ptr_->getController(plc_controller_ptr_);
+      this->amr_safety_ptr_ = std::make_shared<AmrSafety>();
+      this->amr_safety_ptr_->getController(plc_controller_ptr_);
 
-  //     while (ros::ok() && plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
-  //     {
-  //       if (!this->monitor_ptr_)
-  //         continue;
-  //       nav_2d_msgs::Twist2D velocity;
-  //       if (this->monitor_ptr_->getVelocity(velocity))
-  //       {
-  //         cmd_vel_mtx.lock();
-  //         this->amr_safety_ptr_->safetyHandle(velocity, cmd_vel_max_, cmd_vel_recommended_);
-  //         cmd_vel_mtx.unlock();
-  //       }
+      while (ros::ok() && plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
+      {
+        if (!this->monitor_ptr_)
+          continue;
+        nav_2d_msgs::Twist2D velocity;
+        if (this->monitor_ptr_->getVelocity(velocity))
+        {
+          cmd_vel_mtx.lock();
+          this->amr_safety_ptr_->safetyHandle(velocity, cmd_vel_max_, cmd_vel_recommended_);
+          cmd_vel_mtx.unlock();
+        }
 
-  //       if (move_base_ptr_ != nullptr &&
-  //           move_base_ptr_->nav_feedback_ != nullptr &&
-  //           move_base_ptr_->nav_feedback_->is_ready)
-  //       {
-  //         if (velocity.x <= -0.01)
-  //           this->amr_safety_ptr_->writeMutesSafety(true);
-  //         else
-  //         {
-  //           this->amr_safety_ptr_->writeMutesSafety(this->muted_);
-  //         }
-  //       }
+        if (move_base_ptr_ != nullptr &&
+            move_base_ptr_->getFeedback() != nullptr &&
+            move_base_ptr_->getFeedback()->is_ready)
+        {
+          if (velocity.x <= -0.01)
+            this->amr_safety_ptr_->writeMutesSafety(true);
+          else
+          {
+            this->amr_safety_ptr_->writeMutesSafety(this->muted_);
+          }
+        }
 
-  //       bool have_goods;
-  //       int have_goods_regsister = 142;
-  //       if (!OpcUAServerAPI::belt_cancel_)
-  //       {
-  //         plc_controller_ptr_->getM(have_goods_regsister, have_goods);
-  //         OpcUAServerAPI::have_goods_ = have_goods;
-  //       }
-  //       else
-  //       {
-  //         plc_controller_ptr_->resetM(have_goods_regsister);
-  //       }
+        bool have_goods;
+        int have_goods_regsister = 142;
+        if (!OpcUAServerAPI::belt_cancel_)
+        {
+          plc_controller_ptr_->getM(have_goods_regsister, have_goods);
+          OpcUAServerAPI::have_goods_ = have_goods;
+        }
+        else
+        {
+          plc_controller_ptr_->resetM(have_goods_regsister);
+        }
 
-  //       if (vda_5050_client_api_ptr_)
-  //       {
-  //         amr_control::OperatingMode mode_;
-  //         bool operating_mode[4];
-  //         plc_controller_ptr_->mulGetM(14, 17, operating_mode);
-  //         if (operating_mode[0])
-  //           mode_ = amr_control::OperatingMode::AUTOMATIC;
-  //         else if (operating_mode[2])
-  //           mode_ = amr_control::OperatingMode::SERVICE;
-  //         else if (operating_mode[3])
-  //           mode_ = amr_control::OperatingMode::MANUAL;
+        if (vda_5050_client_api_ptr_)
+        {
+          amr_control::OperatingMode mode_;
+          bool operating_mode[4];
+          plc_controller_ptr_->mulGetM(14, 17, operating_mode);
+          if (operating_mode[0])
+            mode_ = amr_control::OperatingMode::AUTOMATIC;
+          else if (operating_mode[2])
+            mode_ = amr_control::OperatingMode::SERVICE;
+          else if (operating_mode[3])
+            mode_ = amr_control::OperatingMode::MANUAL;
 
-  //         switch (mode_)
-  //         {
-  //         case amr_control::OperatingMode::AUTOMATIC:
-  //           vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::AUTOMATIC;
-  //           break;
-  //         case amr_control::OperatingMode::MANUAL:
-  //           vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::MANUAL;
-  //           break;
-  //         case amr_control::OperatingMode::SERVICE:
-  //           vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE;
-  //           break;
-  //         default:
-  //           vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE; // Default
-  //           break;
-  //         }
-  //       }
-  //       r.sleep();
-  //       ros::spinOnce();
-  //     }
-  //     if (plc_controller_ptr_)
-  //       plc_controller_ptr_.reset();
-  //     if (amr_safety_ptr_)
-  //       amr_safety_ptr_.reset();
-  //   }
-  // }
+          switch (mode_)
+          {
+          case amr_control::OperatingMode::AUTOMATIC:
+            vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::AUTOMATIC;
+            break;
+          case amr_control::OperatingMode::MANUAL:
+            vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::MANUAL;
+            break;
+          case amr_control::OperatingMode::SERVICE:
+            vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE;
+            break;
+          default:
+            vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE; // Default
+            break;
+          }
+        }
+        r.sleep();
+        ros::spinOnce();
+      }
+      if (plc_controller_ptr_)
+        plc_controller_ptr_.reset();
+      if (amr_safety_ptr_)
+        amr_safety_ptr_.reset();
+    }
+  }
 
-  // void AmrController::isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg)
-  // {
-  //   this->muted_ = msg->data;
-  // }
+  void AmrController::isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg)
+  {
+    this->muted_ = msg->data;
+  }
 
-  // void AmrController::threadHandle()
-  // {
-  //   ros::Rate r(5);
-  //   while (ros::ok())
-  //   {
-  //     if (this->arm_thread_.joinable())
-  //     {
-  //       std::lock_guard<std::mutex> lock(this->arm_mutex_);
-  //       {
-  //         if (!this->arm_joined_)
-  //         {
-  //           this->arm_thread_.join();
-  //         }
-  //       }
-  //     }
+  void AmrController::threadHandle()
+  {
+    ros::Rate r(5);
+    while (ros::ok())
+    {
+      if (this->arm_thread_.joinable())
+      {
+        std::lock_guard<std::mutex> lock(this->arm_mutex_);
+        {
+          if (!this->arm_joined_)
+          {
+            this->arm_thread_.join();
+          }
+        }
+      }
 
-  //     if (this->belt_thread_.joinable())
-  //     {
-  //       std::lock_guard<std::mutex> lock(this->belt_mutex_);
-  //       {
-  //         if (!this->belt_joined_)
-  //         {
-  //           this->belt_thread_.join();
-  //         }
-  //       }
-  //     }
+      if (this->belt_thread_.joinable())
+      {
+        std::lock_guard<std::mutex> lock(this->belt_mutex_);
+        {
+          if (!this->belt_joined_)
+          {
+            this->belt_thread_.join();
+          }
+        }
+      }
 
-  //     if (move_base_ptr_ != nullptr &&
-  //         move_base_ptr_->nav_feedback_ != nullptr &&
-  //         move_base_ptr_->nav_feedback_->is_ready)
-  //     {
-  //       nav_2d_msgs::Twist2D velocity;
-  //       if (this->monitor_ptr_ && this->monitor_ptr_->getVelocity(velocity))
-  //       {
-  //         this->cmd_vel_max_.x = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, std::max(OpcUAServerAPI::cmd_vel_max_.x, vda_5050_client_api_ptr_->cmd_vel_max_.x));
-  //         this->cmd_vel_max_.theta = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, vda_5050_client_api_ptr_->cmd_vel_max_.theta);
+      if (move_base_ptr_ != nullptr &&
+          move_base_ptr_->getFeedback() != nullptr &&
+          move_base_ptr_->getFeedback()->is_ready)
+      {
+        nav_2d_msgs::Twist2D velocity;
+        if (this->monitor_ptr_ && this->monitor_ptr_->getVelocity(velocity))
+        {
+          this->cmd_vel_max_.x = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, std::max(OpcUAServerAPI::cmd_vel_max_.x, vda_5050_client_api_ptr_->cmd_vel_max_.x));
+          this->cmd_vel_max_.theta = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, vda_5050_client_api_ptr_->cmd_vel_max_.theta);
 
-  //         geometry_msgs::Vector3 linear;
-  //         geometry_msgs::Vector3 angular;
+          robot_geometry_msgs::Vector3 linear;
+          robot_geometry_msgs::Vector3 angular;
 
-  //         cmd_vel_mtx.lock();
-  //         linear.x = this->amr_safety_ptr_ != nullptr ? this->cmd_vel_recommended_.x : this->cmd_vel_max_.x;
-  //         angular.z = this->cmd_vel_max_.theta;
-  //         cmd_vel_mtx.unlock();
+          cmd_vel_mtx.lock();
+          linear.x = this->amr_safety_ptr_ != nullptr ? this->cmd_vel_recommended_.x : this->cmd_vel_max_.x;
+          angular.z = this->cmd_vel_max_.theta;
+          cmd_vel_mtx.unlock();
 
-  //         this->move_base_ptr_->setTwistLinear(linear);
-  //         linear.x *= -1.0;
-  //         this->move_base_ptr_->setTwistLinear(linear);
-  //         this->move_base_ptr_->setTwistAngular(angular);
-  //       }
-  //     }
-  //     r.sleep();
-  //     ros::spinOnce();
-  //   }
-  // }
+          this->move_base_ptr_->setTwistLinear(linear);
+          linear.x *= -1.0;
+          this->move_base_ptr_->setTwistLinear(linear);
+          this->move_base_ptr_->setTwistAngular(angular);
+        }
+      }
+      r.sleep();
+      ros::spinOnce();
+    }
+  }
 }

Controllers/Packages/amr_control/src/amr_control_node.cpp

@@ -21,7 +21,7 @@ int main(int argc, char** argv)
   }
   catch (const std::exception& exc)
   {
-    std::cout << exc.what() << std::endl;
+    ROS_ERROR("Exception: %s", exc.what());
   }
   return 0;
 }

Controllers/Packages/amr_control/src/amr_publiser.cpp

@@ -0,0 +1,594 @@
+#include <amr_control/amr_publiser.h>
+#include <robot/robot.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <robot_costmap_2d/cost_values.h>
+#include <boost/bind.hpp>
+
+
+namespace amr_control
+{
+
+AmrPublisher::AmrPublisher(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr)
+  : nh_(nh),
+    move_base_ptr_(move_base_ptr)
+{
+  // Initialize default topic names
+  std::string global_costmap_topic = "/global_costmap/costmap";
+  std::string local_costmap_topic = "/local_costmap/costmap";
+  std::string cmd_vel_topic = "/cmd_vel";
+  // Get topic names from parameter server if available
+  nh_.param("global_costmap_topic", global_costmap_topic, global_costmap_topic);
+  nh_.param("local_costmap_topic", local_costmap_topic, local_costmap_topic);
+  nh_.param("cmd_vel_topic", cmd_vel_topic, cmd_vel_topic);
+  // Initialize publishers with callback for new subscriptions
+  global_planner_obj_.pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(
+    global_costmap_topic, 1, 
+    boost::bind(&AmrPublisher::onNewSubscriptionGlobal, this, _1));
+  global_planner_obj_.update_pub_ = nh_.advertise<map_msgs::OccupancyGridUpdate>(global_costmap_topic + "_updates", 1);
+  global_planner_obj_.footprint_pub_ = nh_.advertise<geometry_msgs::PolygonStamped>(global_costmap_topic + "/footprint", 1);
+  global_planner_obj_.plan_pub_ = nh_.advertise< nav_msgs::Path>(global_costmap_topic + "/plan", 1);
+  
+  local_planner_obj_.pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(
+    local_costmap_topic, 1, 
+    boost::bind(&AmrPublisher::onNewSubscriptionLocal, this, _1));
+  local_planner_obj_.update_pub_ = nh_.advertise<map_msgs::OccupancyGridUpdate>(local_costmap_topic + "_updates", 1);
+  local_planner_obj_.footprint_pub_ = nh_.advertise<geometry_msgs::PolygonStamped>(local_costmap_topic + "/footprint", 1);
+  local_planner_obj_.plan_pub_ = nh_.advertise< nav_msgs::Path>(local_costmap_topic + "/plan", 1);
+
+  cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>(cmd_vel_topic, 1);
+  cmd_vel_publishing_active_ = false;
+  robot::log_info("[AmrPublisher] Initialized. Cmd vel topic: %s", cmd_vel_topic.c_str());
+  // Initialize topic names in CostmapObject
+  global_planner_obj_.topic_ = global_costmap_topic;
+  local_planner_obj_.topic_ = local_costmap_topic;
+  
+  // Initialize other CostmapObject members
+  global_planner_obj_.active_ = false;
+  local_planner_obj_.active_ = false;
+
+  // Initialize plan publishing flags
+  global_plan_publishing_active_ = false;
+  local_plan_publishing_active_ = false;
+
+
+  robot::log_info("[AmrPublisher] Initialized. Global costmap topic: %s, Local costmap topic: %s",
+                  global_costmap_topic.c_str(), local_costmap_topic.c_str());
+  robot::log_info("[AmrPublisher] Global footprint topic: %s/footprint, Local footprint topic: %s/footprint",
+                  global_costmap_topic.c_str(), local_costmap_topic.c_str());
+              
+}
+
+AmrPublisher::~AmrPublisher()
+{
+  stopPublishing();
+  stopCmdVelPublishing();
+  stopPlanPublishing();
+}
+
+void AmrPublisher::convertToNavOccupancyGrid(const robot_nav_msgs::OccupancyGrid &robot_grid, 
+                                              nav_msgs::OccupancyGrid &nav_grid)
+{
+  // Convert header
+  nav_grid.header.seq = robot_grid.header.seq;
+  nav_grid.header.frame_id = robot_grid.header.frame_id;
+  nav_grid.header.stamp = ros::Time::now();
+  
+  // Convert map metadata
+  nav_grid.info.map_load_time = ros::Time::now();
+  nav_grid.info.resolution = robot_grid.info.resolution;
+  nav_grid.info.width = robot_grid.info.width;
+  nav_grid.info.height = robot_grid.info.height;
+
+  // Convert origin
+  nav_grid.info.origin.position.x = robot_grid.info.origin.position.x;
+  nav_grid.info.origin.position.y = robot_grid.info.origin.position.y;
+  nav_grid.info.origin.position.z = robot_grid.info.origin.position.z;
+  nav_grid.info.origin.orientation.x = robot_grid.info.origin.orientation.x;
+  nav_grid.info.origin.orientation.y = robot_grid.info.origin.orientation.y;
+  nav_grid.info.origin.orientation.z = robot_grid.info.origin.orientation.z;
+  nav_grid.info.origin.orientation.w = robot_grid.info.origin.orientation.w;
+
+  // Copy occupancy data (both use int8_t, so direct copy is safe)
+  nav_grid.data = robot_grid.data;
+}
+
+void AmrPublisher::convertToRobotOccupancyGridUpdate(const robot_map_msgs::OccupancyGridUpdate &robot_grid_update, 
+                                                      map_msgs::OccupancyGridUpdate &grid_update)
+{
+  // Convert header
+  grid_update.header.stamp = ros::Time::now();
+  grid_update.header.frame_id = robot_grid_update.header.frame_id;
+  grid_update.x = robot_grid_update.x;
+  grid_update.y = robot_grid_update.y;
+  grid_update.width = robot_grid_update.width;
+  grid_update.height = robot_grid_update.height;
+  grid_update.data = robot_grid_update.data;
+}
+
+void AmrPublisher::convertToNavPolygonStamped(const robot_geometry_msgs::PolygonStamped &robot_footprint, 
+                                               geometry_msgs::PolygonStamped &nav_footprint)
+{
+  // Convert header
+  nav_footprint.header.seq = robot_footprint.header.seq;
+  nav_footprint.header.frame_id = robot_footprint.header.frame_id;
+  nav_footprint.header.stamp = ros::Time::now();
+  
+  // Convert polygon points
+  nav_footprint.polygon.points.clear();
+  nav_footprint.polygon.points.reserve(robot_footprint.polygon.points.size());
+  for (const auto &robot_point : robot_footprint.polygon.points)
+  {
+    geometry_msgs::Point32 nav_point;
+    nav_point.x = robot_point.x;
+    nav_point.y = robot_point.y;
+    nav_point.z = robot_point.z;
+    nav_footprint.polygon.points.push_back(nav_point);
+  }
+}
+
+void AmrPublisher::publishGlobalCostmap(double publish_rate)
+{
+  if (!move_base_ptr_)
+  {
+    robot::log_warning("[AmrPublisher] move_base_ptr_ is null, cannot publish global costmap");
+    return;
+  }
+
+  try
+  {
+    // Get global costmap data from move_base
+    robot::move_base_core::PlannerDataOutput global_data = move_base_ptr_->getGlobalData();
+    
+    if(!global_data.is_costmap_updated)
+    {
+      nav_msgs::OccupancyGrid nav_grid;
+      convertToNavOccupancyGrid(global_data.costmap, nav_grid);
+      global_planner_obj_.pub_.publish(nav_grid);
+    }
+    else
+    {
+      map_msgs::OccupancyGridUpdate grid_update;
+      convertToRobotOccupancyGridUpdate(global_data.costmap_update, grid_update);
+      global_planner_obj_.update_pub_.publish(grid_update);
+    }
+    
+    // Publish footprint
+    publishGlobalFootprint();
+    
+    // robot::log_debug("[AmrPublisher] Published global costmap: %dx%d, frame_id='%s'", 
+    //                  nav_grid.info.width, nav_grid.info.height, nav_grid.header.frame_id.c_str());
+  }
+  catch (const std::exception &e)
+  {
+    robot::log_error("[AmrPublisher] Error publishing global costmap: %s", e.what());
+  }
+}
+
+void AmrPublisher::publishLocalCostmap(double publish_rate)
+{
+  if (!move_base_ptr_)
+  {
+    robot::log_warning("[AmrPublisher] move_base_ptr_ is null, cannot publish local costmap");
+    return;
+  }
+
+  try
+  {
+    // Get local costmap data from move_base
+    robot::move_base_core::PlannerDataOutput local_data = move_base_ptr_->getLocalData();
+    
+
+    if(!local_data.is_costmap_updated)
+    {
+      nav_msgs::OccupancyGrid nav_grid;
+      convertToNavOccupancyGrid(local_data.costmap, nav_grid);
+      local_planner_obj_.pub_.publish(nav_grid);
+    }
+    else
+    {
+      map_msgs::OccupancyGridUpdate grid_update;
+      convertToRobotOccupancyGridUpdate(local_data.costmap_update, grid_update);
+      local_planner_obj_.update_pub_.publish(grid_update);
+    }
+    
+    // Publish footprint
+    publishLocalFootprint();
+    
+    // robot::log_debug("[AmrPublisher] Published local costmap: %dx%d, frame_id='%s'", 
+    //                  nav_grid.info.width, nav_grid.info.height, nav_grid.header.frame_id.c_str());
+  }
+  catch (const std::exception &e)
+  {
+    robot::log_error("[AmrPublisher] Error publishing local costmap: %s", e.what());
+  }
+}
+
+void AmrPublisher::publishCostmaps(double publish_rate)
+{
+  publishGlobalCostmap(publish_rate);
+  publishLocalCostmap(publish_rate);
+}
+
+void AmrPublisher::publishGlobalFootprint()
+{
+  if (!move_base_ptr_)
+  {
+    robot::log_warning("[AmrPublisher] move_base_ptr_ is null, cannot publish global footprint");
+    return;
+  }
+
+  try
+  {
+    // Get global data from move_base
+    robot::move_base_core::PlannerDataOutput global_data = move_base_ptr_->getGlobalData();
+    
+    // Convert and publish footprint
+    geometry_msgs::PolygonStamped nav_footprint;
+    convertToNavPolygonStamped(global_data.footprint, nav_footprint);
+    global_planner_obj_.footprint_pub_.publish(nav_footprint);
+    
+    // robot::log_debug("[AmrPublisher] Published global footprint with %zu points, frame_id='%s'", 
+    //                  nav_footprint.polygon.points.size(), nav_footprint.header.frame_id.c_str());
+  }
+  catch (const std::exception &e)
+  {
+    robot::log_error("[AmrPublisher] Error publishing global footprint: %s", e.what());
+  }
+}
+
+void AmrPublisher::publishLocalFootprint()
+{
+  if (!move_base_ptr_)
+  {
+    robot::log_warning("[AmrPublisher] move_base_ptr_ is null, cannot publish local footprint");
+    return;
+  }
+
+  try
+  {
+    // Get local data from move_base
+    robot::move_base_core::PlannerDataOutput local_data = move_base_ptr_->getLocalData();
+    
+    // Convert and publish footprint
+    geometry_msgs::PolygonStamped nav_footprint;
+    convertToNavPolygonStamped(local_data.footprint, nav_footprint);
+    local_planner_obj_.footprint_pub_.publish(nav_footprint);
+    
+    // robot::log_debug("[AmrPublisher] Published local footprint with %zu points, frame_id='%s'", 
+    //                  nav_footprint.polygon.points.size(), nav_footprint.header.frame_id.c_str());
+  }
+  catch (const std::exception &e)
+  {
+    robot::log_error("[AmrPublisher] Error publishing local footprint: %s", e.what());
+  }
+}
+
+void AmrPublisher::publishFootprints()
+{
+  publishGlobalFootprint();
+  publishLocalFootprint();
+}
+
+void AmrPublisher::startPublishing(double global_rate, double local_rate)
+{
+  stopPublishing(); // Stop any existing publishing first
+
+  // Start global costmap timer if rate > 0
+  if (global_rate > 0.0)
+  {
+    global_planner_obj_.active_ = true;
+    global_planner_obj_.timer_ = nh_.createTimer(
+      ros::Duration(1.0 / global_rate),
+      &AmrPublisher::globalCostmapTimerCallback,
+      this
+    );
+    robot::log_info("[AmrPublisher] Started publishing global costmap at %.2f Hz", global_rate);
+  }
+
+  // Start local costmap timer if rate > 0
+  if (local_rate > 0.0)
+  {
+    local_planner_obj_.active_ = true;
+    local_planner_obj_.timer_ = nh_.createTimer(
+      ros::Duration(1.0 / local_rate),
+      &AmrPublisher::localCostmapTimerCallback,
+      this
+    );
+    robot::log_info("[AmrPublisher] Started publishing local costmap at %.2f Hz", local_rate);
+  }
+}
+
+void AmrPublisher::stopPublishing()
+{
+  // Stop timers
+  if (global_planner_obj_.active_)
+  {
+    global_planner_obj_.timer_.stop();
+    global_planner_obj_.active_ = false;
+  }
+  
+  if (local_planner_obj_.active_)
+  {
+    local_planner_obj_.timer_.stop();
+    local_planner_obj_.active_ = false;
+  }
+
+  robot::log_info("[AmrPublisher] Stopped publishing costmaps");
+}
+
+void AmrPublisher::globalCostmapTimerCallback(const ros::TimerEvent &event)
+{
+  if (global_planner_obj_.active_)
+  {
+    publishGlobalCostmap();
+  }
+}
+
+void AmrPublisher::localCostmapTimerCallback(const ros::TimerEvent &event)
+{
+  if (local_planner_obj_.active_)
+  {
+    publishLocalCostmap();
+  }
+}
+
+void AmrPublisher::onNewSubscriptionLocal(const ros::SingleSubscriberPublisher& pub)
+{
+  robot::log_info("[AmrPublisher] New subscription: %s", pub.getSubscriberName().c_str());
+  publishLocalCostmap();
+}
+
+
+void AmrPublisher::onNewSubscriptionGlobal(const ros::SingleSubscriberPublisher& pub)
+{
+  robot::log_info("[AmrPublisher] New subscription: %s", pub.getSubscriberName().c_str());
+  publishGlobalCostmap();
+}
+
+void AmrPublisher::publishCmdVel()
+{
+  if(!move_base_ptr_)
+  {
+    return;
+  }
+
+  if(!move_base_ptr_->getFeedback()->is_ready)
+  {
+    return;
+  }
+
+  robot_nav_2d_msgs::Twist2DStamped twist = move_base_ptr_->getTwist();
+  if(twist.header.stamp < robot::Time::now() - robot::Duration(0.05))
+  {
+    return;
+  }
+
+  geometry_msgs::Twist cmd_vel;
+  cmd_vel.linear.x = twist.velocity.x;
+  cmd_vel.linear.y = twist.velocity.y;
+  cmd_vel.angular.z = twist.velocity.theta;
+  cmd_vel_pub_.publish(cmd_vel);
+}
+
+void AmrPublisher::startCmdVelPublishing(double rate)
+{
+  stopCmdVelPublishing(); // Stop any existing publishing first
+
+  if (rate <= 0.0)
+  {
+    robot::log_warning("[AmrPublisher] Invalid cmd_vel publishing rate: %.2f Hz. Must be > 0", rate);
+    return;
+  }
+
+  ros::WallDuration period(1.0 / rate);
+  cmd_vel_timer_ = nh_.createWallTimer(
+    period,
+    &AmrPublisher::cmdVelTimerCallback,
+    this,
+    false  // oneshot = false
+  );
+
+  cmd_vel_publishing_active_ = true;
+  robot::log_info("[AmrPublisher] Started publishing cmd_vel at %.2f Hz using ros::WallTimer", rate);
+}
+
+void AmrPublisher::stopCmdVelPublishing()
+{
+  if (cmd_vel_publishing_active_)
+  {
+    cmd_vel_timer_.stop();
+    cmd_vel_publishing_active_ = false;
+    robot::log_info("[AmrPublisher] Stopped publishing cmd_vel");
+  }
+}
+
+void AmrPublisher::cmdVelTimerCallback(const ros::WallTimerEvent &event)
+{
+  (void)event;
+  if (cmd_vel_publishing_active_)
+  {
+    publishCmdVel();
+  }
+}
+
+void AmrPublisher::convertToNavPath(const robot_nav_2d_msgs::Path2D &robot_path, 
+                          nav_msgs::Path &nav_path)
+{
+  // Convert header
+  nav_2d_msgs::Path2D path2d;
+  path2d.header.seq = robot_path.header.seq;
+  path2d.header.frame_id = robot_path.header.frame_id;
+  path2d.header.stamp = ros::Time::now();
+  path2d.poses.resize(robot_path.poses.size());
+  for (unsigned int i = 0; i < robot_path.poses.size(); i++)
+  {
+    path2d.poses[i].pose.x = robot_path.poses[i].pose.x;
+    path2d.poses[i].pose.y = robot_path.poses[i].pose.y;
+    path2d.poses[i].pose.theta = robot_path.poses[i].pose.theta;
+  }
+  nav_path = nav_2d_utils::pathToPath(path2d);
+}
+
+void AmrPublisher::publishGlobalPlan()
+{
+  if(!move_base_ptr_)
+  {
+    return;
+  }
+  
+  // Check if plan is empty
+  if(cached_global_plan_.poses.empty())
+  {
+    return;
+  }
+  
+  robot::Time now = robot::Time::now();
+  robot::Time plan_stamp = cached_global_plan_.header.stamp;
+  
+  // Check if timestamp is zero (uninitialized)
+  if(plan_stamp.sec == 0 && plan_stamp.nsec == 0)
+  {
+    return;
+  }
+  
+  robot::Duration age = now - plan_stamp;
+  robot::Duration max_age(0.5);
+  
+  // Check if timestamp is not older than 0.5 seconds
+  if(plan_stamp < now - max_age)
+  {
+    return;
+  }
+  
+  nav_msgs::Path nav_path;
+  convertToNavPath(cached_global_plan_, nav_path);
+  
+  global_planner_obj_.plan_pub_.publish(nav_path);
+}
+
+void AmrPublisher::publishLocalPlan()
+{
+  if(!move_base_ptr_)
+  {
+    return;
+  }
+  
+  // Check if plan is empty
+  if(cached_local_plan_.poses.empty())
+  {
+    return;
+  }
+  
+  robot::Time now = robot::Time::now();
+  robot::Time plan_stamp = cached_local_plan_.header.stamp;
+  
+  // Check if timestamp is zero (uninitialized)
+  if(plan_stamp.sec == 0 && plan_stamp.nsec == 0)
+  {
+    return;
+  }
+  
+  
+  robot::Duration age = now - plan_stamp;
+  robot::Duration max_age(0.5);
+  
+  // Check if timestamp is not older than 0.5 seconds
+  if(plan_stamp < now - max_age)
+  {
+    return;
+  }
+  
+  nav_msgs::Path nav_path;
+  convertToNavPath(cached_local_plan_, nav_path);
+  local_planner_obj_.plan_pub_.publish(nav_path);
+}
+
+void AmrPublisher::globalPlanTimerCallback(const ros::TimerEvent &event)
+{
+  if (!move_base_ptr_)
+  {
+    return;
+  }
+  
+  // Update cached global plan
+  try
+  {
+    cached_global_plan_ = move_base_ptr_->getGlobalData().plan;
+    // Publish if timestamp is valid
+    publishGlobalPlan();
+  }
+  catch (const std::exception &e)
+  {
+    robot::log_error("[AmrPublisher] Error updating global plan: %s", e.what());
+  }
+}
+
+void AmrPublisher::localPlanTimerCallback(const ros::TimerEvent &event)
+{
+  if (!move_base_ptr_)
+  {
+    return;
+  }
+  
+  // Update cached local plan
+  try
+  {
+    cached_local_plan_ = move_base_ptr_->getLocalData().plan;
+    // Publish if timestamp is valid
+    publishLocalPlan();
+  }
+  catch (const std::exception &e)
+  {
+    robot::log_error("[AmrPublisher] Error updating local plan: %s", e.what());
+  }
+}
+
+void AmrPublisher::startPlanPublishing(double global_rate, double local_rate)
+{
+  stopPlanPublishing(); // Stop any existing publishing first
+
+  // Start global plan timer if rate > 0
+  if (global_rate > 0.0)
+  {
+    global_plan_publishing_active_ = true;
+    global_plan_timer_ = nh_.createTimer(
+      ros::Duration(1.0 / global_rate),
+      &AmrPublisher::globalPlanTimerCallback,
+      this
+    );
+    robot::log_info("[AmrPublisher] Started publishing global plan at %.2f Hz", global_rate);
+  }
+
+  // Start local plan timer if rate > 0
+  if (local_rate > 0.0)
+  {
+    local_plan_publishing_active_ = true;
+    local_plan_timer_ = nh_.createTimer(
+      ros::Duration(1.0 / local_rate),
+      &AmrPublisher::localPlanTimerCallback,
+      this
+    );
+    robot::log_info("[AmrPublisher] Started publishing local plan at %.2f Hz", local_rate);
+  }
+}
+
+void AmrPublisher::stopPlanPublishing()
+{
+  // Stop timers
+  if (global_plan_publishing_active_)
+  {
+    global_plan_timer_.stop();
+    global_plan_publishing_active_ = false;
+  }
+  
+  if (local_plan_publishing_active_)
+  {
+    local_plan_timer_.stop();
+    local_plan_publishing_active_ = false;
+  }
+
+  robot::log_info("[AmrPublisher] Stopped publishing plans");
+}
+
+} // namespace amr_control
+

Controllers/Packages/amr_control/src/amr_subcriber.cpp

@@ -0,0 +1,53 @@
+#include "amr_control/amr_subcriber.h"
+#include <geometry_msgs/PoseStamped.h>
+
+amr_control::AmrSubscriber::AmrSubscriber(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr)
+  : nh_(nh), move_base_ptr_(move_base_ptr)
+{   
+  robot::log_info("[%s:%d] Initializing AmrSubscriber namespace: %s", __FILE__, __LINE__, nh_.getNamespace().c_str());
+  ros::NodeHandle nh_move_base;
+  move_base_simple_sub_ = nh_move_base.subscribe("/move_base_simple/goal", 1, &AmrSubscriber::moveBaseSimpleCallback, this);
+  odometry_sub_ = nh_move_base.subscribe("/odom", 1, &AmrSubscriber::odometryCallback, this);
+
+}
+
+amr_control::AmrSubscriber::~AmrSubscriber()
+{
+}
+
+void amr_control::AmrSubscriber::moveBaseSimpleCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
+{
+  robot_geometry_msgs::PoseStamped goal;
+  goal.header.stamp = robot::Time::now();
+  goal.header.frame_id = msg->header.frame_id;
+  goal.pose.position.x = msg->pose.position.x;
+  goal.pose.position.y = msg->pose.position.y;
+  goal.pose.orientation.x = msg->pose.orientation.x;
+  goal.pose.orientation.y = msg->pose.orientation.y;
+  goal.pose.orientation.z = msg->pose.orientation.z;
+  goal.pose.orientation.w = msg->pose.orientation.w;
+  move_base_ptr_->moveTo(goal, 0.02, 0.02);
+}
+
+void amr_control::AmrSubscriber::odometryCallback(const nav_msgs::Odometry::ConstPtr &msg)
+{
+  if (!move_base_ptr_)
+  {
+    return;
+  }
+  robot_nav_msgs::Odometry odometry;
+  odometry.header.stamp = robot::Time::now();
+  odometry.header.frame_id = msg->header.frame_id;
+  odometry.child_frame_id = msg->child_frame_id;
+  odometry.pose.pose.position.x = msg->pose.pose.position.x;
+  odometry.pose.pose.position.y = msg->pose.pose.position.y;
+  odometry.pose.pose.position.z = msg->pose.pose.position.z;
+  odometry.pose.pose.orientation.x = msg->pose.pose.orientation.x;
+  odometry.pose.pose.orientation.y = msg->pose.pose.orientation.y;
+  odometry.pose.pose.orientation.z = msg->pose.pose.orientation.z;
+  odometry.pose.pose.orientation.w = msg->pose.pose.orientation.w;
+  odometry.twist.twist.linear.x = msg->twist.twist.linear.x;
+  odometry.twist.twist.linear.y = msg->twist.twist.linear.y;
+  odometry.twist.twist.angular.z = msg->twist.twist.angular.z;
+  move_base_ptr_->addOdometry("odometry", odometry);
+}

Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp

@@ -3,6 +3,9 @@
 #include <functional>
 #include <optional>
 #include "vda5050_msgs/Order.h"
+#include "robot_nav_msgs/Path.h"
+#include "robot_geometry_msgs/Pose2D.h"
+#include "robot_nav_2d_utils/conversions.h"
 
 namespace amr_control
 {
@@ -20,7 +23,7 @@ namespace amr_control
     this->count_ok_max_ = new unsigned int(1);
   }
 
-  VDA5050ClientAPI::VDA5050ClientAPI(ros::NodeHandle nh, std::shared_ptr<move_base_core::BaseNavigation> move_base,
+  VDA5050ClientAPI::VDA5050ClientAPI(ros::NodeHandle nh, std::shared_ptr<robot::move_base_core::BaseNavigation> move_base,
                                      std::shared_ptr<loc_core::BaseLocalization> loc_base, std::shared_ptr<AmrMonitor> monitor)
   {
     VDA5050ClientAPI::move_base_ptr_ = move_base;
@@ -116,7 +119,7 @@ namespace amr_control
 
   void VDA5050ClientAPI::moveTo(vda_5050::Order order, uint8_t &status, std::string &message)
   {
-    geometry_msgs::PoseStamped goal;
+    robot_geometry_msgs::PoseStamped goal;
     if (VDA5050ClientAPI::move_base_ptr_)
     {
       if (global_plan_msg_type_ == std::string("nav_msgs::Path"))
@@ -141,22 +144,24 @@ namespace amr_control
         if (!paths.empty())
           return;
 
-        std::vector<geometry_msgs::Pose2D> poses;
+        std::vector<robot_geometry_msgs::Pose2D> poses;
         for (auto &p : paths)
         {
-          geometry_msgs::Pose2D p_2d;
+          robot_geometry_msgs::Pose2D p_2d;
           p_2d.x = p.getX();
           p_2d.y = p.getY();
           p_2d.theta = p.getYaw();
           poses.push_back(p_2d);
         }
-        const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now());
+        const robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(poses, "map", robot::Time::now());
         goal = path.poses.back();
-        VDA5050ClientAPI::plan_pub_.publish(path);
+        // VDA5050ClientAPI::plan_pub_.publish(path);
+        VDA5050ClientAPI::move_base_ptr_->moveTo(goal);
       }
       else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order"))
       {
-        vda5050_msgs::Order order_msg;
+        robot_protocol_msgs::Order order_msg;
+        // vda5050_msgs::Order order_msg;
         order_msg.headerId = order.headerId;
         order_msg.timestamp = order.timestamp;
         order_msg.version = order.version;
@@ -167,7 +172,7 @@ namespace amr_control
 
         for (auto node : order.nodes)
         {
-          vda5050_msgs::Node node_msg;
+          robot_protocol_msgs::Node node_msg;
           node_msg.nodeId = node.nodeId;
           node_msg.sequenceId = node.sequenceId;
           node_msg.nodeDescription = node.nodeDescription;
@@ -186,7 +191,7 @@ namespace amr_control
 
         for (auto edge : order.edges)
         {
-          vda5050_msgs::Edge edge_msg;
+          robot_protocol_msgs::Edge edge_msg;
           edge_msg.edgeId = edge.edgeId;
           edge_msg.sequenceId = edge.sequenceId;
           edge_msg.edgeDescription = edge.edgeDescription;
@@ -207,7 +212,7 @@ namespace amr_control
 
           for (auto controlPoint : edge.trajectory.controlPoints)
           {
-            vda5050_msgs::ControlPoint controlPoint_msg;
+            robot_protocol_msgs::ControlPoint controlPoint_msg;
             controlPoint_msg.x = controlPoint.x;
             controlPoint_msg.y = controlPoint.y;
             controlPoint_msg.weight = controlPoint.weight;
@@ -218,43 +223,46 @@ namespace amr_control
           order_msg.edges.push_back(edge_msg);
         }
 
-        // order_msg.edges zoneSetId =VDA5050ClientAPI::client_ptr_->vda5050_order_.
-        VDA5050ClientAPI::plan_pub_.publish(order_msg);
-
         vda_5050::NodePosition position = order.nodes.back().nodePosition;
-        goal.header.stamp = ros::Time::now();
+        goal.header.stamp = robot::Time::now();
         goal.header.frame_id = "map";
         goal.pose.position.x = position.x;
         goal.pose.position.y = position.y;
-        tf::Quaternion quat;
+        goal.pose.position.z = 0.0;
+
+        // Convert theta (yaw) to Quaternion using setRPY
+        // setRPY(roll, pitch, yaw) - for 2D navigation, only yaw is used
+        tf3::Quaternion quat;
         quat.setRPY(0.0, 0.0, position.theta);
-        tf::quaternionTFToMsg(quat, goal.pose.orientation);
+        goal.pose.orientation.x = quat.x();
+        goal.pose.orientation.y = quat.y();
+        goal.pose.orientation.z = quat.z();
+        goal.pose.orientation.w = quat.w();
+
+        VDA5050ClientAPI::move_base_ptr_->moveTo(order_msg, goal);
       }
       else
         return;
 
-      ros::Duration(0.1).sleep();
-      VDA5050ClientAPI::move_base_ptr_->moveTo(goal);
       cmd_vel_max_.x = 0.2;
       cmd_vel_max_.theta = 0.5;
       cmd_vel_max_saved_ = cmd_vel_max_;
 
-      geometry_msgs::Vector3 linear;
+      robot_geometry_msgs::Vector3 linear;
       linear.x = cmd_vel_max_.x;
       VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);
       linear.x = -cmd_vel_max_.x;
       VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);
 
-      geometry_msgs::Vector3 angular;
+      robot_geometry_msgs::Vector3 angular;
       angular.z = cmd_vel_max_.theta;
       VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular);
- 
     }
   }
 
   void VDA5050ClientAPI::moveToDock(vda_5050::Order order, uint8_t &status, std::string &message)
   {
-    geometry_msgs::PoseStamped goal;
+    robot_geometry_msgs::PoseStamped goal;
     if (VDA5050ClientAPI::move_base_ptr_)
     {
       if (global_plan_msg_type_ == std::string("nav_msgs::Path"))
@@ -288,9 +296,9 @@ namespace amr_control
           p_2d.theta = p.getYaw();
           poses.push_back(p_2d);
         }
-        const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now());
-        goal = path.poses.back();
-        VDA5050ClientAPI::plan_pub_.publish(path);
+        // const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now());
+        // goal = path.poses.back();
+        // VDA5050ClientAPI::plan_pub_.publish(path);
       }
       else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order"))
       {
@@ -359,13 +367,13 @@ namespace amr_control
         VDA5050ClientAPI::plan_pub_.publish(order_msg);
 
         vda_5050::NodePosition position = order.nodes.back().nodePosition;
-        goal.header.stamp = ros::Time::now();
+        goal.header.stamp = robot::Time::now();
         goal.header.frame_id = "map";
         goal.pose.position.x = position.x;
         goal.pose.position.y = position.y;
-        tf::Quaternion quat;
-        quat.setRPY(0.0, 0.0, position.theta);
-        tf::quaternionTFToMsg(quat, goal.pose.orientation);
+        // tf3::Transform transform;
+        // transform.setRotation(tf3::Quaternion(0.0, 0.0, position.theta));
+        // goal = tf3::toMsg(transform);
       }
       else
         return;
@@ -376,35 +384,32 @@ namespace amr_control
       cmd_vel_max_.theta = 0.5;
       cmd_vel_max_saved_ = cmd_vel_max_;
 
-      geometry_msgs::Vector3 linear;
+      robot_geometry_msgs::Vector3 linear;
       linear.x = cmd_vel_max_.x;
       VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);
       linear.x = -cmd_vel_max_.x;
       VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);
 
-      geometry_msgs::Vector3 angular;
+      robot_geometry_msgs::Vector3 angular;
       angular.z = cmd_vel_max_.theta;
       VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular);
-
-
     }
   }
 
   void VDA5050ClientAPI::rotateTo(vda_5050::Order order, uint8_t &status, std::string &message)
   {
-    geometry_msgs::PoseStamped goal;
-
+    robot_geometry_msgs::PoseStamped goal;
     if (VDA5050ClientAPI::move_base_ptr_)
     {
       if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(goal))
       {
         vda_5050::NodePosition position = order.nodes.back().nodePosition;
-        goal.header.stamp = ros::Time::now();
+        goal.header.stamp = robot::Time::now();
         goal.header.frame_id = "map";
-        tf::Quaternion quat;
-        quat.setRPY(0.0, 0.0, position.theta);
-        tf::quaternionTFToMsg(quat, goal.pose.orientation);
-        VDA5050ClientAPI::move_base_ptr_->rotateTo(goal);
+        // tf3::Transform transform;
+        // transform.setRotation(tf3::Quaternion(0.0, 0.0, position.theta));
+        // goal = tf3::toMsg(transform);
+        // VDA5050ClientAPI::move_base_ptr_->rotateTo(goal);
       }
       else
         return;
@@ -521,10 +526,10 @@ namespace amr_control
 
   void VDA5050ClientAPI::updateVelocity(double velocity)
   {
-    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_)
+    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
     {
-      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state;
-      if (nav_state == move_base_core::State::CONTROLLING)
+      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
+      if (nav_state == robot::move_base_core::State::CONTROLLING)
       {
         if (fabs(velocity) > 2.0)
           velocity = (fabs(velocity) / velocity) * 2.0;
@@ -541,10 +546,10 @@ namespace amr_control
 
   void VDA5050ClientAPI::updateAngular(double angular)
   {
-    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_)
+    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
     {
-      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state;
-      if (nav_state == move_base_core::State::CONTROLLING)
+      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
+      if (nav_state == robot::move_base_core::State::CONTROLLING)
       {
         if (fabs(angular) > 1.0)
           angular = (fabs(angular) / angular) * 1.0;
@@ -645,7 +650,7 @@ namespace amr_control
 
         // Can runing actions be interrupted ?
         if (VDA5050ClientAPI::move_base_ptr_ != nullptr &&
-            VDA5050ClientAPI::move_base_ptr_->nav_feedback_ != nullptr)
+            VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
         {
           VDA5050ClientAPI::move_base_ptr_->cancel();
           *this->cancel_action_ = true;
@@ -653,7 +658,7 @@ namespace amr_control
           *this->pause_action_ = false;
           ros::Rate r(5);
           while (ros::ok() &&
-                 VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state == move_base_core::State::CONTROLLING)
+                 VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state == robot::move_base_core::State::CONTROLLING)
           {
             ROS_INFO_THROTTLE(1.0, "Waiting to cancel");
             r.sleep();
@@ -714,18 +719,18 @@ namespace amr_control
 
   void VDA5050ClientAPI::unDockFromStation(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
   {
-    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_)
+    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
     {
       action_state->actionStatus = vda_5050::ActionState::ActionStatus::INITIALIZING;
       ros::Rate r(5);
-      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state;
-      while (ros::ok() && !*this->cancel_action_ && nav_state == move_base_core::State::CONTROLLING)
+      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
+      while (ros::ok() && !*this->cancel_action_ && nav_state == robot::move_base_core::State::CONTROLLING)
       {
         r.sleep();
         ros::spinOnce();
       }
 
-      geometry_msgs::PoseStamped pose;
+      robot_geometry_msgs::PoseStamped pose;
       if (!VDA5050ClientAPI::move_base_ptr_->getRobotPose(pose))
       {
         action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
@@ -808,9 +813,9 @@ namespace amr_control
         return;
       }
 
-      geometry_msgs::PoseStamped goal = move_base_core::offset_goal(pose, distance);
+      robot_geometry_msgs::PoseStamped goal = robot::move_base_core::offset_goal(pose, distance);
       VDA5050ClientAPI::move_base_ptr_->moveStraightTo(goal);
-      geometry_msgs::Vector3 linear;
+      robot_geometry_msgs::Vector3 linear;
       linear.x = fabs(velocity);
       VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);
       linear.x = -fabs(velocity);
@@ -818,7 +823,7 @@ namespace amr_control
 
       while (ros::ok() && !*this->cancel_action_)
       {
-        if (nav_state == move_base_core::State::CONTROLLING)
+        if (nav_state == robot::move_base_core::State::CONTROLLING)
           break;
         r.sleep();
         ros::spinOnce();
@@ -827,15 +832,15 @@ namespace amr_control
       while (ros::ok() && !*this->cancel_action_)
       {
         action_state->actionStatus = vda_5050::ActionState::ActionStatus::RUNNING;
-        if (nav_state == move_base_core::State::SUCCEEDED ||
-            nav_state == move_base_core::State::ABORTED ||
-            nav_state == move_base_core::State::PREEMPTED)
+        if (nav_state == robot::move_base_core::State::SUCCEEDED ||
+            nav_state == robot::move_base_core::State::ABORTED ||
+            nav_state == robot::move_base_core::State::PREEMPTED)
           break;
         r.sleep();
         ros::spinOnce();
       }
 
-      if (nav_state == move_base_core::State::SUCCEEDED)
+      if (nav_state == robot::move_base_core::State::SUCCEEDED)
       {
         action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED;
         action_state->resultDescription = "Done";
@@ -851,11 +856,11 @@ namespace amr_control
 
   void VDA5050ClientAPI::pickUp(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
   {
-    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_)
+    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
     {
       ros::Rate r(5);
-      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state;
-      while (ros::ok() && nav_state == move_base_core::State::CONTROLLING)
+      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
+      while (ros::ok() && nav_state == robot::move_base_core::State::CONTROLLING)
       {
         r.sleep();
         ros::spinOnce();
@@ -978,18 +983,18 @@ namespace amr_control
 
   void VDA5050ClientAPI::dockToStaton(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
   {
-    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_)
+    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
     {
       action_state->actionStatus = vda_5050::ActionState::ActionStatus::INITIALIZING;
       ros::Rate r(5);
-      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state;
-      while (ros::ok() && nav_state == move_base_core::State::CONTROLLING)
+      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
+      while (ros::ok() && nav_state == robot::move_base_core::State::CONTROLLING)
       {
         action_state->actionStatus = vda_5050::ActionState::ActionStatus::RUNNING;
         r.sleep();
         ros::spinOnce();
       }
-      if (nav_state == move_base_core::State::SUCCEEDED)
+      if (nav_state == robot::move_base_core::State::SUCCEEDED)
       {
         action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED;
         action_state->resultDescription = "Done";
@@ -1003,23 +1008,23 @@ namespace amr_control
   void VDA5050ClientAPI::unLoad(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
   {
     ROS_INFO("UnLoad running");
-    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_)
+    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
     {
       ros::Rate r(5);
-      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state;
+      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
       while (ros::ok() && !*enable_action_ && !*this->cancel_action_)
       {
-        if (nav_state == move_base_core::State::ABORTED ||
-            nav_state == move_base_core::State::REJECTED)
+        if (nav_state == robot::move_base_core::State::ABORTED ||
+            nav_state == robot::move_base_core::State::REJECTED)
         {
           if (action_state != nullptr)
           {
             action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
-            action_state->resultDescription = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->feed_back_str;
+            action_state->resultDescription = VDA5050ClientAPI::move_base_ptr_->getFeedback()->feed_back_str;
           }
           return;
         }
-        if (nav_state == move_base_core::State::SUCCEEDED)
+        if (nav_state == robot::move_base_core::State::SUCCEEDED)
           break;
         r.sleep();
       }
@@ -1081,23 +1086,23 @@ namespace amr_control
   void VDA5050ClientAPI::load(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
   {
     ROS_INFO("Load running");
-    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_)
+    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
     {
       ros::Rate r(5);
-      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state;
+      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
       while (ros::ok() && !*enable_action_ && !*this->cancel_action_)
       {
-        if (nav_state == move_base_core::State::ABORTED ||
-            nav_state == move_base_core::State::REJECTED)
+        if (nav_state == robot::move_base_core::State::ABORTED ||
+            nav_state == robot::move_base_core::State::REJECTED)
         {
           if (action_state != nullptr)
           {
             action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
-            action_state->resultDescription = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->feed_back_str;
+            action_state->resultDescription = VDA5050ClientAPI::move_base_ptr_->getFeedback()->feed_back_str;
           }
           return;
         }
-        if (nav_state == move_base_core::State::SUCCEEDED)
+        if (nav_state == robot::move_base_core::State::SUCCEEDED)
           break;
         r.sleep();
       }
@@ -1185,7 +1190,7 @@ namespace amr_control
     auto &global_visualization = this->client_ptr_->vda5050_visualization_;
     if (VDA5050ClientAPI::move_base_ptr_)
     {
-      geometry_msgs::Pose2D robot_pose;
+      robot_geometry_msgs::Pose2D robot_pose;
       if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(robot_pose))
       {
         global_visualization.agvPosition.x = robot_pose.x;
@@ -1211,9 +1216,9 @@ namespace amr_control
     std::lock_guard<std::mutex> lock(this->client_ptr_->state_mutex);
     auto &global_state = this->client_ptr_->vda5050_state_;
     if (VDA5050ClientAPI::move_base_ptr_ != nullptr &&
-        VDA5050ClientAPI::move_base_ptr_->nav_feedback_ != nullptr)
+        VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
     {
-      geometry_msgs::Pose2D robot_pose;
+      robot_geometry_msgs::Pose2D robot_pose;
       if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(robot_pose))
       {
         global_state.agvPosition.x = robot_pose.x;
@@ -1221,14 +1226,14 @@ namespace amr_control
         global_state.agvPosition.theta = robot_pose.theta;
       }
 
-      move_base_core::State nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state;
-      if (nav_state == move_base_core::State::CONTROLLING)
+      robot::move_base_core::State nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
+      if (nav_state == robot::move_base_core::State::CONTROLLING)
       {
         global_state.driving = true;
         global_state.paused = false;
         global_state.newBaseRequest = false;
       }
-      else if (nav_state == move_base_core::State::PAUSED)
+      else if (nav_state == robot::move_base_core::State::PAUSED)
       {
         global_state.driving = false;
         global_state.paused = true;
@@ -1254,7 +1259,7 @@ namespace amr_control
     *this->pause_action_ = false;
   }
 
-  std::shared_ptr<move_base_core::BaseNavigation> VDA5050ClientAPI::move_base_ptr_ = nullptr;
+  std::shared_ptr<robot::move_base_core::BaseNavigation> VDA5050ClientAPI::move_base_ptr_ = nullptr;
   std::shared_ptr<loc_core::BaseLocalization> VDA5050ClientAPI::loc_base_ptr_ = nullptr;
   std::shared_ptr<AmrMonitor> VDA5050ClientAPI::monitor_ptr_ = nullptr;
 }

Controllers/Packages/amr_control/src/sensor_converter.cpp

@@ -0,0 +1,132 @@
+#include "amr_control/sensor_converter.h"
+
+amr_control::SensorConverter::SensorConverter(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr)
+  : nh_(nh), move_base_ptr_(move_base_ptr)
+{
+    map_server_sub_ = nh_.subscribe("/map", 1, &SensorConverter::mapServerCallback, this);
+    laser_f_scan_sub_ = nh_.subscribe("/f_scan", 1, &SensorConverter::laserFScanCallback, this);
+    laser_b_scan_sub_ = nh_.subscribe("/b_scan", 1, &SensorConverter::laserBScanCallback, this);
+}
+
+amr_control::SensorConverter::~SensorConverter()
+{
+    map_server_sub_.shutdown();
+    laser_f_scan_sub_.shutdown();
+    laser_b_scan_sub_.shutdown();
+}
+
+void amr_control::SensorConverter::requestMapFromServer(ros::NodeHandle &nh)
+{
+  // Thử request map từ service trước (nếu map server có service)
+  ros::ServiceClient map_client = nh.serviceClient<nav_msgs::GetMap>("/static_map");
+  
+  if (map_client.exists())
+  {
+    nav_msgs::GetMap srv;
+    if (map_client.call(srv))
+    {
+      robot::log_info("[%s:%d] Got map from service", __FILE__, __LINE__);
+      // Convert và add map
+      convertAndAddMap(srv.response.map);
+      return;
+    }
+    else
+    {
+      robot::log_warning("[%s:%d] Service call failed, trying to wait for message...", __FILE__, __LINE__);
+    }
+  }
+  else
+  {
+    robot::log_info("[%s:%d] Map service not available, waiting for message on /map topic...", __FILE__, __LINE__);
+  }
+  
+  // Nếu không có service hoặc service call failed, đợi message từ topic
+  nav_msgs::OccupancyGridConstPtr map_msg = ros::topic::waitForMessage<nav_msgs::OccupancyGrid>("/map", nh, ros::Duration(5.0));
+  if (map_msg)
+  {
+    robot::log_info("[%s:%d] Got map from topic", __FILE__, __LINE__);
+    convertAndAddMap(*map_msg);
+  }
+  else
+  {
+    robot::log_warning("[%s:%d] No map received after waiting 5 seconds", __FILE__, __LINE__);
+  }
+}
+
+void amr_control::SensorConverter::convertAndAddMap(const nav_msgs::OccupancyGrid &nav_map)
+{
+  if (move_base_ptr_ != nullptr)
+  {
+    robot_nav_msgs::OccupancyGrid map;
+    map.header.frame_id = nav_map.header.frame_id;
+    map.header.stamp = robot::Time::now();
+    map.header.seq = nav_map.header.seq;
+
+    map.info.map_load_time = robot::Time::now();
+    map.info.width = nav_map.info.width;
+    map.info.height = nav_map.info.height;
+    map.info.resolution = nav_map.info.resolution;
+    map.info.origin.position.x = nav_map.info.origin.position.x;
+    map.info.origin.position.y = nav_map.info.origin.position.y;
+    map.info.origin.position.z = nav_map.info.origin.position.z;
+    map.info.origin.orientation.x = nav_map.info.origin.orientation.x;
+    map.info.origin.orientation.y = nav_map.info.origin.orientation.y;
+    map.info.origin.orientation.z = nav_map.info.origin.orientation.z;
+    map.info.origin.orientation.w = nav_map.info.origin.orientation.w;
+    
+    map.data = nav_map.data;
+    move_base_ptr_->addStaticMap("/map", map);
+  }
+}
+
+void amr_control::SensorConverter::mapServerCallback(const nav_msgs::OccupancyGrid::ConstPtr &msg)
+{
+  robot::log_info("[%s:%d]\n Adding static map to move_base from callback... %p", __FILE__, __LINE__, move_base_ptr_.get());
+  convertAndAddMap(*msg);
+}
+
+void amr_control::SensorConverter::laserFScanCallback(const sensor_msgs::LaserScan::ConstPtr &msg)
+{
+  if (move_base_ptr_ != nullptr && msg)
+  {
+    // Convert sensor_msgs::LaserScan to robot_sensor_msgs::LaserScan
+    robot_sensor_msgs::LaserScan robot_scan;
+    robot_scan.header.frame_id = msg->header.frame_id;
+    robot_scan.header.stamp = robot::Time(msg->header.stamp.toSec());
+    robot_scan.header.seq = msg->header.seq;
+    robot_scan.angle_min = msg->angle_min;
+    robot_scan.angle_max = msg->angle_max;
+    robot_scan.angle_increment = msg->angle_increment;
+    robot_scan.time_increment = msg->time_increment;
+    robot_scan.scan_time = msg->scan_time;
+    robot_scan.range_min = msg->range_min;
+    robot_scan.range_max = msg->range_max;
+    robot_scan.ranges = msg->ranges;
+    robot_scan.intensities = msg->intensities;
+    
+    move_base_ptr_->addLaserScan("/f_scan", robot_scan);
+  }
+}
+
+void amr_control::SensorConverter::laserBScanCallback(const sensor_msgs::LaserScan::ConstPtr &msg)
+{
+  if (move_base_ptr_ != nullptr && msg)
+  {
+    // Convert sensor_msgs::LaserScan to robot_sensor_msgs::LaserScan
+    robot_sensor_msgs::LaserScan robot_scan;
+    robot_scan.header.frame_id = msg->header.frame_id;
+    robot_scan.header.stamp = robot::Time(msg->header.stamp.toSec());
+    robot_scan.header.seq = msg->header.seq;
+    robot_scan.angle_min = msg->angle_min;
+    robot_scan.angle_max = msg->angle_max;
+    robot_scan.angle_increment = msg->angle_increment;
+    robot_scan.time_increment = msg->time_increment;
+    robot_scan.scan_time = msg->scan_time;
+    robot_scan.range_min = msg->range_min;
+    robot_scan.range_max = msg->range_max;
+    robot_scan.ranges = msg->ranges;
+    robot_scan.intensities = msg->intensities;
+    
+    move_base_ptr_->addLaserScan("/b_scan", robot_scan);
+  }
+}

Controllers/Packages/amr_control/src/tf_converter.cpp

@@ -0,0 +1,269 @@
+#include "amr_control/tf_converter.h"
+
+amr_control::TfConverter::TfConverter(std::shared_ptr<tf2_ros::Buffer> tf)
+  : tf_(tf)
+{
+  tf3_buffer_ = std::make_shared<tf3::BufferCore>();
+  tf3_buffer_->setUsingDedicatedThread(true);
+  tf_thread_ = std::thread(&amr_control::TfConverter::tfWorker, this);
+}
+
+amr_control::TfConverter::~TfConverter()
+{
+  stop_tf_thread_ = true;
+  tf_thread_.join();
+}
+void amr_control::TfConverter::tfWorker()
+{
+  struct TFEdge {
+    std::string parent;
+    std::string child;
+  }; 
+  std::vector<TFEdge> edges;
+  tf2_ros::Buffer tfBuffer;
+  tf2_ros::TransformListener tfListener(tfBuffer);
+
+  std::string last_tree;
+  std::string tree;
+  std::string line;
+
+  int count_tf_receive_done = 0;
+  ros::Rate rate(20);
+  while (ros::ok() && !stop_tf_thread_)
+  {
+    ros::spinOnce();
+    tree = tfBuffer.allFramesAsString();
+    if (!tree.empty() && tree == last_tree)
+    {
+      count_tf_receive_done++;
+    }
+    else
+    {
+      count_tf_receive_done = 0;
+    }
+
+    if(count_tf_receive_done > 2)
+    {
+      ROS_WARN("TF tree stabilized: \n%s", tree.c_str());
+      break;
+    }
+
+    last_tree = tree;
+    
+    rate.sleep();
+  }
+
+  std::istringstream iss(tree);
+
+  while (ros::ok() && std::getline(iss, line) && !stop_tf_thread_)
+  {
+      // Frame X exists with parent Y
+      std::size_t f1 = line.find("Frame ");
+      std::size_t f2 = line.find(" exists with parent ");
+
+      if (f1 != std::string::npos && f2 != std::string::npos)
+      {
+          std::string child  = line.substr(f1 + 6, f2 - (f1 + 6));
+          std::string parent = line.substr(f2 + 20);
+
+          if (!parent.empty() && parent.back() == '.')
+          parent.pop_back();
+
+          edges.push_back({parent, child});
+          ROS_WARN("parent: %s | child: %s", parent.c_str(), child.c_str());
+      }
+  }
+
+  while (ros::ok() && !stop_tf_thread_)
+  {
+    if (!tf_ || !tf3_buffer_)
+    {
+      rate.sleep();
+      continue;
+    }
+
+    try
+    {
+      for (const auto& e : edges)
+      {
+        try
+        {
+          if (tf_->canTransform(e.parent, e.child, ros::Time(0)))
+          { 
+            auto transform =
+            tf_->lookupTransform(e.parent, e.child, ros::Time(0));
+            
+            tf3::TransformStampedMsg t;
+            t.header.stamp = tf3::Time::now();
+            t.header.frame_id = transform.header.frame_id;
+            t.child_frame_id = transform.child_frame_id;
+            t.transform.translation.x = transform.transform.translation.x;
+            t.transform.translation.y = transform.transform.translation.y;
+            t.transform.translation.z = transform.transform.translation.z;
+            t.transform.rotation.x = transform.transform.rotation.x;
+            t.transform.rotation.y = transform.transform.rotation.y;
+            t.transform.rotation.z = transform.transform.rotation.z;
+            t.transform.rotation.w = transform.transform.rotation.w;
+
+            {
+              std::lock_guard<std::mutex> lock(tf3_mutex_);
+              tf3_buffer_->setTransform(t, "dynamic_tf");
+            }
+          }
+        }
+        catch (tf2::TransformException &ex)
+        {
+            ROS_WARN("TF %s -> %s failed: %s",
+                    e.parent.c_str(),
+                    e.child.c_str(),
+                    ex.what());
+        }
+      }
+      // ROS_INFO("TF worker thread running");
+    }
+    catch (const std::exception& e)
+    {
+      ROS_WARN("TF worker exception: %s", e.what());
+    }
+
+    rate.sleep();
+  }
+
+  ROS_INFO("TF worker thread cleanly exited");
+}
+
+void amr_control::TfConverter::TfBuffer(std::shared_ptr<tf3::BufferCore> &tf_buffer)
+{
+  tf_buffer = tf3_buffer_;
+}
+
+void amr_control::TfConverter::checkTfCoreData(const std::vector<std::pair<std::string, std::string>>& check_frames, robot::TFListenerPtr &tf_core_ptr)
+{
+  if (!tf_core_ptr)
+  {
+    robot::log_warning("[%s:%d] tf_core_ptr_ is nullptr", __FILE__, __LINE__);
+    return;
+  }
+
+  robot::log_info("[%s:%d] ========== Checking tf_core_ptr_ Data ==========", __FILE__, __LINE__);
+  
+  // In ra tất cả frames có trong buffer
+  try
+  {
+    std::string all_frames = tf_core_ptr->allFramesAsString();
+    if (!all_frames.empty())
+    {
+      robot::log_info("[%s:%d] All frames in tf_core_ptr_:\n%s", __FILE__, __LINE__, all_frames.c_str());
+    }
+    else
+    {
+      robot::log_warning("[%s:%d] No frames found in tf_core_ptr_", __FILE__, __LINE__);
+    }
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_error("[%s:%d] Exception getting all frames: %s", __FILE__, __LINE__, ex.what());
+  }
+
+  // Kiểm tra các frame pairs được chỉ định
+  if (!check_frames.empty())
+  {
+    robot::log_info("[%s:%d] Checking specific frame pairs:", __FILE__, __LINE__);
+    for (const auto& frame_pair : check_frames)
+    {
+      const std::string& target_frame = frame_pair.first;
+      const std::string& source_frame = frame_pair.second;
+      
+      try
+      {
+        std::string error_msg;
+        bool can_transform = tf_core_ptr->canTransform(target_frame, source_frame, tf3::Time(), &error_msg);
+        
+        if (can_transform)
+        {
+          try
+          {
+            tf3::TransformStampedMsg transform = tf_core_ptr->lookupTransform(target_frame, source_frame, tf3::Time());
+            robot::log_info("[%s:%d] Transform [%s] -> [%s]:", __FILE__, __LINE__, 
+                           source_frame.c_str(), target_frame.c_str());
+            robot::log_info("  Translation: (%.3f, %.3f, %.3f)", 
+                           transform.transform.translation.x,
+                           transform.transform.translation.y,
+                           transform.transform.translation.z);
+            robot::log_info("  Rotation: (%.3f, %.3f, %.3f, %.3f)", 
+                           transform.transform.rotation.x,
+                           transform.transform.rotation.y,
+                           transform.transform.rotation.z,
+                           transform.transform.rotation.w);
+            robot::log_info("  Stamp: %.3f", transform.header.stamp.toSec());
+          }
+          catch (const tf3::LookupException& ex)
+          {
+            robot::log_error("[%s:%d] LookupException for [%s] -> [%s]: %s", 
+                           __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), ex.what());
+          }
+          catch (const tf3::ConnectivityException& ex)
+          {
+            robot::log_error("[%s:%d] ConnectivityException for [%s] -> [%s]: %s", 
+                           __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), ex.what());
+          }
+          catch (const tf3::ExtrapolationException& ex)
+          {
+            robot::log_error("[%s:%d] ExtrapolationException for [%s] -> [%s]: %s", 
+                           __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), ex.what());
+          }
+        }
+        else
+        {
+          robot::log_warning("[%s:%d] Cannot transform [%s] -> [%s]: %s", 
+                           __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), error_msg.c_str());
+        }
+      }
+      catch (const std::exception& ex)
+      {
+        robot::log_error("[%s:%d] Exception checking transform [%s] -> [%s]: %s", 
+                       __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), ex.what());
+      }
+    }
+  }
+  else
+  {
+    // Nếu không có frame pairs được chỉ định, kiểm tra một số frame phổ biến
+    std::vector<std::pair<std::string, std::string>> common_frames = {
+      {"map", "base_footprint"},
+      {"odom", "base_footprint"},
+      {"base_footprint", "base_link"}
+    };
+    
+    robot::log_info("[%s:%d] Checking common frame pairs:", __FILE__, __LINE__);
+    for (const auto& frame_pair : common_frames)
+    {
+      const std::string& target_frame = frame_pair.first;
+      const std::string& source_frame = frame_pair.second;
+      
+      try
+      {
+        std::string error_msg;
+        bool can_transform = tf_core_ptr->canTransform(target_frame, source_frame, tf3::Time(), &error_msg);
+        
+        if (can_transform)
+        {
+          robot::log_info("[%s:%d] ✓ Transform available: [%s] -> [%s]", 
+                         __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str());
+        }
+        else
+        {
+          robot::log_info("[%s:%d] ✗ Transform NOT available: [%s] -> [%s] (%s)", 
+                         __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), error_msg.c_str());
+        }
+      }
+      catch (const std::exception& ex)
+      {
+        robot::log_warning("[%s:%d] Exception checking [%s] -> [%s]: %s", 
+                         __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), ex.what());
+      }
+    }
+  }
+  
+  robot::log_info("[%s:%d] ========== End Checking tf_core_ptr ==========", __FILE__, __LINE__);
+}

Controllers/Packages/amr_startup/CMakeLists.txt

@@ -0,0 +1,205 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(amr_startup)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## 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 amr_startup
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/amr_startup.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(${PROJECT_NAME}_node src/amr_startup_node.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(${PROJECT_NAME}_node
+#   ${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
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_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(DIRECTORY
+    config
+    launch
+    rviz
+    sdf
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  )
+  
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_amr_startup.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)

Controllers/Packages/amr_startup/config/base_local_planner_params.yaml

@@ -0,0 +1,48 @@
+#For full documentation of the parameters in this file, and a list of all the
+#parameters available for TrajectoryPlannerROS, please see
+#http://www.ros.org/wiki/base_local_planner
+TrajectoryPlannerROS:
+  #Set the acceleration limits of the robot
+  acc_lim_th: 3.2 
+  acc_lim_x: 0.5
+  acc_lim_y: 0.5
+
+  #Set the velocity limits of the robot
+  max_vel_x: 0.4
+  min_vel_x: 0.1
+  max_rotational_vel: 1.0
+  min_in_place_rotational_vel: 0.4
+
+  #The velocity the robot will command when trying to escape from a stuck situation
+  escape_vel: -0.2
+  
+  #For this example, we'll use a holonomic robot
+  holonomic_robot: false
+
+  #Since we're using a holonomic robot, we'll set the set of y velocities it will sample
+  y_vels: [-0.3, -0.1, 0.1, 0.3]
+
+  #Set the tolerance on achieving a goal
+  xy_goal_tolerance: 0.1
+  yaw_goal_tolerance: 0.05
+
+  #We'll configure how long and with what granularity we'll forward simulate trajectories
+  sim_time: 3.0
+  sim_granularity: 0.025
+  vx_samples: 5
+  vtheta_samples: 20
+
+  #Parameters for scoring trajectories
+  goal_distance_bias: 0.4
+  path_distance_bias: 0.7
+  occdist_scale: 0.3
+  heading_lookahead: 0.325
+
+  #We'll use the Dynamic Window Approach to control instead of Trajectory Rollout for this example
+  dwa: false
+
+  #How far the robot must travel before oscillation flags are reset
+  oscillation_reset_dist: 0.01
+
+  #Eat up the plan as the robot moves along it
+  prune_plan: false

Controllers/Packages/amr_startup/config/costmap_common_params.yaml

@@ -0,0 +1,54 @@
+robot_base_frame: base_footprint
+transform_tolerance: 1.0
+obstacle_range: 3.0
+#mark_threshold: 1
+publish_voxel_map: true
+footprint_padding: 0.0
+
+navigation_map:
+  map_topic: /map
+  map_pkg: managerments
+  map_file: maze
+
+virtual_walls_map:
+  map_topic: /virtual_walls/map
+  namespace: /virtual_walls
+  map_pkg: managerments
+  map_file: maze
+  use_maximum: true
+  lethal_cost_threshold: 100
+
+obstacles:
+  observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
+  f_scan_marking:
+    topic: /f_scan
+    data_type: LaserScan
+    clearing: false
+    marking: true
+    inf_is_valid: false
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25
+  f_scan_clearing:
+    topic: /f_scan
+    data_type: LaserScan
+    clearing: true
+    marking: false
+    inf_is_valid: false
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25
+  b_scan_marking:
+    topic: /b_scan
+    data_type: LaserScan
+    clearing: false
+    marking: true
+    inf_is_valid: false
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25
+  b_scan_clearing:
+    topic: /b_scan
+    data_type: LaserScan
+    clearing: true
+    marking: false
+    inf_is_valid: false
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25

Controllers/Packages/amr_startup/config/costmap_global_params.yaml

@@ -0,0 +1,13 @@
+global_costmap:
+    global_frame: map
+    update_frequency: 1.0
+    publish_frequency: 1.0
+    raytrace_range: 2.0
+    resolution: 0.05
+    z_resolution: 0.2
+    rolling_window: false
+    z_voxels: 10
+    inflation:
+      cost_scaling_factor:  10.0  # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius.
+      inflation_radius:     0.6  # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.
+

Controllers/Packages/amr_startup/config/costmap_global_params_plugins_no_virtual_walls.yaml

@@ -0,0 +1,11 @@
+global_costmap:
+    library_path: libplugins
+    frame_id: map
+    plugins:
+        - {name: navigation_map, type: "costmap_2d::StaticLayer" }
+        - {name: virtual_walls_map, type: "costmap_2d::StaticLayer" }
+        - {name: obstacles,  type: "costmap_2d::VoxelLayer" }
+        - {name: inflation,  type: "costmap_2d::InflationLayer" }
+    obstacles:
+        enabled: false
+        footprint_clearing_enabled: false

Controllers/Packages/amr_startup/config/costmap_local_params.yaml

@@ -0,0 +1,16 @@
+local_costmap:  
+    global_frame: odom
+    update_frequency: 5.0
+    publish_frequency: 5.0
+    rolling_window: true
+    raytrace_range: 2.0
+    resolution: 0.05
+    z_resolution: 0.15
+    z_voxels: 8
+    inflation:
+      cost_scaling_factor: 10.0  # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius.
+      inflation_radius: 0.3  # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.
+    width: 8.0
+    height: 8.0
+    origin_x: 0.0
+    origin_y: 0.0

Controllers/Packages/amr_startup/config/costmap_local_params_plugins_no_virtual_walls.yaml

@@ -0,0 +1,9 @@
+local_costmap:
+    library_path: libplugins
+    global_frame: odom
+    plugins:
+        - {name: obstacles,  type: "VoxelLayer" }
+        - {name: inflation,  type: "InflationLayer" }
+    obstacles:
+        enabled: true
+        footprint_clearing_enabled: true

Controllers/Packages/amr_startup/config/custom_global_params.yaml

@@ -0,0 +1,17 @@
+base_global_planner: CustomPlanner
+CustomPlanner:
+  environment_type: XYThetaLattice
+  planner_type: ARAPlanner
+  allocated_time: 10.0
+  initial_epsilon: 1.0
+  force_scratch_limit: 10000
+  forward_search: false
+  nominalvel_mpersecs: 0.8
+  timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s
+  allow_unknown: true
+  directory_to_save_paths: "/init/paths"
+  pathway_filename: "pathway.txt"
+  current_pose_topic_name: "/amcl_pose"
+  map_frame_id: "map"
+  base_frame_id: "base_link"
+  

Controllers/Packages/amr_startup/config/dwa_local_planner_params.yaml

@@ -0,0 +1,55 @@
+base_local_planner: dwa_local_planner/DWAPlannerROS
+DWAPlannerROS:
+  # Robot configuration
+  max_vel_x:  0.8
+  min_vel_x: -0.2
+
+  max_vel_y: 0.0  # diff drive robot
+  min_vel_y: 0.0  # diff drive robot
+
+  max_trans_vel: 0.8  # choose slightly less than the base's capability
+  min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
+  trans_stopped_vel: 0.03
+
+  # 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: 1.0  # choose slightly less than the base's capability
+  min_rot_vel: 0.1  # this is the min angular velocity when there is negligible translational velocity
+  rot_stopped_vel: 0.1
+
+  acc_lim_x: 1.5
+  acc_lim_y: 0.0      # diff drive robot
+  acc_limit_trans: 1.5
+  acc_lim_theta: 2.0
+
+  # Goal tolerance
+  yaw_goal_tolerance: 0.03  # yaw_goal_tolerance > (sim_time * min_rot_vel) / 2 (from Navigation Tuning Guide)
+  xy_goal_tolerance: 0.08   # xy_goal_tolerance  > (sim_time * min_vel_x)   / 2
+  latch_xy_goal_tolerance: true
+
+  # Forward simulation
+  sim_time: 1.2
+  vx_samples: 15
+  vy_samples: 1       # diff drive robot, there is only one sample
+  vtheta_samples: 20
+
+  # Trajectory scoring
+  path_distance_bias: 64.0      # default: 32.0  mir: 32.0  - weighting for how much it should stick to the global path plan
+  goal_distance_bias: 12.0      # default: 24.0  mir: 48.0  - weighting for how much it should attempt to reach its goal
+  occdist_scale: 0.5            # default: 0.01  mir: 0.01  - weighting for how much the controller should avoid obstacles
+  forward_point_distance: 0.325 # default: 0.325 mir: 0.325 - how far along to place an additional scoring point
+  stop_time_buffer: 0.2         # default: 0.2   mir: 0.2   - amount of time a robot must stop before colliding for a valid traj.
+  scaling_speed: 0.25           # default: 0.25  mir: 0.25  - absolute velocity at which to start scaling the robot's footprint
+  max_scaling_factor: 0.2       # default: 0.2   mir: 0.2   - how much to scale the robot's footprint when at speed.
+  prune_plan: true
+
+  # Oscillation prevention
+  oscillation_reset_dist: 0.05  # 0.05   - how far to travel before resetting oscillation flags, in m
+  oscillation_reset_angle: 0.2  # 0.2    - the angle the robot must turn before resetting Oscillation flags, in rad
+
+  # Debugging
+  publish_traj_pc : true
+  publish_cost_grid_pc: true
+  global_frame_id: /odom  # or <robot namespace>/odom

Controllers/Packages/amr_startup/config/ekf.yaml

@@ -0,0 +1,217 @@
+# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
+# computation until it receives at least one message from one of the inputs. It will then run continuously at the
+# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
+frequency: 50
+
+# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
+# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
+# filter will generate new output. Defaults to 1 / frequency if not specified.
+sensor_timeout: 0.1
+
+# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
+# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
+# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
+# by, for example, an IMU. Defaults to false if unspecified.
+two_d_mode: true
+
+# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
+# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
+# unspecified.
+transform_time_offset: 0.0
+
+# Use this parameter to specify how long the tf listener should wait for a transform to become available.
+# Defaults to 0.0 if unspecified.
+transform_timeout: 0.0
+
+# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
+# unhappy with any settings or data.
+print_diagnostics: true
+
+# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
+# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
+# effects on the performance of the node. Defaults to false if unspecified.
+debug: false
+
+# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
+debug_out_file: /path/to/debug/file.txt
+
+# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
+publish_tf: true
+
+# Whether to publish the acceleration state. Defaults to false if unspecified.
+publish_acceleration: false
+
+# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
+# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
+# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
+# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
+# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
+# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
+# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
+# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
+# Here is how to use the following settings:
+# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
+#     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
+#         odom_frame.
+# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
+#   "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
+# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
+# from landmark observations) then:
+#     3a. Set your "world_frame" to your map_frame value
+#     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
+#         estimation node from robot_localization! However, that instance should *not* fuse the global data.
+map_frame: map                                   # Defaults to "map" if unspecified
+odom_frame: $(arg tf_prefix)odom                 # Defaults to "odom" if unspecified
+base_link_frame: $(arg tf_prefix)base_footprint  # Defaults to "base_link" if unspecified
+world_frame: $(arg tf_prefix)odom                # Defaults to the value of odom_frame if unspecified
+
+# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
+# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
+# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
+# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
+# default values, and must be specified.
+odom0: odom
+
+# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
+# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
+# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
+# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
+# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
+# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
+# if unspecified, effectively making this parameter required for each sensor.
+# see http://docs.ros.org/melodic/api/robot_localization/html/configuring_robot_localization.html
+odom0_config: [false, false, false,   # x y z
+               false, false, false,   # roll pitch yaw
+               true,  true,  false,   # vx vy vz
+               false, false, true,    # vroll vpitch vyaw
+               false, false, false]   # ax ay az
+
+# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
+# the size of the subscription queue so that more measurements are fused.
+odom0_queue_size: 10
+
+# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
+# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
+# algorithm.
+odom0_nodelay: false
+
+# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
+# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
+# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
+# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
+# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
+# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
+# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
+# for twist measurements has no effect.
+odom0_differential: false
+
+# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
+# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
+# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
+# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
+odom0_relative: false
+
+# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
+# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
+# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
+# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
+# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
+# the thresholds.
+#odom0_pose_rejection_threshold: 5
+#odom0_twist_rejection_threshold: 1
+
+# Further input parameter examples
+# see http://docs.ros.org/melodic/api/robot_localization/html/configuring_robot_localization.html
+imu0: imu_data
+imu0_config: [false, false, false,   # x y z
+              false, false, true,    # roll pitch yaw
+              false, false, false,   # vx vy vz
+              false, false, true,    # vroll vpitch vyaw
+              true,  false, false]    # ax ay az
+imu0_nodelay: false
+imu0_differential: false
+imu0_relative: true
+imu0_queue_size: 10
+#imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
+#imu0_twist_rejection_threshold: 0.8                #
+#imu0_linear_acceleration_rejection_threshold: 0.8  #
+
+# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
+# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
+imu0_remove_gravitational_acceleration: false
+
+# [ADVANCED]  The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
+# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
+# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
+# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
+# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
+# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
+# for the velocity variable in question, or decrease the  variance of the variable in question in the measurement
+# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
+# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
+# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
+# inputs, the control term will be ignored.
+# Whether or not we use the control input during predicition. Defaults to false.
+use_control: false
+# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
+# false.
+stamped_control: false
+# The last issued control command will be used in prediction for this period. Defaults to 0.2.
+control_timeout: 0.2
+# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
+control_config: [true, false, false, false, false, true]
+# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
+acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
+# Acceleration and deceleration limits are not always the same for robots.
+deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
+# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
+# gains
+acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
+# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
+# gains
+deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
+
+# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
+# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
+# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
+# However, if users find that a given variable is slow to converge, one approach is to increase the
+# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
+# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
+# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
+# unspecified.
+process_noise_covariance: [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]
+
+# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
+# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
+# question. Users should take care not to use large values for variables that will not be measured directly. The values
+# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
+#if unspecified.
+initial_estimate_covariance: [100.0,0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    100.0,0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    1.0,  0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1.0,   0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     10.0, 0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    10.0, 0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]

Controllers/Packages/amr_startup/config/localization.yaml

@@ -0,0 +1,36 @@
+Amcl:
+  use_map_topic: true
+  odom_model_type: "diff-corrected"
+  gui_publish_rate: 5.0
+  save_pose_rate: 0.5
+  laser_max_beams: 300
+  laser_min_range: -1.0
+  laser_max_range: -1.0
+  min_particles: 1000
+  max_particles: 3000
+  kld_err: 0.05
+  kld_z: 0.99
+  odom_alpha1: 0.02
+  odom_alpha2: 0.01
+  odom_alpha3: 0.01
+  odom_alpha4: 0.02
+  laser_z_hit: 0.5
+  laser_z_short:       0.05
+  laser_z_max: 0.05
+  laser_z_rand: 0.5
+  laser_sigma_hit:     0.2
+  laser_lambda_short:  0.1
+  laser_model_type:    "likelihood_field"
+  laser_likelihood_max_dist: 1.0
+  update_min_d: 0.05
+  update_min_a: 0.05
+  odom_frame_id: odom
+  base_frame_id: base_footprint
+  global_frame_id: map
+  resample_interval: 1
+  transform_tolerance: 0.2
+  recovery_alpha_slow: 0.001
+  recovery_alpha_fast: 0.001
+  initial_pose_x: 0.0
+  initial_pose_y: 0.0
+  initial_pose_a: 0.0

Controllers/Packages/amr_startup/config/maker_sources.yaml

@@ -0,0 +1,120 @@
+maker_sources: trolley charger dock_station  undock_station
+trolley:
+  plugins:
+    - {name: 4legs,  docking_planner: "DockPlanner", docking_nav: ""}
+    - {name: qrcode, docking_planner: "TwoPointsPlanner", docking_nav: "" }
+
+  4legs:
+    maker_goal_frame: trolley_goal
+    footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
+    delay: 2.0
+    timeout: 60.0
+    vel_x: 0.1
+    vel_theta: 0.3
+    yaw_goal_tolerance: 0.017  
+    xy_goal_tolerance: 0.01 
+    min_lookahead_dist: 0.4
+    max_lookahead_dist: 1.0
+    lookahead_time: 1.5
+    angle_threshold: 0.36
+
+  qrcode:
+    maker_goal_frame: qr_trolley
+    delay: 2.0
+    timeout: 60.0
+    vel_x: 0.05
+    vel_theta: 0.2
+    allow_rotate: true
+    yaw_goal_tolerance: 0.017  
+    xy_goal_tolerance: 0.01 
+    min_lookahead_dist: 0.4
+    max_lookahead_dist: 1.0
+    lookahead_time: 1.5
+    angle_threshold: 0.36
+
+charger:
+  plugins:
+    - {name: charger, docking_planner: "DockPlanner", docking_nav: ""}
+
+  charger:
+    maker_goal_frame: charger_goal
+    footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
+    delay: 2
+    timeout: 60
+    vel_x: 0.1
+    yaw_goal_tolerance: 0.017  
+    xy_goal_tolerance: 0.01 
+    min_lookahead_dist: 0.4
+    max_lookahead_dist: 1.0
+    lookahead_time: 1.5
+    angle_threshold: 0.36
+
+dock_station:
+  plugins:
+  - {name: station, docking_planner: "DockPlanner", docking_nav: ""}
+
+  station:
+    maker_goal_frame: dock_station_goal
+    footprint: [[1.15,-0.55],[1.15,0.55],[-1.15,0.55],[-1.15,-0.55]]
+    delay: 2
+    timeout: 60
+    vel_x: 0.15
+    vel_theta: 0.3
+    yaw_goal_tolerance: 0.01  
+    xy_goal_tolerance: 0.01 
+    min_lookahead_dist: 0.4
+    max_lookahead_dist: 1.0
+    lookahead_time: 1.5
+    angle_threshold: 0.36
+
+
+dock_station_2:
+  plugins:
+  - {name: station, docking_planner: "DockPlanner", docking_nav: ""}
+
+  station:
+    maker_goal_frame: dock_station_goal_2
+    footprint: [[1.15,-0.55],[1.15,0.55],[-1.15,0.55],[-1.15,-0.55]]
+    delay: 2
+    timeout: 60
+    vel_x: 0.15
+    vel_theta: 0.3
+    yaw_goal_tolerance: 0.01  
+    xy_goal_tolerance: 0.01 
+    min_lookahead_dist: 0.4
+    max_lookahead_dist: 1.0
+    lookahead_time: 1.5
+    angle_threshold: 0.36
+
+undock_station:
+  plugins:
+    - {name: station,  docking_planner: "DockPlanner", docking_nav: ""}
+    - {name: qrcode, docking_planner: "TwoPointsPlanner", docking_nav: "" }
+
+  station:
+    maker_goal_frame: undock_station_goal
+    footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
+    delay: 2.0
+    timeout: 60.0
+    vel_x: 0.15
+    vel_theta: 0.3
+    yaw_goal_tolerance: 0.017  
+    xy_goal_tolerance: 0.01 
+    min_lookahead_dist: 0.4
+    max_lookahead_dist: 1.0
+    lookahead_time: 1.5
+    angle_threshold: 0.36
+
+  qrcode:
+    maker_goal_frame: qr_station
+    delay: 2.0
+    timeout: 60.0
+    vel_x: 0.05
+    vel_theta: 0.2
+    allow_rotate: true
+    yaw_goal_tolerance: 0.01 
+    xy_goal_tolerance: 0.01
+    min_lookahead_dist: 0.4
+    max_lookahead_dist: 1.0
+    lookahead_time: 1.5
+    angle_threshold: 0.36

Controllers/Packages/amr_startup/config/mapping.yaml

@@ -0,0 +1,63 @@
+SlamToolBox:
+  # Plugin params
+  solver_plugin: solver_plugins::CeresSolver
+  ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+  ceres_preconditioner: SCHUR_JACOBI
+  ceres_trust_strategy: LEVENBERG_MARQUARDT
+  ceres_dogleg_type: TRADITIONAL_DOGLEG
+  ceres_loss_function: None
+
+  # ROS Parameters
+  odom_frame: odom
+  map_frame: map
+  base_frame: base_link
+  scan_topic: /scan
+  mode: mapping #localization
+  debug_logging: false
+  throttle_scans: 1
+  transform_publish_period: 0.02 #if 0 never publishes odometry
+  map_update_interval: 10.0
+  resolution: 0.05
+  max_laser_range: 20.0 #for rastering images
+  minimum_time_interval: 0.5
+  transform_timeout: 0.2
+  tf_buffer_duration: 14400.
+  stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
+  enable_interactive_mode: true
+
+  # General Parameters
+  use_scan_matching: true
+  use_scan_barycenter: true
+  minimum_travel_distance: 0.5
+  minimum_travel_heading: 0.5
+  scan_buffer_size: 10
+  scan_buffer_maximum_scan_distance: 10
+  link_match_minimum_response_fine: 0.1  
+  link_scan_maximum_distance: 1.5
+  loop_search_maximum_distance: 3.0
+  do_loop_closing: true 
+  loop_match_minimum_chain_size: 10           
+  loop_match_maximum_variance_coarse: 3.0  
+  loop_match_minimum_response_coarse: 0.35    
+  loop_match_minimum_response_fine: 0.45
+
+  # Correlation Parameters - Correlation Parameters
+  correlation_search_space_dimension: 0.5
+  correlation_search_space_resolution: 0.01
+  correlation_search_space_smear_deviation: 0.1 
+
+  # Correlation Parameters - Loop Closure Parameters
+  loop_search_space_dimension: 8.0
+  loop_search_space_resolution: 0.05
+  loop_search_space_smear_deviation: 0.03
+
+  # Scan Matcher Parameters
+  distance_variance_penalty: 0.5      
+  angle_variance_penalty: 1.0    
+
+  fine_search_angle_offset: 0.00349     
+  coarse_search_angle_offset: 0.349   
+  coarse_angle_resolution: 0.0349        
+  minimum_angle_penalty: 0.9
+  minimum_distance_penalty: 0.5
+  use_response_expansion: true

Controllers/Packages/amr_startup/config/move_base_common_params.yaml

@@ -0,0 +1,24 @@
+
+### replanning
+controller_frequency: 20.0   # run controller at 15.0 Hz
+controller_patience: 0.0    # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan
+planner_frequency: 0.0       # don't continually replan (only when controller failed)
+planner_patience: 5.0        # if the first planning attempt failed, abort planning retries after 5.0 s...
+max_planning_retries: 5     # ... or after 10 attempts (whichever happens first)
+oscillation_timeout: -1.0    # abort controller and trigger recovery behaviors after 30.0 s
+oscillation_distance: 1.5
+### recovery behaviors
+recovery_behavior_enabled: false
+recovery_behaviors: [       
+                      {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery},
+                      {name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery},                      
+                    ]
+
+conservative_reset:
+  reset_distance: 3.0        # clear obstacles farther away than 3.0 m
+  invert_area_to_clear: true
+
+aggressive_reset:
+  reset_distance: 3.0
+
+

Controllers/Packages/amr_startup/config/mpc_local_planner_params.yaml

@@ -0,0 +1,106 @@
+base_local_planner: mpc_local_planner/MpcLocalPlannerROS
+MpcLocalPlannerROS:
+
+  odom_topic: odom
+    
+   ## Robot settings
+  robot:
+    type: "unicycle"
+    unicycle:
+      max_vel_x: 0.5
+      max_vel_x_backwards: 0.3
+      max_vel_theta: 0.3
+      acc_lim_x: 0.4 # deactive bounds with zero
+      dec_lim_x: 0.4 # deactive bounds with zero
+      acc_lim_theta: 0.6 # deactivate bounds with zero
+
+  ## Footprint model for collision avoidance
+  footprint_model:
+    type: "point"
+    is_footprint_dynamic: False
+
+  ## Collision avoidance
+  collision_avoidance:
+    min_obstacle_dist: 0.5 # Note, this parameter must be chosen w.r.t. the footprint_model
+    enable_dynamic_obstacles: False
+    force_inclusion_dist: 0.5
+    cutoff_dist: 2.0  
+    include_costmap_obstacles: True
+    costmap_obstacles_behind_robot_dist: 1.5
+    collision_check_no_poses: 5
+
+  ## Planning grid 
+  grid:
+    type: "fd_grid"
+    grid_size_ref: 20 # Set horizon length here (T = (grid_size_ref-1) * dt_ref); Note, also check max_global_plan_lookahead_dist
+    dt_ref: 0.3 # and here the corresponding temporal resolution  
+    xf_fixed: [False, False, False] # Unfix final state -> we use terminal cost below
+    warm_start: True
+    collocation_method: "forward_differences"
+    cost_integration_method: "left_sum"
+    variable_grid:
+      enable: False # We want a fixed grid  
+      min_dt: 0.0;
+      max_dt: 10.0;
+      grid_adaptation:
+        enable: True
+        dt_hyst_ratio: 0.1
+        min_grid_size: 2
+        max_grid_size: 50
+
+  ## Planning options
+  planning:
+    objective:
+      type: "quadratic_form" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly
+      quadratic_form:
+        state_weights: [2.0, 2.0, 0.25]
+        control_weights: [0.1, 0.05]
+        integral_form: False
+    terminal_cost:
+      type: "quadratic" # can be "none"
+      quadratic:
+        final_state_weights: [10.0, 10.0, 0.5]
+    terminal_constraint:
+      type: "none" # can be "none"
+      l2_ball:
+        weight_matrix: [1.0, 1.0, 1.0]
+        radius: 5
+
+  ## Controller options
+  controller:
+    outer_ocp_iterations: 1
+    xy_goal_tolerance: 0.05
+    yaw_goal_tolerance: 0.04
+    global_plan_overwrite_orientation: False
+    global_plan_prune_distance: 1.5
+    allow_init_with_backward_motion: True
+    max_global_plan_lookahead_dist: 1.0 # Check horizon length
+    force_reinit_new_goal_dist: 1.0 
+    force_reinit_new_goal_angular: 1.57
+    force_reinit_num_steps: 0
+    prefer_x_feedback: False
+    publish_ocp_results: True
+    
+  ## Solver settings
+  solver:
+    type: "ipopt"
+    ipopt:
+      iterations: 100
+      max_cpu_time: -1.0
+      ipopt_numeric_options:
+        tol: 1e-3
+      ipopt_string_options:
+        linear_solver: "mumps" 
+        hessian_approximation: "exact" # exact or limited-memory
+    lsq_lm:
+      iterations: 10
+      weight_init_eq: 2
+      weight_init_ineq: 2
+      weight_init_bounds: 2
+      weight_adapt_factor_eq: 1.5
+      weight_adapt_factor_ineq: 1.5
+      weight_adapt_factor_bounds: 1.5
+      weight_adapt_max_eq: 500
+      weight_adapt_max_ineq: 500
+      weight_adapt_max_bounds: 500
+

Controllers/Packages/amr_startup/config/mprim/genmprim_unicycle_highcost_5cm.m

@@ -0,0 +1,280 @@
+% /*
+%  * Copyright (c) 2008, Maxim Likhachev
+%  * All rights reserved.
+%  *
+%  * Redistribution and use in source and binary forms, with or without
+%  * modification, are permitted provided that the following conditions are met:
+%  *
+%  *     * Redistributions of source code must retain the above copyright
+%  *       notice, this list of conditions and the following disclaimer.
+%  *     * Redistributions in binary form must reproduce the above copyright
+%  *       notice, this list of conditions and the following disclaimer in the
+%  *       documentation and/or other materials provided with the distribution.
+%  *     * Neither the name of the Carnegie Mellon University nor the names of its
+%  *       contributors may be used to endorse or promote products derived from
+%  *       this software without specific prior written permission.
+%  *
+%  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+%  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+%  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+%  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+%  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+%  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+%  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+%  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+%  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+%  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+%  * POSSIBILITY OF SUCH DAMAGE.
+%  */
+
+function[] = genmprim_unicycle_highcost_5cm(outfilename)
+
+%
+%generates motion primitives and saves them into file
+%
+%written by Maxim Likhachev
+%---------------------------------------------------
+%
+
+%defines
+
+UNICYCLE_MPRIM_16DEGS = 1;
+
+
+if UNICYCLE_MPRIM_16DEGS == 1
+    resolution = 0.05;
+    numberofangles = 16; %preferably a power of 2, definitely multiple of 8
+    numberofprimsperangle = 7;
+
+    %multipliers (multiplier is used as costmult*cost)
+    forwardcostmult = 1;
+    backwardcostmult = 40;
+    forwardandturncostmult = 2;
+    sidestepcostmult = 10;
+    turninplacecostmult = 20;
+
+    %note, what is shown x,y,theta changes (not absolute numbers)
+
+    %0 degreees
+    basemprimendpts0_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult
+    %x aligned with the heading of the robot, angles are positive
+    %counterclockwise
+    %0 theta change
+    basemprimendpts0_c(1,:) = [1 0 0 forwardcostmult];
+    basemprimendpts0_c(2,:) = [8 0 0 forwardcostmult];
+    basemprimendpts0_c(3,:) = [-1 0 0 backwardcostmult];
+    %1/16 theta change
+    basemprimendpts0_c(4,:) = [8 1 1 forwardandturncostmult];
+    basemprimendpts0_c(5,:) = [8 -1 -1 forwardandturncostmult];
+    %turn in place
+    basemprimendpts0_c(6,:) = [0 0 1 turninplacecostmult];
+    basemprimendpts0_c(7,:) = [0 0 -1 turninplacecostmult];
+
+    %45 degrees
+    basemprimendpts45_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
+    %x aligned with the heading of the robot, angles are positive
+    %counterclockwise
+    %0 theta change
+    basemprimendpts45_c(1,:) = [1 1 0 forwardcostmult];
+    basemprimendpts45_c(2,:) = [6 6 0 forwardcostmult];
+    basemprimendpts45_c(3,:) = [-1 -1 0 backwardcostmult];
+    %1/16 theta change
+    basemprimendpts45_c(4,:) = [5 7 1 forwardandturncostmult];
+    basemprimendpts45_c(5,:) = [7 5 -1 forwardandturncostmult];
+    %turn in place
+    basemprimendpts45_c(6,:) = [0 0 1 turninplacecostmult];
+    basemprimendpts45_c(7,:) = [0 0 -1 turninplacecostmult];
+
+    %22.5 degrees
+    basemprimendpts22p5_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
+    %x aligned with the heading of the robot, angles are positive
+    %counterclockwise
+    %0 theta change
+    basemprimendpts22p5_c(1,:) = [2 1 0 forwardcostmult];
+    basemprimendpts22p5_c(2,:) = [6 3 0 forwardcostmult];
+    basemprimendpts22p5_c(3,:) = [-2 -1 0 backwardcostmult];
+    %1/16 theta change
+    basemprimendpts22p5_c(4,:) = [5 4 1 forwardandturncostmult];
+    basemprimendpts22p5_c(5,:) = [7 2 -1 forwardandturncostmult];
+    %turn in place
+    basemprimendpts22p5_c(6,:) = [0 0 1 turninplacecostmult];
+    basemprimendpts22p5_c(7,:) = [0 0 -1 turninplacecostmult];
+
+else
+    fprintf(1, 'ERROR: undefined mprims type\n');
+    return;
+end;
+
+
+fout = fopen(outfilename, 'w');
+
+
+%write the header
+fprintf(fout, 'resolution_m: %f\n', resolution);
+fprintf(fout, 'numberofangles: %d\n', numberofangles);
+fprintf(fout, 'totalnumberofprimitives: %d\n', numberofprimsperangle*numberofangles);
+
+%iterate over angles
+for angleind = 1:numberofangles
+
+    figure(1);
+    hold off;
+
+    text(0, 0, int2str(angleind));
+
+    %iterate over primitives
+    for primind = 1:numberofprimsperangle
+        fprintf(fout, 'primID: %d\n', primind-1);
+        fprintf(fout, 'startangle_c: %d\n', angleind-1);
+
+        %current angle
+        currentangle = (angleind-1)*2*pi/numberofangles;
+        currentangle_36000int = round((angleind-1)*36000/numberofangles);
+
+        %compute which template to use
+        if (rem(currentangle_36000int, 9000) == 0)
+            basemprimendpts_c = basemprimendpts0_c(primind,:);
+            angle = currentangle;
+        elseif (rem(currentangle_36000int, 4500) == 0)
+            basemprimendpts_c = basemprimendpts45_c(primind,:);
+            angle = currentangle - 45*pi/180;
+        elseif (rem(currentangle_36000int-7875, 9000) == 0)
+            basemprimendpts_c = basemprimendpts33p75_c(primind,:);
+            basemprimendpts_c(1) = basemprimendpts33p75_c(primind, 2); %reverse x and y
+            basemprimendpts_c(2) = basemprimendpts33p75_c(primind, 1);
+            basemprimendpts_c(3) = -basemprimendpts33p75_c(primind, 3); %reverse the angle as well
+            angle = currentangle - 78.75*pi/180;
+            fprintf(1, '78p75\n');
+        elseif (rem(currentangle_36000int-6750, 9000) == 0)
+            basemprimendpts_c = basemprimendpts22p5_c(primind,:);
+            basemprimendpts_c(1) = basemprimendpts22p5_c(primind, 2); %reverse x and y
+            basemprimendpts_c(2) = basemprimendpts22p5_c(primind, 1);
+            basemprimendpts_c(3) = -basemprimendpts22p5_c(primind, 3); %reverse the angle as well
+            %fprintf(1, '%d %d %d onto %d %d %d\n', basemprimendpts22p5_c(1), basemprimendpts22p5_c(2), basemprimendpts22p5_c(3), ...
+            %    basemprimendpts_c(1), basemprimendpts_c(2), basemprimendpts_c(3));
+            angle = currentangle - 67.5*pi/180;
+            fprintf(1, '67p5\n');
+        elseif (rem(currentangle_36000int-5625, 9000) == 0)
+            basemprimendpts_c = basemprimendpts11p25_c(primind,:);
+            basemprimendpts_c(1) = basemprimendpts11p25_c(primind, 2); %reverse x and y
+            basemprimendpts_c(2) = basemprimendpts11p25_c(primind, 1);
+            basemprimendpts_c(3) = -basemprimendpts11p25_c(primind, 3); %reverse the angle as well
+            angle = currentangle - 56.25*pi/180;
+            fprintf(1, '56p25\n');
+        elseif (rem(currentangle_36000int-3375, 9000) == 0)
+            basemprimendpts_c = basemprimendpts33p75_c(primind,:);
+            angle = currentangle - 33.75*pi/180;
+            fprintf(1, '33p75\n');
+        elseif (rem(currentangle_36000int-2250, 9000) == 0)
+            basemprimendpts_c = basemprimendpts22p5_c(primind,:);
+            angle = currentangle - 22.5*pi/180;
+            fprintf(1, '22p5\n');
+        elseif (rem(currentangle_36000int-1125, 9000) == 0)
+            basemprimendpts_c = basemprimendpts11p25_c(primind,:);
+            angle = currentangle - 11.25*pi/180;
+            fprintf(1, '11p25\n');
+        else
+            fprintf(1, 'ERROR: invalid angular resolution. angle = %d\n', currentangle_36000int);
+            return;
+        end;
+
+        %now figure out what action will be
+        baseendpose_c = basemprimendpts_c(1:3);
+        additionalactioncostmult = basemprimendpts_c(4);
+        endx_c = round(baseendpose_c(1)*cos(angle) - baseendpose_c(2)*sin(angle));
+        endy_c = round(baseendpose_c(1)*sin(angle) + baseendpose_c(2)*cos(angle));
+        endtheta_c = rem(angleind - 1 + baseendpose_c(3), numberofangles);
+        endpose_c = [endx_c endy_c endtheta_c];
+
+        fprintf(1, 'rotation angle=%f\n', angle*180/pi);
+
+        if baseendpose_c(2) == 0 & baseendpose_c(3) == 0
+            %fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
+        end;
+
+        %generate intermediate poses (remember they are w.r.t 0,0 (and not
+        %centers of the cells)
+        numofsamples = 10;
+        intermcells_m = zeros(numofsamples,3);
+        if UNICYCLE_MPRIM_16DEGS == 1
+            startpt = [0 0 currentangle];
+            endpt = [endpose_c(1)*resolution endpose_c(2)*resolution ...
+                rem(angleind - 1 + baseendpose_c(3), numberofangles)*2*pi/numberofangles];
+            intermcells_m = zeros(numofsamples,3);
+            if ((endx_c == 0 & endy_c == 0) | baseendpose_c(3) == 0) %turn in place or move forward
+                for iind = 1:numofsamples
+                    intermcells_m(iind,:) = [startpt(1) + (endpt(1) - startpt(1))*(iind-1)/(numofsamples-1) ...
+                                            startpt(2) + (endpt(2) - startpt(2))*(iind-1)/(numofsamples-1) ...
+                                            0];
+                    rotation_angle = (baseendpose_c(3) ) * (2*pi/numberofangles);
+                    intermcells_m(iind,3) = rem(startpt(3) + (rotation_angle)*(iind-1)/(numofsamples-1), 2*pi);
+
+                end;
+            else %unicycle-based move forward or backward
+                R = [cos(startpt(3)) sin(endpt(3)) - sin(startpt(3));
+                    sin(startpt(3)) -(cos(endpt(3)) - cos(startpt(3)))];
+                S = pinv(R)*[endpt(1) - startpt(1); endpt(2) - startpt(2)];
+                l = S(1);
+                tvoverrv = S(2);
+                rv = (baseendpose_c(3)*2*pi/numberofangles + l/tvoverrv);
+                tv = tvoverrv*rv;
+
+                if l < 0
+                    fprintf(1, 'WARNING: l = %d < 0 -> bad action start/end points\n', l);
+                    l = 0;
+                end;
+                %compute rv
+                %rv = baseendpose_c(3)*2*pi/numberofangles;
+                %compute tv
+                %tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3)))
+                %tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3)))
+                %tv = (tvx + tvy)/2.0;
+                %generate samples
+                for iind = 1:numofsamples
+                    dt = (iind-1)/(numofsamples-1);
+
+                    %dtheta = rv*dt + startpt(3);
+                    %intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ...
+                    %                        startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ...
+                    %                        dtheta];
+
+                    if(dt*tv < l)
+                        intermcells_m(iind,:) = [startpt(1) + dt*tv*cos(startpt(3)) ...
+                                                 startpt(2) + dt*tv*sin(startpt(3)) ...
+                                                 startpt(3)];
+                    else
+                        dtheta = rv*(dt - l/tv) + startpt(3);
+                        intermcells_m(iind,:) = [startpt(1) + l*cos(startpt(3)) + tvoverrv*(sin(dtheta) - sin(startpt(3))) ...
+                                                 startpt(2) + l*sin(startpt(3)) - tvoverrv*(cos(dtheta) - cos(startpt(3))) ...
+                                                 dtheta];
+                    end;
+                end;
+                %correct
+                errorxy = [endpt(1) - intermcells_m(numofsamples,1) ...
+                           endpt(2) - intermcells_m(numofsamples,2)];
+                fprintf(1, 'l=%f errx=%f erry=%f\n', l, errorxy(1), errorxy(2));
+                interpfactor = [0:1/(numofsamples-1):1];
+                intermcells_m(:,1) = intermcells_m(:,1) + errorxy(1)*interpfactor';
+                intermcells_m(:,2) = intermcells_m(:,2) + errorxy(2)*interpfactor';
+            end;
+        end;
+
+        %write out
+        fprintf(fout, 'endpose_c: %d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
+        fprintf(fout, 'additionalactioncostmult: %d\n', additionalactioncostmult);
+        fprintf(fout, 'intermediateposes: %d\n', size(intermcells_m,1));
+        for interind = 1:size(intermcells_m, 1)
+            fprintf(fout, '%.4f %.4f %.4f\n', intermcells_m(interind,1), intermcells_m(interind,2), intermcells_m(interind,3));
+        end;
+
+        plot(intermcells_m(:,1), intermcells_m(:,2));
+        axis([-0.3 0.3 -0.3 0.3]);
+        text(intermcells_m(numofsamples,1), intermcells_m(numofsamples,2), int2str(endpose_c(3)));
+        hold on;
+
+    end;
+    grid;
+    pause;
+end;
+
+fclose('all');

Controllers/Packages/amr_startup/config/mprim/genmprim_unicycle_highcost_5cm.py

@@ -0,0 +1,416 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) 2016, David Conner (Christopher Newport University)
+# Based on genmprim_unicycle.m
+# Copyright (c) 2008, Maxim Likhachev
+# All rights reserved.
+# converted by libermate utility (https://github.com/awesomebytes/libermate)
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#     * Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+#     * Redistributions in binary form must reproduce the above copyright
+#       notice, this list of conditions and the following disclaimer in the
+#       documentation and/or other materials provided with the distribution.
+#     * Neither the name of the Carnegie Mellon University nor the names of its
+#       contributors may be used to endorse or promote products derived from
+#       this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import numpy as np
+import rospkg
+
+# if available import pylab (from matlibplot)
+matplotlib_found = False
+try:
+    import matplotlib.pylab as plt
+
+    matplotlib_found = True
+except ImportError:
+    pass
+
+
+def matrix_size(mat, elem=None):
+    if not elem:
+        return mat.shape
+    else:
+        return mat.shape[int(elem) - 1]
+
+
+def genmprim_unicycle(outfilename, visualize=False, separate_plots=False):
+    visualize = matplotlib_found and visualize  # Plot the primitives
+
+    # Local Variables: basemprimendpts22p5_c, endtheta_c, endx_c,
+    #   baseendpose_c, additionalactioncostmult, fout, numofsamples,
+    #   basemprimendpts45_c, primind, basemprimendpts0_c, rv, angle, outfilename,
+    #   numberofangles, startpt, UNICYCLE_MPRIM_16DEGS, sidestepcostmult,
+    #   rotation_angle, basemprimendpts_c, forwardandturncostmult,
+    #   forwardcostmult, turninplacecostmult, endpose_c, backwardcostmult,
+    #   interpfactor, S, R, tvoverrv, dtheta, intermcells_m, tv, dt,
+    #   currentangle, numberofprimsperangle, resolution, currentangle_36000int,
+    #   l, iind, errorxy, interind, endy_c, angleind, endpt
+    # Function calls: plot, cos, pi, grid, figure, genmprim_unicycle, text,
+    #   int2str, pause, axis, sin, pinv, fprintf, fclose, rem, zeros, fopen,
+    #   round, size
+    # %
+    # %generates motion primitives and saves them into file
+    # %
+    # %written by Maxim Likhachev
+    # %---------------------------------------------------
+    # %
+    # %defines
+    UNICYCLE_MPRIM_16DEGS = 1.0
+    if UNICYCLE_MPRIM_16DEGS == 1.0:
+        resolution = 0.05
+        numberofangles = 16
+        # %preferably a power of 2, definitely multiple of 8
+        numberofprimsperangle = 7
+        # %multipliers (multiplier is used as costmult*cost)
+        forwardcostmult = 1.0
+        backwardcostmult = 40.0
+        forwardandturncostmult = 2.0
+        # sidestepcostmult = 10.0
+        turninplacecostmult = 20.0
+        # %note, what is shown x,y,theta changes (not absolute numbers)
+        # %0 degreees
+        basemprimendpts0_c = np.zeros((numberofprimsperangle, 4))
+        # %x,y,theta,costmult
+        # %x aligned with the heading of the robot, angles are positive
+        # %counterclockwise
+        # %0 theta change
+        basemprimendpts0_c[0, :] = np.array(np.hstack((1.0, 0.0, 0.0, forwardcostmult)))
+        basemprimendpts0_c[1, :] = np.array(np.hstack((8.0, 0.0, 0.0, forwardcostmult)))
+        basemprimendpts0_c[2, :] = np.array(np.hstack((-1.0, 0.0, 0.0, backwardcostmult)))
+        # %1/16 theta change
+        basemprimendpts0_c[3, :] = np.array(np.hstack((8.0, 1.0, 1.0, forwardandturncostmult)))
+        basemprimendpts0_c[4, :] = np.array(np.hstack((8.0, -1.0, -1.0, forwardandturncostmult)))
+        # %turn in place
+        basemprimendpts0_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
+        basemprimendpts0_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
+        # %45 degrees
+        basemprimendpts45_c = np.zeros((numberofprimsperangle, 4))
+        # %x,y,theta,costmult (multiplier is used as costmult*cost)
+        # %x aligned with the heading of the robot, angles are positive
+        # %counterclockwise
+        # %0 theta change
+        basemprimendpts45_c[0, :] = np.array(np.hstack((1.0, 1.0, 0.0, forwardcostmult)))
+        basemprimendpts45_c[1, :] = np.array(np.hstack((6.0, 6.0, 0.0, forwardcostmult)))
+        basemprimendpts45_c[2, :] = np.array(np.hstack((-1.0, -1.0, 0.0, backwardcostmult)))
+        # %1/16 theta change
+        basemprimendpts45_c[3, :] = np.array(np.hstack((5.0, 7.0, 1.0, forwardandturncostmult)))
+        basemprimendpts45_c[4, :] = np.array(np.hstack((7.0, 5.0, -1.0, forwardandturncostmult)))
+        # %turn in place
+        basemprimendpts45_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
+        basemprimendpts45_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
+        # %22.5 degrees
+        basemprimendpts22p5_c = np.zeros((numberofprimsperangle, 4))
+        # %x,y,theta,costmult (multiplier is used as costmult*cost)
+        # %x aligned with the heading of the robot, angles are positive
+        # %counterclockwise
+        # %0 theta change
+        basemprimendpts22p5_c[0, :] = np.array(np.hstack((2.0, 1.0, 0.0, forwardcostmult)))
+        basemprimendpts22p5_c[1, :] = np.array(np.hstack((6.0, 3.0, 0.0, forwardcostmult)))
+        basemprimendpts22p5_c[2, :] = np.array(np.hstack((-2.0, -1.0, 0.0, backwardcostmult)))
+        # %1/16 theta change
+        basemprimendpts22p5_c[3, :] = np.array(np.hstack((5.0, 4.0, 1.0, forwardandturncostmult)))
+        basemprimendpts22p5_c[4, :] = np.array(np.hstack((7.0, 2.0, -1.0, forwardandturncostmult)))
+        # %turn in place
+        basemprimendpts22p5_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
+        basemprimendpts22p5_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
+    else:
+        print('ERROR: undefined mprims type\n')
+        return []
+
+    fout = open(outfilename, 'w')
+    # %write the header
+    fout.write('resolution_m: %f\n' % (resolution))
+    fout.write('numberofangles: %d\n' % (numberofangles))
+    fout.write('totalnumberofprimitives: %d\n' % (numberofprimsperangle * numberofangles))
+    # %iterate over angles
+    for angleind in np.arange(1.0, (numberofangles) + 1):
+        currentangle = ((angleind - 1) * 2.0 * np.pi) / numberofangles
+        currentangle_36000int = np.round((angleind - 1) * 36000.0 / numberofangles)
+        if visualize:
+            if separate_plots:
+                fig = plt.figure(angleind)
+                plt.title('angle {:2.0f} (= {:3.1f} degrees)'.format(angleind - 1, currentangle_36000int / 100.0))
+            else:
+                fig = plt.figure(1)
+
+            plt.axis('equal')
+            plt.axis([-10 * resolution, 10 * resolution, -10 * resolution, 10 * resolution])
+            ax = fig.add_subplot(1, 1, 1)
+            major_ticks = np.arange(-8 * resolution, 9 * resolution, 4 * resolution)
+            minor_ticks = np.arange(-8 * resolution, 9 * resolution, resolution)
+            ax.set_xticks(major_ticks)
+            ax.set_xticks(minor_ticks, minor=True)
+            ax.set_yticks(major_ticks)
+            ax.set_yticks(minor_ticks, minor=True)
+            ax.grid(which='minor', alpha=0.5)
+            ax.grid(which='major', alpha=0.9)
+
+        # %iterate over primitives
+        for primind in np.arange(1.0, (numberofprimsperangle) + 1):
+            fout.write('primID: %d\n' % (primind - 1))
+            fout.write('startangle_c: %d\n' % (angleind - 1))
+            # %current angle
+            # %compute which template to use
+            if (currentangle_36000int % 9000) == 0:
+                basemprimendpts_c = basemprimendpts0_c[int(primind) - 1, :]
+                angle = currentangle
+            elif (currentangle_36000int % 4500) == 0:
+                basemprimendpts_c = basemprimendpts45_c[int(primind) - 1, :]
+                angle = currentangle - 45.0 * np.pi / 180.0
+
+            # commented out because basemprimendpts33p75_c is undefined
+            # elif ((currentangle_36000int - 7875) % 9000) == 0:
+            #     basemprimendpts_c = (
+            #         1 * basemprimendpts33p75_c[primind, :]
+            #     )  # 1* to force deep copy to avoid reference update below
+            #     basemprimendpts_c[0] = basemprimendpts33p75_c[primind, 1]
+            #     # %reverse x and y
+            #     basemprimendpts_c[1] = basemprimendpts33p75_c[primind, 0]
+            #     basemprimendpts_c[2] = -basemprimendpts33p75_c[primind, 2]
+            #     # %reverse the angle as well
+            #     angle = currentangle - (78.75 * np.pi) / 180.0
+            #     print('78p75\n')
+
+            elif ((currentangle_36000int - 6750) % 9000) == 0:
+                basemprimendpts_c = (
+                    1 * basemprimendpts22p5_c[int(primind) - 1, :]
+                )  # 1* to force deep copy to avoid reference update below
+                basemprimendpts_c[0] = basemprimendpts22p5_c[int(primind) - 1, 1]
+                # %reverse x and y
+                basemprimendpts_c[1] = basemprimendpts22p5_c[int(primind) - 1, 0]
+                basemprimendpts_c[2] = -basemprimendpts22p5_c[int(primind) - 1, 2]
+                # %reverse the angle as well
+                # print(
+                #     '%d : %d %d %d onto %d %d %d\n'
+                #     % (
+                #         primind - 1,
+                #         basemprimendpts22p5_c[int(primind) - 1, 0],
+                #         basemprimendpts22p5_c[int(primind) - 1, 1],
+                #         basemprimendpts22p5_c[int(primind) - 1, 2],
+                #         basemprimendpts_c[0],
+                #         basemprimendpts_c[1],
+                #         basemprimendpts_c[2],
+                #     )
+                # )
+                angle = currentangle - (67.5 * np.pi) / 180.0
+                print('67p5\n')
+
+            # commented out because basemprimendpts11p25_c is undefined
+            # elif ((currentangle_36000int - 5625) % 9000) == 0:
+            #     basemprimendpts_c = (
+            #         1 * basemprimendpts11p25_c[primind, :]
+            #     )  # 1* to force deep copy to avoid reference update below
+            #     basemprimendpts_c[0] = basemprimendpts11p25_c[primind, 1]
+            #     # %reverse x and y
+            #     basemprimendpts_c[1] = basemprimendpts11p25_c[primind, 0]
+            #     basemprimendpts_c[2] = -basemprimendpts11p25_c[primind, 2]
+            #     # %reverse the angle as well
+            #     angle = currentangle - (56.25 * np.pi) / 180.0
+            #     print('56p25\n')
+
+            # commented out because basemprimendpts33p75_c is undefined
+            # elif ((currentangle_36000int - 3375) % 9000) == 0:
+            #     basemprimendpts_c = basemprimendpts33p75_c[int(primind), :]
+            #     angle = currentangle - (33.75 * np.pi) / 180.0
+            #     print('33p75\n')
+
+            elif ((currentangle_36000int - 2250) % 9000) == 0:
+                basemprimendpts_c = basemprimendpts22p5_c[int(primind) - 1, :]
+                angle = currentangle - (22.5 * np.pi) / 180.0
+                print('22p5\n')
+
+            # commented out because basemprimendpts11p25_c is undefined
+            # elif ((currentangle_36000int - 1125) % 9000) == 0:
+            #     basemprimendpts_c = basemprimendpts11p25_c[int(primind), :]
+            #     angle = currentangle - (11.25 * np.pi) / 180.0
+            #     print('11p25\n')
+
+            else:
+                print('ERROR: invalid angular resolution. angle = %d\n' % currentangle_36000int)
+                return []
+
+            # %now figure out what action will be
+            baseendpose_c = basemprimendpts_c[0:3]
+            additionalactioncostmult = basemprimendpts_c[3]
+            endx_c = np.round((baseendpose_c[0] * np.cos(angle)) - (baseendpose_c[1] * np.sin(angle)))
+            endy_c = np.round((baseendpose_c[0] * np.sin(angle)) + (baseendpose_c[1] * np.cos(angle)))
+            endtheta_c = np.fmod(angleind - 1 + baseendpose_c[2], numberofangles)
+            endpose_c = np.array(np.hstack((endx_c, endy_c, endtheta_c)))
+            print("endpose_c=", endpose_c)
+            print(('rotation angle=%f\n' % (angle * 180.0 / np.pi)))
+            # if np.logical_and(baseendpose_c[1] == 0., baseendpose_c[2] == 0.):
+            # %fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
+
+            # %generate intermediate poses (remember they are w.r.t 0,0 (and not
+            # %centers of the cells)
+            numofsamples = 10
+            intermcells_m = np.zeros((numofsamples, 3))
+            if UNICYCLE_MPRIM_16DEGS == 1.0:
+                startpt = np.array(np.hstack((0.0, 0.0, currentangle)))
+                endpt = np.array(
+                    np.hstack(
+                        (
+                            (endpose_c[0] * resolution),
+                            (endpose_c[1] * resolution),
+                            (
+                                ((np.fmod(angleind - 1 + baseendpose_c[2], numberofangles)) * 2.0 * np.pi)
+                                / numberofangles
+                            ),
+                        )
+                    )
+                )
+
+                print("startpt =", startpt)
+                print("endpt   =", endpt)
+                intermcells_m = np.zeros((numofsamples, 3))
+                if np.logical_or(np.logical_and(endx_c == 0.0, endy_c == 0.0), baseendpose_c[2] == 0.0):
+                    # %turn in place or move forward
+                    for iind in np.arange(1.0, (numofsamples) + 1):
+                        fraction = float(iind - 1) / (numofsamples - 1)
+                        intermcells_m[int(iind) - 1, :] = np.array(
+                            (
+                                startpt[0] + (endpt[0] - startpt[0]) * fraction,
+                                startpt[1] + (endpt[1] - startpt[1]) * fraction,
+                                0,
+                            )
+                        )
+                        rotation_angle = baseendpose_c[2] * (2.0 * np.pi / numberofangles)
+                        intermcells_m[int(iind) - 1, 2] = np.fmod(startpt[2] + rotation_angle * fraction, (2.0 * np.pi))
+                        # print " ",iind,"  of ",numofsamples," fraction=",fraction," rotation=",rotation_angle
+
+                else:
+                    # %unicycle-based move forward or backward  (http://sbpl.net/node/53)
+                    R = np.array(
+                        np.vstack(
+                            (
+                                np.hstack((np.cos(startpt[2]), np.sin(endpt[2]) - np.sin(startpt[2]))),
+                                np.hstack((np.sin(startpt[2]), -np.cos(endpt[2]) + np.cos(startpt[2]))),
+                            )
+                        )
+                    )
+
+                    S = np.dot(np.linalg.pinv(R), np.array(np.vstack((endpt[0] - startpt[0], endpt[1] - startpt[1]))))
+                    l = S[0]
+                    tvoverrv = S[1]
+                    rv = (baseendpose_c[2] * 2.0 * np.pi / numberofangles) + l / tvoverrv
+                    tv = tvoverrv * rv
+
+                    # print "R=\n",R
+                    # print "Rpi=\n",np.linalg.pinv(R)
+                    # print "S=\n",S
+                    # print "l=",l
+                    # print "tvoverrv=",tvoverrv
+                    # print "rv=",rv
+                    # print "tv=",tv
+
+                    if l < 0.0:
+                        print(('WARNING: l = %f < 0 -> bad action start/end points\n' % (l)))
+                        l = 0.0
+
+                    # %compute rv
+                    # %rv = baseendpose_c(3)*2*pi/numberofangles;
+                    # %compute tv
+                    # %tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3)))
+                    # %tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3)))
+                    # %tv = (tvx + tvy)/2.0;
+                    # %generate samples
+                    for iind in np.arange(1, numofsamples + 1):
+                        dt = (iind - 1) / (numofsamples - 1)
+                        # %dtheta = rv*dt + startpt(3);
+                        # %intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ...
+                        # %                        startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ...
+                        # %                        dtheta];
+                        if (dt * tv) < l:
+                            intermcells_m[int(iind) - 1, :] = np.array(
+                                np.hstack(
+                                    (
+                                        startpt[0] + dt * tv * np.cos(startpt[2]),
+                                        startpt[1] + dt * tv * np.sin(startpt[2]),
+                                        startpt[2],
+                                    )
+                                )
+                            )
+                        else:
+                            dtheta = rv * (dt - l / tv) + startpt[2]
+                            intermcells_m[int(iind) - 1, :] = np.array(
+                                np.hstack(
+                                    (
+                                        startpt[0]
+                                        + l * np.cos(startpt[2])
+                                        + tvoverrv * (np.sin(dtheta) - np.sin(startpt[2])),
+                                        startpt[1]
+                                        + l * np.sin(startpt[2])
+                                        - tvoverrv * (np.cos(dtheta) - np.cos(startpt[2])),
+                                        dtheta,
+                                    )
+                                )
+                            )
+
+                    # %correct
+                    errorxy = np.array(
+                        np.hstack(
+                            (
+                                endpt[0] - intermcells_m[int(numofsamples) - 1, 0],
+                                endpt[1] - intermcells_m[int(numofsamples) - 1, 1],
+                            )
+                        )
+                    )
+                    # print('l=%f errx=%f erry=%f\n'%(l, errorxy[0], errorxy[1]))
+                    interpfactor = np.array(
+                        np.hstack((np.arange(0.0, 1.0 + (1.0 / (numofsamples)), 1.0 / (numofsamples - 1))))
+                    )
+
+                    # print "intermcells_m=",intermcells_m
+                    # print "interp'=",interpfactor.conj().T
+
+                    intermcells_m[:, 0] = intermcells_m[:, 0] + errorxy[0] * interpfactor.conj().T
+                    intermcells_m[:, 1] = intermcells_m[:, 1] + errorxy[1] * interpfactor.conj().T
+
+            # %write out
+            fout.write('endpose_c: %d %d %d\n' % (endpose_c[0], endpose_c[1], endpose_c[2]))
+            fout.write('additionalactioncostmult: %d\n' % (additionalactioncostmult))
+            fout.write('intermediateposes: %d\n' % (matrix_size(intermcells_m, 1.0)))
+            for interind in np.arange(1.0, (matrix_size(intermcells_m, 1.0)) + 1):
+                fout.write(
+                    '%.4f %.4f %.4f\n'
+                    % (
+                        intermcells_m[int(interind) - 1, 0],
+                        intermcells_m[int(interind) - 1, 1],
+                        intermcells_m[int(interind) - 1, 2],
+                    )
+                )
+
+            if visualize:
+                plt.plot(intermcells_m[:, 0], intermcells_m[:, 1], linestyle="-", marker="o")
+                plt.text(endpt[0], endpt[1], '{:2.0f}'.format(endpose_c[2]))
+        # if (visualize):
+        #    plt.waitforbuttonpress()  # uncomment to plot each primitive set one at a time
+
+    fout.close()
+    if visualize:
+        #    plt.waitforbuttonpress()  # hold until buttom pressed
+        plt.show()  # Keep windows open until the program is terminated
+    return []
+
+
+if __name__ == "__main__":
+    rospack = rospkg.RosPack()
+    outfilename = rospack.get_path('mir_navigation') + '/mprim/unicycle_highcost_5cm.mprim'
+    genmprim_unicycle(outfilename, visualize=True)

Controllers/Packages/amr_startup/config/mqtt_general.yaml

@@ -0,0 +1,8 @@
+MQTT:
+  Name: T800
+  Host: 172.20.235.170
+  Port: 1885
+  Client_ID: T800
+  Username: robotics
+  Password: robotics
+  Keep_Alive: 60

Controllers/Packages/amr_startup/config/pnkx_local_planner_params.yaml

@@ -0,0 +1,188 @@
+
+position_planner_name: PNKXLocalPlanner
+docking_planner_name: PNKXDockingLocalPlanner
+go_straight_planner_name: PNKXGoStraightLocalPlanner
+rotate_planner_name: PNKXRotateLocalPlanner
+
+base_local_planner: nav_core_adapter::LocalPlannerAdapter
+yaw_goal_tolerance: 0.017  
+xy_goal_tolerance: 0.015 
+stateful: true
+publish_topic: true  
+
+LocalPlannerAdapter:
+  PNKXLocalPlanner:
+    # Algorithm
+    trajectory_generator_name: mkt_plugins::LimitedAccelGenerator 
+    algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory
+    algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
+    # Goal checking
+    goal_checker_name: mkt_plugins::GoalChecker 
+
+  PNKXDockingLocalPlanner: 
+    # Algorithm
+    trajectory_generator_name: mkt_plugins::LimitedAccelGenerator 
+    algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory
+    algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
+    # Goal checking
+    goal_checker_name: mkt_plugins::GoalChecker 
+
+  PNKXGoStraightLocalPlanner:
+    # Algorithm
+    trajectory_generator_name: mkt_plugins::LimitedAccelGenerator 
+    algorithm_nav_name: mkt_algorithm::diff::GoStraight
+    # Goal checking
+    goal_checker_name: mkt_plugins::GoalChecker 
+
+  PNKXRotateLocalPlanner:
+    # Algorithm
+    algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
+    trajectory_generator_name: mkt_plugins::LimitedAccelGenerator 
+    # Goal checking
+    goal_checker_name: mkt_plugins::SimpleGoalChecker
+    stateful: true
+    
+LimitedAccelGenerator:
+  max_vel_x: 1.2
+  min_vel_x: -1.2
+
+  max_vel_y: 0.0  # diff drive robot
+  min_vel_y: 0.0  # diff drive robot
+
+  max_speed_xy: 2.0 # max_trans_vel: 0.8  # choose slightly less than the base's capability
+  min_speed_xy: 0.1    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
+
+  max_vel_theta: 0.3    # max_rot_vel: 1.0  # choose slightly less than the base's capability
+  min_vel_theta: 0.05  # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity
+
+  acc_lim_x: 1.0
+  acc_lim_y: 0.0      # diff drive robot
+  acc_lim_theta: 0.5
+  decel_lim_x: -2.0
+  decel_lim_y: -0.0
+  decel_lim_theta: -1.0
+
+  # Whether to split the path into segments or not
+  split_path: true
+  sim_time: 1.5
+  vx_samples: 15
+  vy_samples: 1       
+  vtheta_samples: 10
+  discretize_by_time: true
+  angular_granularity: 0.05
+  linear_granularity: 0.1
+  # sim_period
+  include_last_point: true
+
+PredictiveTrajectory:
+  avoid_obstacles: false
+  xy_local_goal_tolerance: 0.01
+  angle_threshold: 0.5
+  index_samples: 60
+  follow_step_path: true
+
+  # Lookahead
+  use_velocity_scaled_lookahead_dist: true    # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
+  # only when false:
+  lookahead_dist: 0.325                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
+  # only when true:
+  min_lookahead_dist: 0.5                     # The minimum lookahead distance (m) threshold. (default: 0.3)
+  max_lookahead_dist: 1.5                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
+  lookahead_time: 1.8                         # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
+  min_journey_squared: 0.3                    # Minimum squared journey to consider for goal (default: 0.2) 
+  max_journey_squared: 0.8                    # Maximum squared journey to consider for goal (default: 0.2) 
+
+  # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
+  use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
+  # only when true:
+  rotate_to_heading_min_angle: 0.03   # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
+
+  # stoped
+  rot_stopped_velocity: 0.03
+  trans_stopped_velocity: 0.06
+  min_approach_linear_velocity: 0.05   # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
+  
+  # Regulated linear velocity scaling
+  use_regulated_linear_velocity_scaling: true   # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true)
+  # only when true:
+  regulated_linear_scaling_min_radius: 0.6      # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9)
+  regulated_linear_scaling_min_speed: 0.05      # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25)
+
+  # Inflation cost scaling (Limit velocity by proximity to obstacles)
+  use_cost_regulated_linear_velocity_scaling: false   # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true)
+  inflation_cost_scaling_factor: 2.0                 # (default: 3.0)   # must be > 0
+  cost_scaling_dist: 0.2                             # (default: 0.6)
+  cost_scaling_gain: 2.0                             # (default: 1.0)
+
+GoStraight:
+  avoid_obstacles: false
+  xy_local_goal_tolerance: 0.01
+  angle_threshold: 0.6
+  index_samples: 60
+  follow_step_path: true
+
+  # Lookahead
+  use_velocity_scaled_lookahead_dist: true   # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
+  # only when false:
+  lookahead_dist: 0.325                        # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
+  # only when true:
+  min_lookahead_dist: 0.5                    # The minimum lookahead distance (m) threshold. (default: 0.3)
+  max_lookahead_dist: 1.5                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
+  lookahead_time: 1.8                         # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
+  min_journey_squared: 0.3                    # Minimum squared journey to consider for goal (default: 0.2) 
+  max_journey_squared: 0.6                    # Minimum squared journey to consider for goal (default: 0.2) 
+
+  # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
+  use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
+  # only when true:
+  rotate_to_heading_min_angle: 0.35   # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
+  # Speed
+  min_approach_linear_velocity: 0.05   # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
+  
+  # stoped
+  rot_stopped_velocity: 0.03
+  trans_stopped_velocity: 0.06
+  min_approach_linear_velocity: 0.05   # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
+  
+  use_regulated_linear_velocity_scaling: false
+  use_cost_regulated_linear_velocity_scaling: false
+
+RotateToGoal:
+  avoid_obstacles: false
+  xy_local_goal_tolerance: 0.01
+  angle_threshold: 0.6
+  index_samples: 60
+  follow_step_path: true
+
+  # Lookahead
+  use_velocity_scaled_lookahead_dist: true    # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
+  # only when false:
+  lookahead_dist: 0.325                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
+  # only when true:
+  min_lookahead_dist: 0.5                     # The minimum lookahead distance (m) threshold. (default: 0.3)
+  max_lookahead_dist: 1.5                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
+  lookahead_time: 1.8                         # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
+  min_journey_squared: 0.3                    # Minimum squared journey to consider for goal (default: 0.2) 
+  max_journey_squared: 0.6                    # Maximum squared journey to consider for goal (default: 0.2) 
+
+  # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
+  use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
+  # only when true:
+  rotate_to_heading_min_angle: 0.03   # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
+
+  # stoped
+  rot_stopped_velocity: 0.03
+  trans_stopped_velocity: 0.06
+  min_approach_linear_velocity: 0.05   # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
+  
+  # Regulated linear velocity scaling
+  use_regulated_linear_velocity_scaling: true   # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true)
+  # only when true:
+  regulated_linear_scaling_min_radius: 0.6      # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9)
+  regulated_linear_scaling_min_speed: 0.05      # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25)
+
+  # Inflation cost scaling (Limit velocity by proximity to obstacles)
+  use_cost_regulated_linear_velocity_scaling: false   # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true)
+  inflation_cost_scaling_factor: 2.0                 # (default: 3.0)   # must be > 0
+  cost_scaling_dist: 0.2                             # (default: 0.6)
+  cost_scaling_gain: 2.0                             # (default: 1.0)

Controllers/Packages/amr_startup/config/ros_diff_drive_controller.yaml

@@ -0,0 +1,71 @@
+# -----------------------------------
+left_wheel  : 'left_wheel_joint' 
+right_wheel : 'right_wheel_joint'
+publish_rate: 50              # this is what the real MiR platform publishes (default: 50)
+# These covariances are exactly what the real MiR platform publishes
+pose_covariance_diagonal : [0.00001, 0.00001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+twist_covariance_diagonal: [0.1, 0.1, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+enable_odom_tf: true
+enable_wheel_tf: true
+allow_multiple_cmd_vel_publishers: true
+# open_loop: false
+# Wheel separation and diameter. These are both optional.
+# diff_drive_controller will attempt to read either one or both from the
+# URDF if not specified as a parameter.
+# We don't set the value here because it's different for each MiR type (100, 250, ...), and
+# the plugin figures out the correct values.
+wheel_separation : 0.5106
+wheel_radius : 0.1
+
+# Wheel separation and radius multipliers
+wheel_separation_multiplier: 1.0 # default: 1.0
+wheel_radius_multiplier    : 1.0 # default: 1.0
+
+# Velocity commands timeout [s], default 0.5
+cmd_vel_timeout: 1.0
+
+# frame_ids (same as real MiR platform)
+base_frame_id: base_footprint # default: base_link base_footprint
+odom_frame_id: odom           # default: odom 
+
+# Velocity and acceleration limits
+# Whenever a min_* is unspecified, default to -max_*
+linear:
+  x:
+    has_velocity_limits    : true
+    max_velocity           : 1.5  # m/s; move_base max_vel_x: 0.8
+    min_velocity           : -1.5 # m/s
+    has_acceleration_limits: true
+    max_acceleration       : 3.0  # m/s^2; move_base acc_lim_x: 1.5
+    min_acceleration       : -3.0 # m/s^2
+    has_jerk_limits        : true
+    max_jerk               : 5.0  # m/s^3
+angular:
+  z:
+    has_velocity_limits    : true
+    max_velocity           : 0.6  # rad/s; move_base max_rot_vel: 1.0
+    min_velocity           : -0.6 
+    has_acceleration_limits: true
+    max_acceleration       : 3.0 # rad/s^2; move_base acc_lim_th: 2.0
+    has_jerk_limits        : true
+    max_jerk               : 2.5  # rad/s^3
+
+left_wheel_joint: 
+  lookup_name: WheelPlugin
+  max_publish_rate: 50
+  mesurement_topic: left_encoder
+  frame_id: left_wheel_link
+  wheel_topic: /left_wheel
+  subscribe_queue_size: 1
+  command_timeout: 5.0
+  origin : [0.0, 0.255, 0.075, 0.0, 0.0, 0.0]  # origin from base_frame_id
+
+right_wheel_joint:  
+  lookup_name: WheelPlugin
+  max_publish_rate: 50
+  mesurement_topic: right_encoder
+  frame_id: right_wheel_link
+  wheel_topic: /right_wheel
+  subscribe_queue_size: 1
+  command_timeout: 5.0
+  origin : [0.0, -0.255, 0.075, 0.0, 0.0, 0.0]  # origin from base_frame_id

Controllers/Packages/amr_startup/config/sbpl_global_params.yaml

@@ -0,0 +1,10 @@
+base_global_planner: SBPLLatticePlanner
+SBPLLatticePlanner:
+  environment_type: XYThetaLattice
+  planner_type: ARAPlanner
+  allocated_time: 10.0
+  initial_epsilon: 1.0
+  force_scratch_limit: 10000
+  forward_search: true
+  nominalvel_mpersecs: 0.2
+  timetoturn45degsinplace_secs: 0.6 # = 0.6 rad/s

Controllers/Packages/amr_startup/config/sick_line_guidance_mls.yaml

@@ -0,0 +1,61 @@
+bus:
+  device: can0
+  driver_plugin: can::SocketCANInterface
+  master_allocator: canopen::SimpleMaster::Allocator
+sync:
+  interval_ms: 10
+  overflow: 0
+#
+# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats)
+#   rate: 100 # heartbeat rate (1/rate in seconds)
+#   msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started
+nodes:
+  f_mlse:
+    id: 0x01 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
+    eds_pkg: sick_line_guidance # package name for relative path
+    eds_file: mls/SICK-MLS.eds      # path to EDS/DCF file
+    publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"]
+    # MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022
+    
+    # sick_line_guidance configuration of this node:
+    sick_device_family: "MLS"              # can devices of OLS10, OLS20 or MLS family currently supported
+    sick_topic: "f_mlse"                      # MLS_Measurement messages are published in topic "/mls"
+    subscribe_queue_size: 1
+    sick_frame_id: "f_mlse" # MLS_Measurement messages are published frame_id "mls_measurement_frame"
+
+    # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
+    # example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
+    # dcf_overlay:
+    #   "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0
+    #   "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0
+    #   "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0
+    #   "2025":     "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600
+    #   "2026":        "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0
+    #   "2027":     "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
+    #   "2029":     "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0
+    #
+
+  b_mlse:
+    id: 0x02 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
+    eds_pkg: sick_line_guidance # package name for relative path
+    eds_file: mls/SICK-MLS.eds      # path to EDS/DCF file
+    publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"]
+    # MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022
+    
+    # sick_line_guidance configuration of this node:
+    sick_device_family: "MLS"              # can devices of OLS10, OLS20 or MLS family currently supported
+    sick_topic: "b_mlse"                      # MLS_Measurement messages are published in topic "/mls"
+    subscribe_queue_size: 1
+    sick_frame_id: "b_mlse" # MLS_Measurement messages are published frame_id "mls_measurement_frame"
+
+    # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
+    # example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
+    # dcf_overlay:
+    #   "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0
+    #   "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0
+    #   "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0
+    #   "2025":     "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600
+    #   "2026":        "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0
+    #   "2027":     "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
+    #   "2029":     "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0
+    #

Controllers/Packages/amr_startup/config/two_points_global_params.yaml

@@ -0,0 +1,3 @@
+base_global_planner: TwoPointsPlanner
+TwoPointsPlanner:
+  lethal_obstacle: 20

Controllers/Packages/amr_startup/launch/amr_control.launch

@@ -0,0 +1,50 @@
+<launch>
+  <arg name="robot_type" default="T800" doc="Can be 'hook_150' or 'imr' for now." />
+  <arg name="local_planner"   default="pnkx"  doc="Local planner can be either dwa, dwb, eband, base, teb or pose" />
+  <arg name="global_planner"  default="custom" doc="Global planner can be either sbpl two_points custom"/>
+  <arg name="global_plan_msg_type" default="vda5050_msgs::Order" doc="nav_msgs::Path or vda5050_msgs::Order"/> 
+  <arg name="primitive_filename" default="$(find amr_startup)/config/mprim/unicycle_highcost_5cm.mprim"/>
+  <arg name="with_virtual_walls" default="true" doc="Enables usage of virtual walls when set. Set to false when running SLAM." />
+  <arg name="prefix" default="" doc="Prefix used for robot tf frames" /> <!-- used in the config files -->
+
+  <param name="rosconsole_config_file" value="$(find amr_startup)/rosconsole.config"/>
+  <rosparam file="$(find amr_startup)/config/mqtt_general.yaml" command="load" />
+  <node pkg="amr_control" type="amr_control_node" respawn="false" name="amr_node" output="screen" clear_params="true">
+    <!-- robot type -->
+    <rosparam param="footprint" if="$(eval robot_type == 'imr')">
+      [[0.412, -0.304], [0.412, 0.304], [-0.412, 0.304], [-0.412, -0.304]]
+    </rosparam>
+    <!-- Footprint cho imr -->
+    <rosparam param="footprint" if="$(eval robot_type == 'hook_150')">
+      [[0.511,-0.1955],[0.511,0.1955],[-0.511,0.1955],[-0.511,-0.1955]]
+    </rosparam>
+
+    <rosparam param="footprint" if="$(eval robot_type == 'T800')">
+      [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
+    </rosparam>
+
+    <param name="primitive_filename" value="$(arg primitive_filename)" />
+    <param name="global_plan_msg_type" value="$(arg global_plan_msg_type)" />
+    
+    <rosparam file="$(find amr_startup)/config/maker_sources.yaml" command="load" />
+    <rosparam file="$(find amr_startup)/config/move_base_common_params.yaml" command="load" />
+    <rosparam file="$(find amr_startup)/config/$(arg global_planner)_global_params.yaml" command="load" />
+    <rosparam file="$(find amr_startup)/config/$(arg local_planner)_local_planner_params.yaml" command="load" />
+
+    <!-- global costmap params -->
+    <rosparam file="$(find amr_startup)/config/costmap_common_params.yaml" command="load" ns="global_costmap" subst_value="true" />
+    <rosparam file="$(find amr_startup)/config/costmap_global_params.yaml" command="load" />
+    <rosparam file="$(find amr_startup)/config/costmap_global_params_plugins_no_virtual_walls.yaml" command="load" />
+
+    <!-- local costmap params -->
+    <rosparam file="$(find amr_startup)/config/costmap_common_params.yaml" command="load" ns="local_costmap" subst_value="true" />
+    <rosparam file="$(find amr_startup)/config/costmap_local_params.yaml" command="load" subst_value="true" />
+    <rosparam file="$(find amr_startup)/config/costmap_local_params_plugins_no_virtual_walls.yaml" command="load" />
+
+    <rosparam file="$(find amr_startup)/config/localization.yaml" command="load" />
+    
+    <remap from="map" to="/map" />
+    <remap from="marker" to="move_base_node/markers" />
+    <!-- <remap from="cmd_vel" to="/cmd_vel_1" /> -->
+  </node>
+</launch>

Controllers/Packages/amr_startup/launch/amr_startup.launch

@@ -0,0 +1,102 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="tf_prefix" default="" />
+
+  <node pkg="tf2_ros" type="static_transform_publisher" name="base_footprint_to_base_link" args="0 0 0 0 0 0 base_footprint base_link" />
+  <node pkg="tf2_ros" type="static_transform_publisher" name="imu_link_to_base_link" args="0 0 0.45 0 0 0 base_link imu_link" />
+  <node pkg="tf2_ros" type="static_transform_publisher" name="surface_to_base_link" args="0 0 0.9 0 0 0 base_link surface" />
+
+
+  <include file="$(find sick_line_guidance)/launch/sick_line_guidance.launch">
+    <arg name="yaml"     value="$(find amr_startup)/config/sick_line_guidance_mls.yaml"/>
+  </include>
+  <include file="$(find mlse_tf_base_link)/launch/msle_tf_base_link.launch">
+    <arg name="yaml"     value="$(find amr_startup)/config/sick_line_guidance_mls.yaml"/>
+  </include>
+
+  <node pkg="diff_wheel_plugin"   type="diff_wheel_feedback" name="diff_wheel_feedback" output="screen"> 
+    <rosparam file="$(find amr_startup)/config/plc_config.yaml" command="load" /> 
+    <param name="start_bit"     value="111"/>
+    <param name="end_bit"       value="114"/>
+  </node>
+
+  <node pkg="diff_wheel_plugin" type="diff_wheel_controller" name="diff_wheel_controller" output="screen">
+    <rosparam file="$(find amr_startup)/config/motorInfomation.yaml" command="load" /> 
+    <param name="port_name" type="str" value="/dev/ttyTHS0"/> 
+  </node>
+
+  <arg name="frame_id" default="imu_frame"/>
+  <node pkg="tf2_ros" type="static_transform_publisher" name="$(arg frame_id)_to_imu_link" args="0 0 0 0 0 0 imu_link $(arg frame_id)" />
+  <node pkg="wit_wt901ble_reader" name="wit_wt901ble_reader_node" type="wit_wt901ble_reader_node" output="screen">
+    <param name="baudrate" type="int" value="115200"/>
+    <param name="portname" type="string" value="/dev/USB_IMU"/>
+    <param name="topic_name" type="string" value="/imu_data"/>
+    <param name="frame_id" type="string" value="$(arg frame_id)"/>
+    <param name="imu_diagnostics_topic_name" type="string" value="/imu_diagnostics"/>
+  </node>
+  
+  <node pkg="ros_kinematics" type="ros_kinematics_node" name="ros_kinematics" output="screen"> 
+    <rosparam file="$(find amr_startup)/config/ros_diff_drive_controller.yaml" command="load" />
+    <param name="use_encoder" type="bool" value="false"/>
+    <param name="type" type="int" value="2"/>
+    <remap from="/ros_kinematics/odom" to="/odom" />
+  </node>
+
+  <!-- <include file="$(find manual_psx)/launch/ps4.launch"></include> -->
+
+  <!-- <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_node" clear_params="true" output="screen">
+    <rosparam command="load" file="$(find amr_startup)/config/ekf.yaml" subst_value="true" />
+  </node> -->
+
+  <!-- Load URDF -->
+  <include file="$(find robot_description)/launch/upload_urdf.launch">
+      <arg name="robot_type" value="imr" />
+      <arg name="tf_prefix" value="$(arg tf_prefix)" />
+  </include>
+
+  <!-- driver -->
+  <arg name="pkg" value="olelidar"/>
+  
+  <arg name="driver" default="true"/>
+  <arg name="f_device_ip" default="192.168.2.101"/>
+  <arg name="f_device_port" default="2368"/>
+  <arg name="local_ip" default="192.168.2.100"/>
+  <arg name="multiaddr" default=""/>
+  
+  <!-- decoder -->
+  <arg name="f_frame_id" default="front_laser_link"/>
+  <arg name="f_topic_name" default="/f_scan"/>
+  <arg name="f_r_max" default="25"/>
+  <arg name="f_ang_start" default="-90"/>
+  <arg name="f_ang_end" default="90"/>
+  <arg name="decoder" default="true"/>
+  <arg name="inverted" default="false"/>
+  <arg name="f_debug" default="false"/>
+  <env if="$(arg f_debug)" name="ROSCONSOLE_CONFIG_FILE" value="$(find olelidar)/launch/debug.conf"/>
+
+  <node pkg="tf2_ros" type="static_transform_publisher" name="$(arg f_frame_id)_to_base_link" args="0.432 0 0.135 0 0 0 base_link $(arg f_frame_id)" />
+
+  <node pkg="$(arg pkg)" type="$(arg pkg)_decoder" name="$(arg pkg)_decoder" output="screen">
+    <param name="frame_id" type="string" value="$(arg f_frame_id)"/>
+    <param name="r_max" type="int" value="$(arg f_r_max)"/>
+    <param name="ang_start" type="int" value="$(arg f_ang_start)"/>
+    <param name="ang_end" type="int" value="$(arg f_ang_end)"/>
+    <param name="inverted" type="bool" value="$(arg inverted)"/>
+    <param name="device_ip" type="string" value="$(arg f_device_ip)"/>
+    <param name="device_port" type="int" value="$(arg f_device_port)"/>
+    <param name="local_ip" type="string" value="$(arg local_ip)"/>
+    <param name="multiaddr" type="string" value="$(arg multiaddr)"/>
+
+    <remap from="~packet" to="packet"/>
+    <remap from="~scan" to="$(arg f_topic_name)"/>
+  </node>
+
+  <node pkg="topic_tools" type="relay" name="f_scan_relay" args="$(arg f_topic_name) scan"/>
+
+  <include file="$(find amr_startup)/launch/amr_control.launch"></include>
+
+  <!-- <include file="$(find amr_startup)/launch/includes/rviz.launch"></include> -->
+
+  <!-- <include file="$(find slam_toolbox)/launch/offline.launch"></include> -->
+
+</launch>  

Controllers/Packages/amr_startup/launch/fake_detected_shelf.launch

@@ -0,0 +1,5 @@
+
+<launch>
+    <!-- <node pkg="tf" type="static_transform_publisher" name="goal_charger_broadcaster" args="1.9 0.07 0 0.0 0 0  base_link shelf_target 100" /> -->
+    <node pkg="tf" type="static_transform_publisher" name="base_link_surface_broadcaster" args="0 0 0 0 0 0  base_link surface 1000" />
+</launch>

Controllers/Packages/amr_startup/launch/hector_mapping.launch

@@ -0,0 +1,65 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/>
+  <arg name="base_frame" default="base_footprint"/>
+  <arg name="odom_frame" default="odom"/>
+  <arg name="pub_map_odom_transform" default="true"/>
+  <arg name="scan_subscriber_queue_size" default="5"/>
+  <arg name="scan_topic" default="scan"/>
+  <arg name="map_size" default="2048"/>
+    <!-- set use_tf_pose_start_estimate and map_with_known_poses to `true` when
+         the map-base_footprint transform is provided by a different node, such
+         as fake_localization -->
+  <arg name="disable_localization" default="false"/>
+
+  <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
+
+    <!-- Frame names -->
+    <param name="map_frame" value="map" />
+    <param name="base_frame" value="$(arg base_frame)" />
+    <param name="odom_frame" value="$(arg odom_frame)" />
+
+    <!-- Tf use -->
+    <param name="use_tf_scan_transformation" value="true"/>
+    <param name="use_tf_pose_start_estimate" value="$(arg disable_localization)"/>
+    <param name="map_with_known_poses" value="$(arg disable_localization)" />
+    <param name="pub_map_odom_transform" value="$(arg pub_map_odom_transform)"/>
+    <param name="pub_map_scanmatch_transform" value="true" />
+
+    <!-- Map size / start point -->
+    <param name="map_resolution" value="0.050"/>
+    <param name="map_size" value="$(arg map_size)"/>
+    <param name="map_start_x" value="0.5"/>
+    <param name="map_start_y" value="0.5" />
+    <param name="map_multi_res_levels" value="3" />
+    <param name="map_pub_period" value ="2.0" />
+
+    <!-- Map update parameters -->
+    <param name="update_factor_free" value="0.4"/>
+    <param name="update_factor_occupied" value="0.9" />
+    <param name="map_update_distance_thresh" value="0.4"/>
+    <param name="map_update_angle_thresh" value="0.06" />
+    <param name="laser_min_dist" value="0.1" />
+    <param name="laser_max_dist" value="10.0" />
+    <param name="laser_z_min_value" value="-1.0" />
+    <param name="laser_z_max_value" value="1.0" />
+
+    <!-- Advertising config -->
+    <param name="advertise_map_service" value="true"/>
+
+    <param name="scan_subscriber_queue_size" value="$(arg scan_subscriber_queue_size)"/>
+    <param name="scan_topic" value="$(arg scan_topic)"/>
+    <param name="pose_update_topic" value="poseupdate" />
+    <param name="sys_msg_topic" value="syscommand" />
+    <param name="pub_odometry" value="false" />
+
+    <!-- Debug parameters -->
+    <!--
+      <param name="output_timing" value="false"/>
+      <param name="pub_drawings" value="true"/>
+      <param name="pub_debug_output" value="true"/>
+    -->
+    <param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />
+    <remap from="map" to="/map" />
+  </node>
+</launch>

Controllers/Packages/amr_startup/launch/includes/amcl.launch

@@ -0,0 +1,105 @@
+<?xml version="1.0" ?>
+
+<launch>
+  <arg name="tf_prefix" default="" />
+  <arg name="namespace" default="$(arg tf_prefix)" doc="Namespace to push all topics into."/>
+
+  <arg name="use_map_topic"   default="true"/>
+  <arg name="scan_topic"      default="scan"/>
+  <arg name="map_topic"       default="/map"/>         <!-- if use_map_topic = true  -->
+  <arg name="map_service"     default="/static_map"/>  <!-- if use_map_topic = false -->
+  <arg name="initial_pose_x"  default="0.0"/>
+  <arg name="initial_pose_y"  default="0.0"/>
+  <arg name="initial_pose_a"  default="0.0"/>
+  <arg name="odom_frame_id"   default="$(arg tf_prefix)/odom"/>
+  <arg name="base_frame_id"   default="$(arg tf_prefix)/base_footprint"/>
+  <arg name="global_frame_id" default="/map"/>
+
+  <group if="$(eval namespace != '')" ns="$(arg namespace)">
+    <node pkg="amcl" type="amcl" name="amcl" output="screen" >
+      <param name="use_map_topic"             value="$(arg use_map_topic)"/>
+      <param name="odom_model_type"           value="diff-corrected"/>
+      <param name="gui_publish_rate"          value="10.0"/>
+      <param name="save_pose_rate"            value="0.5"/>
+      <param name="laser_max_beams"           value="200"/>
+      <param name="laser_min_range"           value="-1.0"/>
+      <param name="laser_max_range"           value="-1.0"/>
+      <param name="min_particles"             value="500"/>
+      <param name="max_particles"             value="5000"/>
+      <param name="kld_err"                   value="0.09"/>
+      <param name="kld_z"                     value="0.99"/>
+      <param name="odom_alpha1"               value="0.02"/>
+      <param name="odom_alpha2"               value="0.01"/>
+      <param name="odom_alpha3"               value="0.01"/>
+      <param name="odom_alpha4"               value="0.02"/>
+      <param name="laser_z_hit"               value="0.5"/>
+      <param name="laser_z_short"             value="0.05"/>
+      <param name="laser_z_max"               value="0.05"/>
+      <param name="laser_z_rand"              value="0.5"/>
+      <param name="laser_sigma_hit"           value="0.2"/>
+      <param name="laser_lambda_short"        value="0.1"/>
+      <param name="laser_model_type"          value="likelihood_field"/>
+      <param name="laser_likelihood_max_dist" value="2.0"/>
+      <param name="update_min_d"              value="0.2"/>
+      <param name="update_min_a"              value="0.2"/>
+      <param name="odom_frame_id"             value="$(arg odom_frame_id)"/>
+      <param name="base_frame_id"             value="$(arg base_frame_id)"/>
+      <param name="global_frame_id"           value="$(arg global_frame_id)"/>
+      <param name="resample_interval"         value="1"/>
+      <param name="transform_tolerance"       value="0.2"/>
+      <param name="recovery_alpha_slow"       value="0.0"/>
+      <param name="recovery_alpha_fast"       value="0.0"/>
+      <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
+      <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
+      <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
+      <remap from="scan"                      to="$(arg scan_topic)"/>
+      <remap from="map"                       to="$(arg map_topic)"/>
+      <remap from="static_map"                to="$(arg map_service)"/>
+    </node>
+  </group>
+
+  <!-- Duplicate of the above in case namespace is empty. This is necessary to
+       avoid the "<group> tag has an empty 'ns' attribute" parsing error. -->
+  <group unless="$(eval namespace != '')">
+    <node pkg="amcl" type="amcl" name="amcl" output="screen" >
+      <param name="use_map_topic"             value="$(arg use_map_topic)"/>
+      <param name="odom_model_type"           value="diff-corrected"/>
+      <param name="gui_publish_rate"          value="10.0"/>
+      <param name="save_pose_rate"            value="0.5"/>
+      <param name="laser_max_beams"           value="200"/>
+      <param name="laser_min_range"           value="-1.0"/>
+      <param name="laser_max_range"           value="-1.0"/>
+      <param name="min_particles"             value="500"/>
+      <param name="max_particles"             value="5000"/>
+      <param name="kld_err"                   value="0.09"/>
+      <param name="kld_z"                     value="0.99"/>
+      <param name="odom_alpha1"               value="0.02"/>
+      <param name="odom_alpha2"               value="0.01"/>
+      <param name="odom_alpha3"               value="0.01"/>
+      <param name="odom_alpha4"               value="0.02"/>
+      <param name="laser_z_hit"               value="0.5"/>
+      <param name="laser_z_short"             value="0.05"/>
+      <param name="laser_z_max"               value="0.05"/>
+      <param name="laser_z_rand"              value="0.5"/>
+      <param name="laser_sigma_hit"           value="0.2"/>
+      <param name="laser_lambda_short"        value="0.1"/>
+      <param name="laser_model_type"          value="likelihood_field"/>
+      <param name="laser_likelihood_max_dist" value="2.0"/>
+      <param name="update_min_d"              value="0.2"/>
+      <param name="update_min_a"              value="0.2"/>
+      <param name="odom_frame_id"             value="$(arg odom_frame_id)"/>
+      <param name="base_frame_id"             value="$(arg base_frame_id)"/>
+      <param name="global_frame_id"           value="$(arg global_frame_id)"/>
+      <param name="resample_interval"         value="1"/>
+      <param name="transform_tolerance"       value="0.2"/>
+      <param name="recovery_alpha_slow"       value="0.0"/>
+      <param name="recovery_alpha_fast"       value="0.0"/>
+      <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
+      <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
+      <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
+      <remap from="scan"                      to="$(arg scan_topic)"/>
+      <remap from="map"                       to="$(arg map_topic)"/>
+      <remap from="static_map"                to="$(arg map_service)"/>
+    </node>
+  </group>
+</launch>

Controllers/Packages/amr_startup/launch/includes/ekf.launch.xml

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="tf_prefix" default="" />
+  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_node" clear_params="true" output="screen">
+    <rosparam command="load" file="$(find amr_startup)/config/ekf.yaml" subst_value="true" />
+  </node>
+</launch>

Controllers/Packages/amr_startup/launch/includes/rviz.launch

@@ -0,0 +1,3 @@
+<launch>
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find amr_startup)/rviz/navigation.rviz" required="true" />
+</launch>

Controllers/Packages/amr_startup/launch/includes/spawn_maze.launch.xml

@@ -0,0 +1,5 @@
+<?xml version="1.0"?>
+<launch>
+    <node name="spawn_maze" pkg="gazebo_ros" type="spawn_model" args="-sdf
+      -file $(find amr_startup)/sdf/maze/model.sdf -model walls" output="screen" />
+</launch>

Controllers/Packages/amr_startup/launch/includes/spawn_trolley.launch

@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="robot_x"   default="17.5" />
+    <arg name="robot_y"   default="34.5" />
+    <arg name="robot_yaw" default="1.57079" />
+    <arg name="model" default="trolley" />
+    <node name="spawn_maze" pkg="gazebo_ros" type="spawn_model" 
+          args="-sdf -file $(find factory_ss_demo)models/sehc_trolley/model.sdf -model $(arg model) 
+          -x $(arg robot_x) -y $(arg robot_y) -Y $(arg robot_yaw)" output="screen" />
+
+</launch>

Controllers/Packages/amr_startup/launch/robot_empty_world.launch

@@ -0,0 +1,72 @@
+<?xml version="1.0" ?>
+<launch>
+  <arg name="gui" default="true" />
+  <arg name="world_name" default="worlds/empty.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable, but can also be an absolute path -->
+
+  <arg name="robot_x"   default="0.0" />
+  <arg name="robot_y"   default="0.0" />
+  <arg name="robot_yaw" default="0.0" />
+
+  <arg name="robot_type" default="hook_150" doc="Can be 'hook_150' or 'imr' for now." />
+
+  <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
+
+  <arg name="namespace" default="$(arg tf_prefix)" doc="Namespace to push all topics into."/>
+  
+  <group if="$(eval namespace != '')">
+    <group>
+      <remap from="$(arg namespace)/joint_states"                   to="$(arg namespace)/robot/joint_states" />
+      <remap from="$(arg namespace)/mobile_base_controller/cmd_vel" to="$(arg namespace)/cmd_vel" />
+      <remap from="$(arg namespace)/mobile_base_controller/odom"    to="$(arg namespace)/odom" />
+
+      <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" value="$(arg world_name)"/>
+        <arg name="paused" default="false" />
+        <arg name="use_sim_time" default="true" />
+        <arg name="gui" default="true" />
+        <arg name="debug" default="false" />
+        <arg name="verbose" default="true" /> 
+      </include>
+    </group>
+
+    <group ns="$(arg namespace)">
+      <!-- spawn robot and bring up controllers etc. -->
+      <include file="$(find amr_startup)/launch/robot_gazebo_common.launch">
+        <arg name="robot_x"   value="$(arg robot_x)" />
+        <arg name="robot_y"   value="$(arg robot_y)" />
+        <arg name="robot_yaw" value="$(arg robot_yaw)" />
+        <arg name="robot_type"  value="$(arg robot_type)" />
+        <arg name="tf_prefix" value="$(arg tf_prefix)" />
+      </include>
+    </group>
+  </group>
+
+  <!-- Duplicate of the above in case namespace is empty. This is necessary to
+       avoid the "<group> tag has an empty 'ns' attribute" parsing error. -->
+  <group unless="$(eval namespace != '')">
+    <group>
+      <remap from="joint_states"                   to="robot/joint_states" />
+      <remap from="mobile_base_controller/cmd_vel" to="cmd_vel" />
+      <remap from="mobile_base_controller/odom"    to="odom" />
+
+      <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" value="$(arg world_name)"/>
+        <arg name="paused" default="false" />
+        <arg name="use_sim_time" default="true" />
+        <arg name="gui" default="true" />
+        <arg name="debug" default="false" />
+        <arg name="verbose" default="true" /> 
+      </include>
+    </group>
+
+
+    <include file="$(find amr_startup)/launch/robot_gazebo_common.launch">
+      <arg name="robot_x"   value="$(arg robot_x)" />
+      <arg name="robot_y"   value="$(arg robot_y)" />
+      <arg name="robot_yaw" value="$(arg robot_yaw)" />
+        <arg name="robot_type"  value="$(arg robot_type)" />
+      <arg name="tf_prefix" value="$(arg tf_prefix)" />
+    </include>
+  </group>
+
+</launch>

Controllers/Packages/amr_startup/launch/robot_gazebo_common.launch

@@ -0,0 +1,59 @@
+<?xml version="1.0" ?>
+<launch>
+  <arg name="robot_x"   default="0.0" />
+  <arg name="robot_y"   default="0.0" />
+  <arg name="robot_yaw" default="0.0" />
+  <arg name="robot_type" default="hool_150" doc="Can be 'hool_150' or 'imr' for now." />
+  <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
+  <arg name="prefix" value="$(arg tf_prefix)/" if="$(eval tf_prefix != '')" /> <!-- $(arg prefix) is used in all the config files! TODO: For multiple robots, create groups when loading the parameters to overwrite the arg? -->
+  <arg name="prefix" value=""                  unless="$(eval tf_prefix != '')" />
+  <arg name="model_name" default="robot" doc="Name of the Gazebo robot model (needs to be different for each robot)" />
+
+
+  <param name="rosconsole_config_file" value="$(find amr_startup)/rosconsole.config"/>
+
+  <!-- Load URDF -->
+  <include file="$(find robot_description)/launch/upload_urdf.launch">
+      <arg name="robot_type" value="$(arg robot_type)" />
+      <arg name="tf_prefix" value="$(arg tf_prefix)" />
+  </include>
+
+  <!-- Spawn the robot into Gazebo -->
+  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model $(arg model_name) 
+      -x $(arg robot_x) -y $(arg robot_y) -Y $(arg robot_yaw)" output="screen" />
+
+  <!-- Load ros_control controller configurations -->
+  <rosparam file="$(find robot_description)/config/joint_state_controller.yaml" command="load" />
+  <rosparam file="$(find robot_description)/config/diffdrive_controller.yaml" command="load" subst_value="true" />
+  <rosparam file="$(find robot_description)/config/pid_gains.yaml" command="load" />
+
+  <!-- Start the controllers -->
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" output="screen"
+      args="joint_state_controller mobile_base_controller"/>
+
+  <!-- Add passive + mimic joints to joint_states topic -->
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+    <rosparam param="source_list">[robot/joint_states]</rosparam>
+    <param name="rate" value="200.0" />
+  </node>
+
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" />
+
+  <node name="rqt_robot_steering" pkg="rqt_robot_steering" type="rqt_robot_steering">
+    <param name="default_topic" value="cmd_vel"/>
+    <param name="default_vx_max" value="1.0" />
+    <param name="default_vx_min" value="-1.0" />
+    <param name="default_vw_max" value="1.5" />
+    <param name="default_vw_min" value="-1.5" />
+  </node>
+
+  <!-- EKF -->
+  <include file="$(find amr_startup)/launch/includes/ekf.launch.xml">
+      <arg name="tf_prefix" value="$(arg prefix)" />
+  </include>
+
+  <!-- create combined scan topic (like on robot real) -->
+  <node pkg="topic_tools" type="relay" name="b_scan_relay" args="b_scan scan"/>
+  <node pkg="topic_tools" type="relay" name="f_scan_relay" args="f_scan scan"/>
+
+</launch>

Controllers/Packages/amr_startup/launch/robot_maze_world.launch

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="gui" default="true" />
+  <arg name="robot_type" default="T800" doc="Can be 'hook_150' or 'imr' for now." />
+  <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
+
+  <include file="$(find amr_startup)/launch/robot_empty_world.launch">
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="robot_type" value="$(arg robot_type)" />
+    <arg name="tf_prefix" value="$(arg tf_prefix)" />
+  </include>
+
+  <include file="$(find amr_startup)/launch/includes/spawn_maze.launch.xml" />
+  <!-- <include file="$(find amr_startup)/launch/includes/rviz.launch" /> -->
+</launch>

Controllers/Packages/amr_startup/launch/sehc_world.launch

@@ -0,0 +1,18 @@
+<launch>
+    <arg name="gui" default="true" />
+    <arg name="robot_type" default="imr" doc="Can be 'hook_150' or 'imr' for now." />
+    <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
+    <include file="$(find amr_startup)/launch/robot_empty_world.launch">   
+        <arg name='world_name' value="$(find factory_ss_demo)/worlds/factory_ss_demo.world"/>
+        <arg name="robot_x" value="5.5"/>
+        <arg name="robot_y" value="34.5"/>
+        <arg name="robot_yaw" value="0.0"/>
+        <arg name="gui" value="$(arg gui)" />
+        <arg name="robot_type" value="$(arg robot_type)" />
+        <arg name="tf_prefix" value="$(arg tf_prefix)" />
+    </include>
+
+    <include file="$(find amr_startup)/launch/includes/spawn_trolley.launch"/> 
+
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find amr_startup)/rviz/navigation.rviz" required="false" />
+</launch>

Controllers/Packages/amr_startup/launch/start_maps.launch

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="map_file" default="$(find managerments)/maps/maze.yaml" doc="Path to a map .yaml file (required)." />
+  <arg name="virtual_walls_map_file" default="$(arg map_file)" doc="Path to a virtual walls map .yaml file (optional)." />
+  <arg name="with_virtual_walls" default="true" />
+
+  <node name="static_map_server" pkg="map_server" type="map_server" args="$(arg map_file)" ns="/" output="screen">
+    <param name="frame_id" type="string" value="map"/>
+  </node>
+
+  <node if="$(arg with_virtual_walls)" name="virtual_walls_map_server" pkg="map_server" type="map_server" args="$(arg virtual_walls_map_file)" ns="/virtual_walls" output="screen">
+    <param name="frame_id" type="string" value="map"/>
+  </node>
+</launch>

Controllers/Packages/amr_startup/launch/ware_houses_world.launch

@@ -0,0 +1,16 @@
+<launch>
+    <arg name="gui" default="true" />
+    <arg name="robot_type" default="imr" doc="Can be 'hook_150' or 'imr' for now." />
+    <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
+    <include file="$(find amr_startup)/launch/robot_empty_world.launch">   
+        <arg name='world_name' value="$(find dynamic_logistics_warehouse)/worlds/warehouse.world"/>
+        <arg name="robot_x" value="12.0"/>
+        <arg name="robot_y" value="22.0"/>
+        <arg name="robot_yaw" value="3.141592654"/>
+        <arg name="gui" value="$(arg gui)" />
+        <arg name="robot_type" value="$(arg robot_type)" />
+        <arg name="tf_prefix" value="$(arg tf_prefix)" />
+    </include>
+
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find amr_startup)/rviz/navigation.rviz" required="true" />
+</launch>

Controllers/Packages/amr_startup/package.xml

@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>amr_startup</name>
+  <version>0.0.0</version>
+  <description>The amr_startup package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="robotics@todo.todo">robotics</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/amr_startup</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Controllers/Packages/amr_startup/rosconsole.config

@@ -0,0 +1,2 @@
+log4j.logger.ros.tf=ERROR
+log4j.logger.ros.tf2=ERROR

Controllers/Packages/amr_startup/rviz/navigation.rviz

@@ -0,0 +1,643 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Grid1/Offset1
+        - /TF1/Frames1
+        - /Local Map1
+        - /Local Map1/Plan1
+      Splitter Ratio: 0.37295082211494446
+    Tree Height: 682
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: Back LaserScan
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 5
+        Y: 5
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 100
+      Reference Frame: map
+      Value: true
+    - Alpha: 0.4000000059604645
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 0.5
+      Class: rviz/RobotModel
+      Collision Enabled: true
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        back_nanoscan3_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        back_nanoscan3_sensor_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        bl_caster_rotation_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        bl_caster_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        br_caster_rotation_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        br_caster_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fl_caster_rotation_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fl_caster_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fr_caster_rotation_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fr_caster_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_nanoscan3_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_nanoscan3_sensor_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        imu_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        lifting_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        surface:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        back_nanoscan3_base_link:
+          Value: false
+        back_nanoscan3_sensor_link:
+          Value: false
+        base_footprint:
+          Value: true
+        base_link:
+          Value: false
+        bl_caster_rotation_link:
+          Value: false
+        bl_caster_wheel_link:
+          Value: false
+        br_caster_rotation_link:
+          Value: false
+        br_caster_wheel_link:
+          Value: false
+        camera_link:
+          Value: false
+        fl_caster_rotation_link:
+          Value: false
+        fl_caster_wheel_link:
+          Value: false
+        fr_caster_rotation_link:
+          Value: false
+        fr_caster_wheel_link:
+          Value: false
+        front_nanoscan3_base_link:
+          Value: false
+        front_nanoscan3_sensor_link:
+          Value: false
+        imu_frame:
+          Value: false
+        imu_link:
+          Value: false
+        left_wheel_link:
+          Value: false
+        lifting_link:
+          Value: false
+        map:
+          Value: true
+        odom:
+          Value: true
+        right_wheel_link:
+          Value: false
+        surface:
+          Value: false
+      Marker Alpha: 1
+      Marker Scale: 0.5
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          odom:
+            base_footprint:
+              base_link:
+                back_nanoscan3_base_link:
+                  back_nanoscan3_sensor_link:
+                    {}
+                bl_caster_rotation_link:
+                  bl_caster_wheel_link:
+                    {}
+                br_caster_rotation_link:
+                  br_caster_wheel_link:
+                    {}
+                camera_link:
+                  {}
+                fl_caster_rotation_link:
+                  fl_caster_wheel_link:
+                    {}
+                fr_caster_rotation_link:
+                  fr_caster_wheel_link:
+                    {}
+                front_nanoscan3_base_link:
+                  front_nanoscan3_sensor_link:
+                    {}
+                imu_link:
+                  imu_frame:
+                    {}
+                left_wheel_link:
+                  {}
+                lifting_link:
+                  surface:
+                    {}
+                right_wheel_link:
+                  {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 204; 0; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: Front LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 6
+      Size (m): 0.09000000357627869
+      Style: Points
+      Topic: f_scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 0; 255; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: Back LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /b_scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Arrow Length: 0.20000000298023224
+      Axes Length: 0.30000001192092896
+      Axes Radius: 0.009999999776482582
+      Class: rviz/PoseArray
+      Color: 0; 192; 0
+      Enabled: false
+      Head Length: 0.07000000029802322
+      Head Radius: 0.029999999329447746
+      Name: Amcl Particle Swarm
+      Queue Size: 10
+      Shaft Length: 0.23000000417232513
+      Shaft Radius: 0.009999999776482582
+      Shape: Arrow (Flat)
+      Topic: particlecloud
+      Unreliable: false
+      Value: false
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 0.5
+          Class: rviz/Map
+          Color Scheme: costmap
+          Draw Behind: true
+          Enabled: true
+          Name: Costmap
+          Topic: /global_costmap/costmap
+          Unreliable: false
+          Use Timestamp: false
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 25; 255; 0
+          Enabled: true
+          Head Diameter: 0.30000001192092896
+          Head Length: 0.20000000298023224
+          Length: 0.20000000298023224
+          Line Style: Lines
+          Line Width: 0.05000000074505806
+          Name: Plan
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0.8999999761581421
+          Pose Color: 255; 85; 255
+          Pose Style: Axes
+          Queue Size: 10
+          Radius: 0.019999999552965164
+          Shaft Diameter: 0.10000000149011612
+          Shaft Length: 0.10000000149011612
+          Topic: /global_costmap/costmap/plan
+          Unreliable: false
+          Value: true
+        - Alpha: 1
+          Class: rviz/Polygon
+          Color: 255; 0; 0
+          Enabled: true
+          Name: Footprint
+          Queue Size: 10
+          Topic: /global_costmap/costmap/footprint
+          Unreliable: false
+          Value: true
+      Enabled: true
+      Name: Global Map
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 0.699999988079071
+          Class: rviz/Map
+          Color Scheme: map
+          Draw Behind: false
+          Enabled: true
+          Name: Costmap
+          Topic: /local_costmap/costmap
+          Unreliable: false
+          Use Timestamp: false
+          Value: true
+        - Alpha: 1
+          Class: rviz/Polygon
+          Color: 25; 255; 0
+          Enabled: true
+          Name: Footprint
+          Queue Size: 10
+          Topic: /local_costmap/costmap/footprint
+          Unreliable: false
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 239; 41; 41
+          Enabled: true
+          Head Diameter: 0.30000001192092896
+          Head Length: 0.20000000298023224
+          Length: 0.029999999329447746
+          Line Style: Billboards
+          Line Width: 0.029999999329447746
+          Name: Plan
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0.6000000238418579
+          Pose Color: 255; 85; 255
+          Pose Style: Axes
+          Queue Size: 10
+          Radius: 0.009999999776482582
+          Shaft Diameter: 0.10000000149011612
+          Shaft Length: 0.10000000149011612
+          Topic: /local_costmap/costmap/plan
+          Unreliable: false
+          Value: true
+        - Class: rviz/Group
+          Displays:
+            - Alpha: 1
+              Axes Length: 0.10000000149011612
+              Axes Radius: 0.029999999329447746
+              Class: rviz/Pose
+              Color: 0; 0; 0
+              Enabled: true
+              Head Length: 0.05000000074505806
+              Head Radius: 0.05000000074505806
+              Name: SubGoalPose
+              Queue Size: 10
+              Shaft Length: 0.029999999329447746
+              Shaft Radius: 0.029999999329447746
+              Shape: Arrow
+              Topic: /amr_node/sub_goal
+              Unreliable: false
+              Value: true
+            - Alpha: 1
+              Axes Length: 0.30000001192092896
+              Axes Radius: 0.10000000149011612
+              Class: rviz/Pose
+              Color: 252; 233; 79
+              Enabled: false
+              Head Length: 0.05000000074505806
+              Head Radius: 0.05000000074505806
+              Name: ClosetPose
+              Queue Size: 10
+              Shaft Length: 0.029999999329447746
+              Shaft Radius: 0.029999999329447746
+              Shape: Arrow
+              Topic: /amr_node/closet_robot_goal
+              Unreliable: false
+              Value: false
+            - Alpha: 1
+              Axes Length: 0.05000000074505806
+              Axes Radius: 0.019999999552965164
+              Class: rviz/Pose
+              Color: 173; 127; 168
+              Enabled: false
+              Head Length: 0.05000000074505806
+              Head Radius: 0.05000000074505806
+              Name: Look A Head Pose
+              Queue Size: 10
+              Shaft Length: 0.05000000074505806
+              Shaft Radius: 0.029999999329447746
+              Shape: Axes
+              Topic: /amr_node/lookahead_point
+              Unreliable: false
+              Value: false
+            - Alpha: 1
+              Arrow Length: 0.30000001192092896
+              Axes Length: 0.30000001192092896
+              Axes Radius: 0.029999999329447746
+              Class: rviz/PoseArray
+              Color: 0; 0; 0
+              Enabled: true
+              Head Length: 0.07000000029802322
+              Head Radius: 0.05000000074505806
+              Name: reached intermediated goals
+              Queue Size: 10
+              Shaft Length: 0.23000000417232513
+              Shaft Radius: 0.029999999329447746
+              Shape: Axes
+              Topic: /amr_node/reached_intermediate_goals
+              Unreliable: false
+              Value: true
+            - Alpha: 1
+              Buffer Length: 1
+              Class: rviz/Path
+              Color: 245; 121; 0
+              Enabled: true
+              Head Diameter: 0.30000001192092896
+              Head Length: 0.019999999552965164
+              Length: 0.029999999329447746
+              Line Style: Billboards
+              Line Width: 0.009999999776482582
+              Name: transform plan
+              Offset:
+                X: 0
+                Y: 0
+                Z: 0.5
+              Pose Color: 138; 226; 52
+              Pose Style: Axes
+              Queue Size: 10
+              Radius: 0.019999999552965164
+              Shaft Diameter: 0.009999999776482582
+              Shaft Length: 0.009999999776482582
+              Topic: /amr_node/transformed_plan
+              Unreliable: false
+              Value: true
+            - Class: rviz/Marker
+              Enabled: false
+              Marker Topic: /amr_node/PredictiveTrajectory/cost_left_goals
+              Name: L
+              Namespaces:
+                {}
+              Queue Size: 100
+              Value: false
+            - Class: rviz/Marker
+              Enabled: false
+              Marker Topic: /amr_node/PredictiveTrajectory/cost_right_goals
+              Name: R
+              Namespaces:
+                {}
+              Queue Size: 100
+              Value: false
+          Enabled: true
+          Name: Trajectory
+      Enabled: true
+      Name: Local Map
+    - Alpha: 1
+      Axes Length: 0.05000000074505806
+      Axes Radius: 0.014999999664723873
+      Class: rviz/Pose
+      Color: 46; 52; 54
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Queue Size: 10
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Axes
+      Topic: /amr_node/current_goal
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/MoveCamera
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/Select
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: move_base_simple/goal
+    - Class: rviz/Measure
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: clicked_point
+  Value: true
+  Views:
+    Current:
+      Angle: -3.150002956390381
+      Class: rviz/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Scale: -191.32327270507812
+      Target Frame: base_link
+      X: 0.8287617564201355
+      Y: 0.9812669157981873
+    Saved:
+      - Angle: -34.55989074707031
+        Class: rviz/TopDownOrtho
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.05999999865889549
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Invert Z Axis: false
+        Name: TopDownOrtho
+        Near Clip Distance: 0.009999999776482582
+        Scale: -132.97349548339844
+        Target Frame: base_link
+        X: 34.338645935058594
+        Y: 35.28913879394531
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 833
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001fa000002e7fc0200000008fb000000100044006900730070006c006100790073010000003d000002e7000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d00650000000000000007520000041800fffffffb0000000800540069006d0065010000000000000450000000000000000000000235000002e700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1077
+  X: 0
+  Y: 0

Controllers/Packages/amr_startup/sdf/maze/model.config

@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+    <name>maze</name>
+    <version>1.0</version>
+    <sdf version="1.6">model.sdf</sdf>
+    <author>
+        <name>Martin Günther</name>
+        <email>martin.guenther@dfki.de</email>
+    </author>
+    <description></description>
+</model>

Controllers/Packages/amr_startup/sdf/maze/model.sdf

@@ -0,0 +1,345 @@
+<?xml version='1.0'?>
+<sdf version='1.6'>
+  <model name='maze'>
+    <pose frame=''>-0.078283 0.098984 0 0 -0 0</pose>
+    <link name='Wall_0'>
+      <collision name='Wall_0_Collision'>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_0_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>0.030536 9.925 0 0 -0 0</pose>
+    </link>
+    <link name='Wall_1'>
+      <collision name='Wall_1_Collision'>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_1_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>9.95554 0 0 0 0 -1.5708</pose>
+    </link>
+    <link name='Wall_2'>
+      <collision name='Wall_2_Collision'>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_2_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>0.030536 -9.925 0 0 -0 3.14159</pose>
+    </link>
+    <link name='Wall_24'>
+      <collision name='Wall_24_Collision'>
+        <geometry>
+          <box>
+            <size>1.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_24_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>1.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>5.35089 3.21906 0 0 -0 3.14159</pose>
+    </link>
+    <link name='Wall_25'>
+      <collision name='Wall_25_Collision'>
+        <geometry>
+          <box>
+            <size>5.25 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_25_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>5.25 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>4.67589 5.76906 0 0 -0 1.5708</pose>
+    </link>
+    <link name='Wall_27'>
+      <collision name='Wall_27_Collision'>
+        <geometry>
+          <box>
+            <size>5.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_27_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>5.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>7.10914 4.73454 0 0 0 -1.5708</pose>
+    </link>
+    <link name='Wall_28'>
+      <collision name='Wall_28_Collision'>
+        <geometry>
+          <box>
+            <size>3 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_28_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>3 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>8.53414 2.05954 0 0 -0 0</pose>
+    </link>
+    <link name='Wall_3'>
+      <collision name='Wall_3_Collision'>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_3_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>-9.89446 0 0 0 -0 1.5708</pose>
+    </link>
+    <link name='Wall_30'>
+      <collision name='Wall_30_Collision'>
+        <geometry>
+          <box>
+            <size>5.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_30_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>5.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>-4.35914 -2.82889 0 0 0 -1.5708</pose>
+    </link>
+    <link name='Wall_31'>
+      <collision name='Wall_31_Collision'>
+        <geometry>
+          <box>
+            <size>5.75 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_31_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>5.75 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>-7.15914 -5.50389 0 0 -0 3.14159</pose>
+    </link>
+    <link name='Wall_5'>
+      <collision name='Wall_5_Collision'>
+        <geometry>
+          <box>
+            <size>16 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_5_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>16 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>-1.89911 1.86906 0 0 -0 0</pose>
+    </link>
+    <link name='Wall_6'>
+      <collision name='Wall_6_Collision'>
+        <geometry>
+          <box>
+            <size>1.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_6_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>1.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>6.02589 2.54406 0 0 -0 1.5708</pose>
+    </link>
+    <link name='Wall_8'>
+      <collision name='Wall_8_Collision'>
+        <geometry>
+          <box>
+            <size>0.15 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_8_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>6.02589 3.21906 0 0 -0 0</pose>
+    </link>
+    <static>1</static>
+  </model>
+</sdf>

Controllers/Packages/cititruck_description/CHANGELOG.rst

@@ -0,0 +1,133 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package cititruck_description
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.1.7 (2023-01-20)
+------------------
+* Don't set cmake_policy CMP0048
+* Contributors: Martin Günther
+
+1.1.6 (2022-06-02)
+------------------
+* URDF: Downsize inertia box, move to lower back
+* URDF: Pull out inertia properties
+* URDF: Update masses according to data sheet
+* URDF: Add mir_250
+* Add arg mir_type to launch files and urdfs
+* Add mir_250 meshes
+* URDF: Make wheels black
+* Add mir_100_v1.urdf.xacro for backwards compatibility
+* Rename mir_100 -> mir
+* Refactor URDF to prepare for MiR250 support
+* Gazebo: Don't manually specify wheel params for diffdrive controller
+* Simplify mir_100 collision mesh further
+* Contributors: Martin Günther
+
+1.1.5 (2022-02-11)
+------------------
+* Remove xacro comment to work around xacro bug
+  Since xacro 1.14.11, xacro now also evaluates expressions in comments
+  and throws an error if the substition argument is undefined. In xacro
+  1.14.12, this error was changed to a warning.
+  This commit removes that warning.
+  Workaround for https://github.com/ros/xacro/issues/309 .
+* xacro: drop --inorder option
+  In-order processing became default in ROS Melodic.
+* Add gazebo_plugins to dependency list (`#103 <https://github.com/DFKI-NI/mir_robot/issues/103>`_)
+  This is needed for the ground truth pose via p3d plugin.
+* Contributors: Martin Günther, moooeeeep
+
+1.1.4 (2021-12-10)
+------------------
+* Replace gazebo_plugins IMU with hector_gazebo_plugins
+* Use cylinders instead of STLs for wheel collision geometries
+  Fixes `#99 <https://github.com/DFKI-NI/mir_robot/issues/99>`_.
+* mir_debug_urdf.launch: Fix GUI display
+* Contributors: Martin Günther
+
+1.1.3 (2021-06-11)
+------------------
+* Merge branch 'melodic-2.8' into noetic
+* Rename tf frame and topic 'odom_comb' -> 'odom'
+  This is how they are called on the real MiR since MiR software 2.0.
+* Contributors: Martin Günther
+
+1.1.2 (2021-05-12)
+------------------
+* Fix laser scan frame_id with gazebo_plugins 2.9.2
+* Contributors: Martin Günther
+
+1.1.1 (2021-02-11)
+------------------
+* Add prepend_prefix_to_laser_frame to URDF and launch files
+  Fixes `#65 <https://github.com/DFKI-NI/mir_robot/issues/65>`_.
+* Add tf_prefix to URDF and launch files
+* Fix typo in robot_namespace
+* Add missing 'xacro:' xml namespace prefixes
+  Macro calls without 'xacro:' prefix are deprecated in Melodic and will
+  be forbidden in Noetic.
+* Contributors: Martin Günther
+
+1.1.0 (2020-06-30)
+------------------
+* Initial release into noetic
+* Contributors: Martin Günther
+
+1.0.6 (2020-06-30)
+------------------
+* Update to non-deprecated robot_state_publisher node
+* Set cmake_policy CMP0048 to fix warning
+* Contributors: Martin Günther
+
+1.0.5 (2020-05-01)
+------------------
+* Switch from Gazebo GPU laser to normal laser plugin
+  The GPU laser plugin has caused multiple people problems before, because
+  it is not compatible with all GPUS: `#1 <https://github.com/DFKI-NI/mir_robot/issues/1>`_
+  `#32 <https://github.com/DFKI-NI/mir_robot/issues/32>`_
+  `#46 <https://github.com/DFKI-NI/mir_robot/issues/46>`_
+  `#52 <https://github.com/DFKI-NI/mir_robot/issues/52>`_
+  The normal laser plugin directly uses the physics engine, so it doesn't
+  depend on any specific GPU. Also, it doesn't slow down the simulation
+  noticeably (maybe 1-2%).
+* Contributors: Martin Günther
+
+1.0.4 (2019-05-06)
+------------------
+* Add legacyModeNS param to gazebo_ros_control plugin
+  This enables the new behavior of the plugin (pid_gains parameter are now
+  in the proper namespace).
+* re-added gazebo friction parameters for the wheels (`#19 <https://github.com/DFKI-NI/mir_robot/issues/19>`_)
+* Contributors: Martin Günther, niniemann
+
+1.0.3 (2019-03-04)
+------------------
+* Merge pull request `#16 <https://github.com/DFKI-NI/mir_robot/issues/16>`_ from niniemann/add-prefix-argument-to-configs
+  Add prefix argument to configs
+* removed prefix from plugin frameName in sick urdf
+  The gazebo plugins automatically use tf_prefix, even if none is set
+  (in that case it defaults to the robot namespace). That's why we can
+  remove the prefix from the plugins configuration, assuming that the
+  robot namespace will be equal to the prefix.
+* adds $(arg prefix) to a lot of configs
+  This is an important step to be able to re-parameterize move base,
+  the diffdrive controller, ekf, amcl and the costmaps for adding a
+  tf prefix to the robots links
+* workaround eval in xacro for indigo support
+* adds tf_prefix argument to imu.gazebo.urdf.xacro
+* Add TFs for ultrasound sensors
+* Contributors: Martin Günther, Nils Niemann
+
+1.0.2 (2018-07-30)
+------------------
+
+1.0.1 (2018-07-17)
+------------------
+* gazebo: Remove leading slashes in TF frames
+  TF2 doesn't like it (e.g., robot_localization).
+* Contributors: Martin Günther
+
+1.0.0 (2018-07-12)
+------------------
+* Initial release
+* Contributors: Martin Günther

Controllers/Packages/cititruck_description/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.5.1)
+project(cititruck_description)
+
+find_package(catkin REQUIRED COMPONENTS
+  roslaunch
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package()
+
+#############
+## Install ##
+#############
+
+# Mark other files for installation (e.g. launch and bag files, etc.)
+install(DIRECTORY
+  config
+  launch
+  meshes
+  rviz
+  urdf
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+roslaunch_add_file_check(launch)

Controllers/Packages/cititruck_description/config/joint_state_controller.yaml

@@ -0,0 +1,4 @@
+# Publish all joint states -----------------------------------
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50

Controllers/Packages/cititruck_description/config/steerdrive_controller.yaml

@@ -0,0 +1,55 @@
+# -----------------------------------
+mobile_base_controller:
+  type        : "steer_drive_controller/SteerDriveController"
+  rear_wheel: '$(arg prefix)steer2sd_wheel_joint'
+  front_steer: '$(arg prefix)base2steer_joint'
+  publish_rate: 41.2               # this is what the real MiR platform publishes (default: 50)
+  # These covariances are exactly what the real MiR platform publishes
+  pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+  twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+  enable_odom_tf: false
+
+  # Wheel separation and diameter. These are both optional.
+  # diff_drive_controller will attempt to read either one or both from the
+  # URDF if not specified as a parameter.
+  # We don't set the value here because it's different for each MiR type (100, 250, ...), and
+  # the plugin figures out the correct values.
+  wheel_separation : 1.3268
+  wheel_radius : 0.125
+
+  # Wheel separation and radius multipliers
+  wheel_separation_multiplier: 1.0 # default: 1.0
+  wheel_radius_multiplier    : 1.0 # default: 1.0
+
+  # Steer position angle multipliers for fine tuning.
+  steer_pos_multiplier       : 1.0
+
+  # Velocity commands timeout [s], default 0.5
+  cmd_vel_timeout: 0.25
+
+  # frame_ids (same as real MiR platform)
+  base_frame_id: $(arg prefix)base_footprint # default: base_link
+  odom_frame_id: $(arg prefix)odom           # default: odom
+
+  # Velocity and acceleration limits
+  # Whenever a min_* is unspecified, default to -max_*
+  linear:
+    x:
+      has_velocity_limits    : true
+      max_velocity           : 1.5  # m/s
+      min_velocity           : -1.5 # m/s
+      has_acceleration_limits: true
+      max_acceleration       : 3.0 # m/s^2
+      min_acceleration       : -3.0 # m/s^2
+      has_jerk_limits        : false
+      max_jerk               : 5.0 # m/s^3
+  angular:
+    z:
+      has_velocity_limits    : true
+      max_velocity           : 2.0  # rad/s
+      min_velocity           : -2.0
+      has_acceleration_limits: true
+      max_acceleration       : 1.5  # rad/s^2
+      min_acceleration       : -1.5
+      has_jerk_limits        : false
+      max_jerk               : 2.5 # rad/s^3

Controllers/Packages/cititruck_description/launch/cititruck_debug_urdf.launch

@@ -0,0 +1,20 @@
+<?xml version="1.0" ?>
+
+<launch>
+  <arg name="robot_type" default="demo" doc="" />
+  <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
+  <arg name="gui" default="false" />
+
+  <!-- load cititruck URDF -->
+  <include file="$(find cititruck_description)/launch/upload_cititruck_urdf.launch">
+    <arg name="tf_prefix" value="$(arg tf_prefix)" />
+    <arg name="robot_type" value="$(arg robot_type)" />
+  </include>
+
+  <node if="$(arg gui)"     name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
+  <node unless="$(arg gui)" name="joint_state_publisher" pkg="joint_state_publisher"     type="joint_state_publisher" />
+
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find cititruck_description)/rviz/cititruck_description.rviz" required="true" />
+</launch>

Controllers/Packages/cititruck_description/launch/debug.launch

@@ -0,0 +1,5 @@
+<?xml version="1.0" ?>
+
+<launch>
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find cititruck_description)/rviz/cititruck_description.rviz" required="true" />
+</launch>

Controllers/Packages/cititruck_description/launch/upload_cititruck_urdf.launch

@@ -0,0 +1,7 @@
+<?xml version="1.0" ?>
+<launch>
+  <arg name="robot_type"  default="demo" doc="" />
+  <arg name="tf_prefix" default="" doc="TF prefix to use for all of the cititruck's TF frames"/>
+  
+  <param name="robot_description" command="$(find xacro)/xacro $(find cititruck_description)/urdf/cititruck.urdf.xacro robot_type:=$(arg robot_type) tf_prefix:=$(arg tf_prefix)" />
+</launch>

Controllers/Packages/cititruck_description/meshes/asus_camera_simple.dae

@@ -0,0 +1,136 @@
+<?xml version="1.0" encoding="utf-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+  <asset>
+    <contributor>
+      <author>Stefan Kohlbrecher</author>
+      <authoring_tool>Blender 2.70.5 commit date:2014-06-04, commit time:10:44, hash:fc1c763</authoring_tool>
+    </contributor>
+    <created>2014-06-11T13:30:44</created>
+    <modified>2014-06-11T13:30:44</modified>
+    <unit name="meter" meter="1"/>
+    <up_axis>Z_UP</up_axis>
+  </asset>
+  <library_images/>
+  <library_effects>
+    <effect id="Material_001-effect">
+      <profile_COMMON>
+        <technique sid="common">
+          <phong>
+            <emission>
+              <color sid="emission">0 0 0 1</color>
+            </emission>
+            <ambient>
+              <color sid="ambient">0 0 0 1</color>
+            </ambient>
+            <diffuse>
+              <color sid="diffuse">0.007095437 0.007095437 0.007095437 1</color>
+            </diffuse>
+            <specular>
+              <color sid="specular">0.25 0.25 0.25 1</color>
+            </specular>
+            <shininess>
+              <float sid="shininess">50</float>
+            </shininess>
+            <index_of_refraction>
+              <float sid="index_of_refraction">1</float>
+            </index_of_refraction>
+          </phong>
+        </technique>
+      </profile_COMMON>
+    </effect>
+    <effect id="Material_002-effect">
+      <profile_COMMON>
+        <technique sid="common">
+          <phong>
+            <emission>
+              <color sid="emission">0 0 0 1</color>
+            </emission>
+            <ambient>
+              <color sid="ambient">0 0 0 1</color>
+            </ambient>
+            <diffuse>
+              <color sid="diffuse">0.512 0.512 0.512 1</color>
+            </diffuse>
+            <specular>
+              <color sid="specular">0.4195 0.4195 0.4195 1</color>
+            </specular>
+            <shininess>
+              <float sid="shininess">5</float>
+            </shininess>
+            <index_of_refraction>
+              <float sid="index_of_refraction">1</float>
+            </index_of_refraction>
+          </phong>
+        </technique>
+      </profile_COMMON>
+    </effect>
+  </library_effects>
+  <library_materials>
+    <material id="Material_001-material" name="Material_001">
+      <instance_effect url="#Material_001-effect"/>
+    </material>
+    <material id="Material_002-material" name="Material_002">
+      <instance_effect url="#Material_002-effect"/>
+    </material>
+  </library_materials>
+  <library_geometries>
+    <geometry id="Cube_001-mesh" name="Cube.001">
+      <mesh>
+        <source id="Cube_001-mesh-positions">
+          <float_array id="Cube_001-mesh-positions-array" count="168">-0.9601472 1.38267 0.3078527 -0.9601472 1.38267 -0.3013365 -0.9601472 -3.015511 -0.3013365 -0.9601472 -3.015511 0.3078527 -0.02340704 1.382557 0.3078527 -0.02330875 1.382666 -0.3013365 -0.02331 -3.015507 -0.3013365 -0.02340704 -3.015511 0.3078527 -0.02340704 -1.737524 0 -0.02340704 0.1250084 -0.165674 -0.02340704 0.05638432 0 -0.02340704 -0.09674459 0 -0.02340704 -0.1653691 -0.1656743 -0.02340704 0.1250084 0.1656737 -0.02340704 -0.1653691 0.1656738 -0.02340704 -0.4967176 -0.165674 -0.02340704 -0.5653417 0 -0.02340704 -1.268926 0 -0.02340704 -0.4967176 0.1656737 -0.02340704 -1.337551 -0.1656743 -0.02340704 -1.337551 0.1656738 -0.02340704 -1.668899 0.1656737 -0.02340704 -1.668899 -0.165674 -0.02340704 -1.503225 -0.2342988 -0.02340704 -0.3310434 0.2342985 -0.02340704 -0.3310434 -0.2342988 -0.02340704 -1.503225 0.2342985 -0.02340704 0.2906828 -0.2342988 -0.02340704 0.2906827 0.2342985 -0.02340704 0.456357 0.1656738 -0.02340704 0.5249815 0 -0.02340704 0.456357 -0.1656743 -0.1343017 0.5249815 -1.34731e-7 -0.1343017 0.456357 0.1656738 -0.1343017 0.2906827 0.2342983 -0.1343017 0.456357 -0.1656745 -0.1343011 0.1250084 0.1656737 -0.1343011 0.2906827 -0.234299 -0.1343017 0.05638402 -1.21696e-7 -0.1343011 0.1250084 -0.165674 -0.1343017 -0.09674459 -1.34731e-7 -0.1343017 -0.1653691 0.1656738 -0.1343017 -0.3310434 0.2342983 -0.1343017 -0.1653691 -0.1656745 -0.1343011 -0.4967176 0.1656737 -0.1343011 -0.3310434 -0.234299 -0.1343017 -0.565342 -1.21696e-7 -0.1343011 -0.4967176 -0.165674 -0.1343017 -1.268926 -1.34731e-7 -0.1343017 -1.337551 0.1656738 -0.1343017 -1.503225 0.2342983 -0.1343017 -1.337551 -0.1656745 -0.1343011 -1.668899 0.1656737 -0.1343011 -1.503225 -0.234299 -0.1343017 -1.737524 -1.21696e-7 -0.1343011 -1.668899 -0.165674</float_array>
+          <technique_common>
+            <accessor source="#Cube_001-mesh-positions-array" count="56" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="Cube_001-mesh-normals">
+          <float_array id="Cube_001-mesh-normals-array" count="321">-1 0 0 1.21024e-4 1 0 1 -7.84187e-5 1.8932e-4 1 -1.00075e-4 4.14522e-5 1 -1.34163e-4 -5.55722e-5 1 -2.47756e-5 1.61396e-4 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 0.9999989 -2.84171e-7 0.001454412 1 5.79847e-5 1.39987e-4 1 3.83854e-5 1.59349e-4 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 0 -1 -6.26192e-6 0 0 -1 0 0 1 2.48287e-7 -0.9238793 -0.3826841 -4.21495e-7 -0.9238796 0.3826832 -1.63916e-6 -0.3826835 0.9238796 0 0.3826856 0.9238786 -2.76314e-6 0.9238795 0.3826837 -1.90774e-6 0.9238794 -0.3826838 1.61387e-6 -0.3826832 -0.9238796 0 0.3826839 -0.9238793 0 -0.9238793 -0.3826841 -7.02491e-7 -0.9238796 0.3826833 -1.73282e-6 -0.3826835 0.9238796 0 0.3826858 0.9238786 -2.90364e-6 0.9238795 0.3826838 -2.06292e-6 0.9238793 -0.3826838 1.61387e-6 -0.3826834 -0.9238796 0 0.3826839 -0.9238793 -9.93149e-7 -0.9238784 -0.3826862 -1.68598e-6 -0.9238788 0.3826854 -1.73282e-6 -0.3826839 0.9238794 4.21498e-7 0.3826863 0.9238784 -3.74662e-7 0.9238781 0.3826869 4.19961e-7 0.9238781 -0.382687 1.61387e-6 -0.3826839 -0.9238794 4.1138e-7 0.3826843 -0.9238792 -1 0 0 4.70811e-6 1 1.78857e-4 1 0 0 0.9999989 0 0.001466631 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 6.91998e-5 2.86637e-5 1 0 0 1 0 0 1 0 0 1 0 0 4.07189e-6 -1 0 0 0 -1 0 0 1 4.64941e-7 -0.9238792 -0.3826842 -4.21495e-7 -0.9238796 0.3826832 -1.54548e-6 -0.3826834 0.9238796 -1.87332e-6 0.3826846 0.9238791 0 0.9238802 0.3826822 0 0.9238798 -0.3826827 0 -0.3826841 -0.9238793 1.61387e-6 0.3826848 -0.923879 5.89085e-7 -0.9238792 -0.3826845 -3.27829e-7 -0.9238796 0.3826835 -1.63915e-6 -0.3826834 0.9238796 -1.73282e-6 0.3826848 0.923879 2.34166e-7 0.9238802 0.382682 2.48289e-7 0.9238799 -0.3826826 0 -0.3826843 -0.9238792 1.61387e-6 0.3826847 -0.923879 4.64944e-7 -0.923878 -0.382687 -4.21494e-7 -0.9238785 0.3826861 -2.01381e-6 -0.3826841 0.9238792 -1.73282e-6 0.3826851 0.9238789 9.83494e-7 0.9238784 0.3826861 9.93154e-7 0.9238782 -0.3826867 -4.11378e-7 -0.382685 -0.9238789 1.61387e-6 0.382685 -0.9238789 1 0 0 1 0 0 1 3.1476e-6 1.30378e-6 1 5.40047e-7 1.30378e-6 1 3.68764e-6 0 1 -8.90277e-6 0 1 0 0 1 0 0 1 3.1476e-6 1.30378e-6 1 5.40047e-7 1.30378e-6 1 3.68764e-6 0 1 -8.90277e-6 0 1 0 0 1 0 0 1 3.14761e-6 1.30378e-6 1 3.14761e-6 1.30378e-6 1 0 0 1 -8.90269e-6 0</float_array>
+          <technique_common>
+            <accessor source="#Cube_001-mesh-normals-array" count="107" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="Cube_001-mesh-vertices">
+          <input semantic="POSITION" source="#Cube_001-mesh-positions"/>
+        </vertices>
+        <polylist material="Material_001-material" count="89">
+          <input semantic="VERTEX" source="#Cube_001-mesh-vertices" offset="0"/>
+          <input semantic="NORMAL" source="#Cube_001-mesh-normals" offset="1"/>
+          <vcount>3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
+          <p>2 0 0 0 1 0 1 1 0 1 4 1 31 2 27 2 5 2 5 3 30 3 31 3 5 4 29 4 30 4 4 5 29 5 5 5 23 6 27 6 25 6 7 7 28 7 4 7 7 8 21 8 26 8 9 9 25 9 27 9 28 10 14 10 13 10 19 11 25 11 15 11 24 12 20 12 18 12 23 13 6 13 5 13 23 14 22 14 6 14 6 15 8 15 7 15 18 16 17 16 16 16 17 17 15 17 16 17 13 18 11 18 10 18 11 19 9 19 10 19 7 20 8 20 21 20 3 21 6 21 7 21 6 22 1 22 5 22 7 23 0 23 3 23 32 24 29 24 33 24 31 25 32 25 35 25 27 26 35 26 37 26 9 27 37 27 39 27 10 28 39 28 38 28 36 29 10 29 38 29 33 30 28 30 34 30 34 31 13 31 36 31 40 32 14 32 41 32 12 33 40 33 43 33 25 34 43 34 45 34 15 35 45 35 47 35 16 36 47 36 46 36 44 37 16 37 46 37 41 38 24 38 42 38 42 39 18 39 44 39 48 40 20 40 49 40 19 41 48 41 51 41 23 42 51 42 53 42 22 43 53 43 55 43 8 44 55 44 54 44 52 45 8 45 54 45 49 46 26 46 50 46 50 47 21 47 52 47 0 48 2 48 3 48 1 49 4 49 5 49 29 50 4 50 28 50 27 51 23 51 5 51 28 52 7 52 26 52 25 53 9 53 12 53 14 54 28 54 24 54 25 55 19 55 23 55 20 56 24 56 26 56 8 57 6 57 22 57 17 58 18 58 20 58 15 59 17 59 19 59 11 60 13 60 14 60 9 61 11 61 12 61 6 62 3 62 2 62 1 63 6 63 2 63 0 64 7 64 4 64 29 65 32 65 30 65 32 66 31 66 30 66 35 67 27 67 31 67 37 68 9 68 27 68 39 69 10 69 9 69 10 70 36 70 13 70 28 71 33 71 29 71 13 72 34 72 28 72 14 73 40 73 11 73 40 74 12 74 11 74 43 75 25 75 12 75 45 76 15 76 25 76 47 77 16 77 15 77 16 78 44 78 18 78 24 79 41 79 14 79 18 80 42 80 24 80 20 81 48 81 17 81 48 82 19 82 17 82 51 83 23 83 19 83 53 84 22 84 23 84 55 85 8 85 22 85 8 86 52 86 21 86 26 87 49 87 20 87 21 88 50 88 26 88</p>
+        </polylist>
+        <polylist material="Material_002-material" count="18">
+          <input semantic="VERTEX" source="#Cube_001-mesh-vertices" offset="0"/>
+          <input semantic="NORMAL" source="#Cube_001-mesh-normals" offset="1"/>
+          <vcount>3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
+          <p>32 89 33 89 35 89 33 90 34 90 35 90 34 91 37 91 35 91 34 92 39 92 37 92 34 93 36 93 39 93 36 94 38 94 39 94 40 95 41 95 43 95 41 96 42 96 43 96 42 97 45 97 43 97 42 98 47 98 45 98 42 99 44 99 47 99 44 100 46 100 47 100 48 101 49 101 51 101 49 102 50 102 51 102 50 103 53 103 51 103 50 104 52 104 53 104 52 105 55 105 53 105 52 106 54 106 55 106</p>
+        </polylist>
+      </mesh>
+    </geometry>
+  </library_geometries>
+  <library_controllers/>
+  <library_visual_scenes>
+    <visual_scene id="Scene" name="Scene">
+      <node id="Cube" name="Cube" type="NODE">
+        <matrix sid="transform">0.04101324 0 0 0 0 0.04101324 0 0.03664687 0 0 0.04101324 0 0 0 0 1</matrix>
+        <instance_geometry url="#Cube_001-mesh">
+          <bind_material>
+            <technique_common>
+              <instance_material symbol="Material_001-material" target="#Material_001-material"/>
+              <instance_material symbol="Material_002-material" target="#Material_002-material"/>
+            </technique_common>
+          </bind_material>
+        </instance_geometry>
+      </node>
+    </visual_scene>
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#Scene"/>
+  </scene>
+</COLLADA>

Controllers/Packages/cititruck_description/meshes/kinect_v2.dae

@@ -0,0 +1,152 @@
+<?xml version="1.0" encoding="utf-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+  <asset>
+    <contributor>
+      <author>Stefan Kohlbrecher</author>
+      <authoring_tool>Blender 2.62.1 r44749</authoring_tool>
+    </contributor>
+    <created>2012-03-09T23:01:39</created>
+    <modified>2012-03-09T23:01:39</modified>
+    <unit name="meter" meter="1"/>
+    <up_axis>Z_UP</up_axis>
+  </asset>
+  <library_effects>
+    <effect id="Material-effect">
+      <profile_COMMON>
+        <technique sid="common">
+          <phong>
+            <emission>
+              <color sid="emission">0 0 0 1</color>
+            </emission>
+            <ambient>
+              <color sid="ambient">0.01 0.01 0.01 1</color>
+            </ambient>
+            <diffuse>
+              <color sid="diffuse">0.008869297 0.008869297 0.008869297 1</color>
+            </diffuse>
+            <specular>
+              <color sid="specular">0.5 0.5 0.5 1</color>
+            </specular>
+            <shininess>
+              <float sid="shininess">50</float>
+            </shininess>
+            <index_of_refraction>
+              <float sid="index_of_refraction">1</float>
+            </index_of_refraction>
+          </phong>
+        </technique>
+        <extra>
+          <technique profile="GOOGLEEARTH">
+            <double_sided>1</double_sided>
+          </technique>
+        </extra>
+      </profile_COMMON>
+      <extra><technique profile="MAX3D"><double_sided>1</double_sided></technique></extra>
+    </effect>
+    <effect id="Material_001-effect">
+      <profile_COMMON>
+        <technique sid="common">
+          <phong>
+            <emission>
+              <color sid="emission">0.2 0.2 0.2 1</color>
+            </emission>
+            <ambient>
+              <color sid="ambient">0.3 0.3 0.3 1</color>
+            </ambient>
+            <diffuse>
+              <color sid="diffuse">0.64 0.64 0.64 1</color>
+            </diffuse>
+            <specular>
+              <color sid="specular">0.839 0.839 0.839 1</color>
+            </specular>
+            <shininess>
+              <float sid="shininess">5</float>
+            </shininess>
+            <index_of_refraction>
+              <float sid="index_of_refraction">1</float>
+            </index_of_refraction>
+          </phong>
+        </technique>
+        <extra>
+          <technique profile="GOOGLEEARTH">
+            <double_sided>1</double_sided>
+          </technique>
+        </extra>
+      </profile_COMMON>
+      <extra><technique profile="MAX3D"><double_sided>1</double_sided></technique></extra>
+    </effect>
+  </library_effects>
+  <library_materials>
+    <material id="Material-material" name="Material">
+      <instance_effect url="#Material-effect"/>
+    </material>
+    <material id="Material_001-material" name="Material.001">
+      <instance_effect url="#Material_001-effect"/>
+    </material>
+  </library_materials>
+  <library_geometries>
+    <geometry id="Cube_001-mesh" name="Cube.001">
+      <mesh>
+        <source id="Cube_001-mesh-positions">
+          <float_array id="Cube_001-mesh-positions-array" count="168">-1.497 2.613 0.4524136 -1.497 2.613 -0.4341885 -1.497 -2.613 -0.4341885 -1.497 -2.613 0.4524136 -0.02330875 3.366 0.4524136 -0.02330875 3.366 -0.4341885 -0.02931594 -3.366 -0.4341885 -0.02931594 -3.366 0.4524136 -0.02788293 -1.760094 0 -0.02618807 0.1392521 -0.165674 -0.02624928 0.07062792 0 -0.02637684 -0.07232695 0 -0.02643811 -0.1409515 -0.1656743 -0.02618807 0.1392521 0.1656737 -0.02643811 -0.1409515 0.1656738 -0.02673375 -0.4722999 -0.165674 -0.02679502 -0.5409241 0 -0.02746474 -1.291496 0 -0.02673375 -0.4722999 0.1656737 -0.02752602 -1.360121 -0.1656743 -0.02752602 -1.360121 0.1656738 -0.02782166 -1.691469 0.1656737 -0.02782166 -1.691469 -0.165674 -0.02767384 -1.525795 -0.2342988 -0.02658593 -0.3066257 0.2342985 -0.02658593 -0.3066257 -0.2342988 -0.02767384 -1.525795 0.2342985 -0.02604025 0.3049264 -0.2342988 -0.02604025 0.3049263 0.2342985 -0.02589237 0.4706006 0.1656738 -0.02583116 0.5392251 0 -0.02589237 0.4706006 -0.1656743 -0.1343017 0.5392251 -1.34731e-7 -0.1343017 0.4706006 0.1656738 -0.1343017 0.3049263 0.2342983 -0.1343017 0.4706006 -0.1656745 -0.1343011 0.1392521 0.1656737 -0.1343011 0.3049264 -0.234299 -0.1343017 0.07062768 -1.21696e-7 -0.1343011 0.1392521 -0.165674 -0.1343017 -0.07232695 -1.34731e-7 -0.1343017 -0.1409515 0.1656738 -0.1343017 -0.3066257 0.2342983 -0.1343017 -0.1409515 -0.1656745 -0.1343011 -0.4722999 0.1656737 -0.1343011 -0.3066257 -0.234299 -0.1343017 -0.5409244 -1.21696e-7 -0.1343011 -0.4722999 -0.165674 -0.1343017 -1.291496 -1.34731e-7 -0.1343017 -1.360121 0.1656738 -0.1343017 -1.525795 0.2342983 -0.1343017 -1.360121 -0.1656745 -0.1343011 -1.691469 0.1656737 -0.1343011 -1.525795 -0.234299 -0.1343017 -1.760094 -1.21696e-7 -0.1343011 -1.691469 -0.165674</float_array>
+          <technique_common>
+            <accessor source="#Cube_001-mesh-positions-array" count="56" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="Cube_001-mesh-normals">
+          <float_array id="Cube_001-mesh-normals-array" count="321">-1 0 0 -0.4550058 0.8904885 0 0.9999996 -8.92333e-4 0 0.9999997 -8.92333e-4 0 0.9999997 -8.92336e-4 0 0.9999997 -8.92334e-4 0 0 0 1 0.9999996 -8.92334e-4 0 0.9999997 -8.92333e-4 0 0.9999996 -8.92336e-4 0 0.9999997 -8.92336e-4 0 0.9999996 -8.92334e-4 0 0.9999995 -8.92334e-4 0 0.9999997 -8.92334e-4 0 0.9999997 -8.92333e-4 0 0.9999996 -8.92334e-4 0 0.9999997 -8.92333e-4 0 0.9999996 -8.92333e-4 0 0.9999996 -8.92331e-4 0 0.9999996 -8.92331e-4 0 0.9999996 -8.92335e-4 0 -0.4564806 -0.8897334 0 0 0 -1 0 0 1 0 -0.9238792 -0.3826842 -9.10222e-7 -0.9238796 0.3826833 -2.15875e-6 -0.3826836 0.9238795 0 0.382686 0.9238785 -2.35517e-6 0.9238793 0.3826841 -2.23774e-6 0.9238793 -0.3826839 1 0 0 1 0 0 1 2.91729e-6 1.20838e-6 1 5.00532e-7 1.20838e-6 1 3.41782e-6 0 1 -8.25134e-6 0 2.16178e-6 -0.3826833 -0.9238796 0 0.3826836 -0.9238794 0 -0.9238792 -0.3826842 -8.66678e-7 -0.9238797 0.3826831 -2.2179e-6 -0.3826835 0.9238795 0 0.382686 0.9238785 -2.60867e-6 0.9238793 0.3826841 -2.50535e-6 0.9238793 -0.3826839 1 0 0 1 0 0 1 2.91729e-6 1.20838e-6 1 5.00532e-7 1.20838e-6 1 3.41782e-6 0 1 -8.25134e-6 0 2.17273e-6 -0.3826834 -0.9238795 0 0.3826836 -0.9238795 0 -0.9238793 -0.3826839 -8.75508e-7 -0.9238798 0.382683 -2.19182e-6 -0.3826834 0.9238796 0 0.382686 0.9238785 -2.14732e-6 0.9238793 0.3826841 -2.0138e-6 0.9238793 -0.3826839 1 0 0 1 0 0 1 2.91729e-6 1.20838e-6 1 2.91729e-6 1.20838e-6 1 0 0 1 -8.25134e-6 0 2.1949e-6 -0.3826834 -0.9238796 0 0.3826836 -0.9238796 -1 0 0 -0.4550058 0.8904885 0 0.9999996 -8.92333e-4 0 0.9999996 -8.92335e-4 0 0.9999996 -8.92335e-4 0 0.9999997 -8.92336e-4 0 0.9999996 -8.92336e-4 0 0.9999996 -8.92335e-4 0 0.9999997 -8.92335e-4 0 0.9999996 -8.92335e-4 0 0.9999996 -8.92334e-4 0 0.9999996 -8.92334e-4 0 0.9999996 -8.92336e-4 0 0.9999996 -8.92336e-4 0 -0.4564806 -0.8897334 0 0 0 -1 0 0 1 0 -0.9238792 -0.3826842 0 -0.9238794 0.3826837 -2.15579e-6 -0.3826836 0.9238795 -2.15874e-6 0.3826848 0.923879 0 0.9238799 0.3826828 0 0.9238799 -0.3826826 0 -0.3826846 -0.9238791 2.16177e-6 0.3826848 -0.9238789 0 -0.9238792 -0.3826842 0 -0.9238795 0.3826836 -2.21485e-6 -0.3826835 0.9238796 -2.16968e-6 0.3826848 0.923879 0 0.9238799 0.3826826 0 0.9238799 -0.3826826 0 -0.3826847 -0.9238791 2.17273e-6 0.3826848 -0.9238789 0 -0.9238794 -0.3826839 0 -0.9238796 0.3826835 -2.18877e-6 -0.3826833 0.9238795 -2.19182e-6 0.3826848 0.923879 0 0.9238798 0.3826829 0 0.9238798 -0.3826829 0 -0.3826846 -0.9238791 2.19489e-6 0.3826848 -0.9238791</float_array>
+          <technique_common>
+            <accessor source="#Cube_001-mesh-normals-array" count="107" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="Cube_001-mesh-vertices">
+          <input semantic="POSITION" source="#Cube_001-mesh-positions"/>
+        </vertices>
+        <polylist material="Material1" count="89">
+          <input semantic="VERTEX" source="#Cube_001-mesh-vertices" offset="0"/>
+          <input semantic="NORMAL" source="#Cube_001-mesh-normals" offset="1"/>
+          <vcount>3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
+          <p>2 0 0 0 1 0 1 1 0 1 4 1 31 2 27 2 5 2 5 3 30 3 31 3 5 4 29 4 30 4 4 5 29 5 5 5 23 6 27 6 25 6 7 7 28 7 4 7 7 8 21 8 26 8 9 9 25 9 27 9 28 10 14 10 13 10 19 11 25 11 15 11 24 12 20 12 18 12 23 13 6 13 5 13 23 14 22 14 6 14 6 15 8 15 7 15 18 16 17 16 16 16 17 17 15 17 16 17 13 18 11 18 10 18 11 19 9 19 10 19 7 20 8 20 21 20 3 21 6 21 7 21 6 22 1 22 5 22 7 23 0 23 3 23 32 24 29 24 33 24 31 25 32 25 35 25 27 26 35 26 37 26 9 27 37 27 39 27 10 28 39 28 38 28 36 29 10 29 38 29 33 36 28 36 34 36 34 37 13 37 36 37 40 38 14 38 41 38 12 39 40 39 43 39 25 40 43 40 45 40 15 41 45 41 47 41 16 42 47 42 46 42 44 43 16 43 46 43 41 50 24 50 42 50 42 51 18 51 44 51 48 52 20 52 49 52 19 53 48 53 51 53 23 54 51 54 53 54 22 55 53 55 55 55 8 56 55 56 54 56 52 57 8 57 54 57 49 64 26 64 50 64 50 65 21 65 52 65 0 66 2 66 3 66 1 67 4 67 5 67 29 68 4 68 28 68 27 69 23 69 5 69 28 70 7 70 26 70 25 71 9 71 12 71 14 72 28 72 24 72 25 73 19 73 23 73 20 74 24 74 26 74 8 75 6 75 22 75 17 76 18 76 20 76 15 77 17 77 19 77 11 78 13 78 14 78 9 79 11 79 12 79 6 80 3 80 2 80 1 81 6 81 2 81 0 82 7 82 4 82 29 83 32 83 30 83 32 84 31 84 30 84 35 85 27 85 31 85 37 86 9 86 27 86 39 87 10 87 9 87 10 88 36 88 13 88 28 89 33 89 29 89 13 90 34 90 28 90 14 91 40 91 11 91 40 92 12 92 11 92 43 93 25 93 12 93 45 94 15 94 25 94 47 95 16 95 15 95 16 96 44 96 18 96 24 97 41 97 14 97 18 98 42 98 24 98 20 99 48 99 17 99 48 100 19 100 17 100 51 101 23 101 19 101 53 102 22 102 23 102 55 103 8 103 22 103 8 104 52 104 21 104 26 105 49 105 20 105 21 106 50 106 26 106</p>
+        </polylist>
+        <polylist material="Material_0012" count="18">
+          <input semantic="VERTEX" source="#Cube_001-mesh-vertices" offset="0"/>
+          <input semantic="NORMAL" source="#Cube_001-mesh-normals" offset="1"/>
+          <vcount>3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
+          <p>32 30 33 30 35 30 33 31 34 31 35 31 34 32 37 32 35 32 34 33 39 33 37 33 34 34 36 34 39 34 36 35 38 35 39 35 40 44 41 44 43 44 41 45 42 45 43 45 42 46 45 46 43 46 42 47 47 47 45 47 42 48 44 48 47 48 44 49 46 49 47 49 48 58 49 58 51 58 49 59 50 59 51 59 50 60 53 60 51 60 50 61 52 61 53 61 52 62 55 62 53 62 52 63 54 63 55 63</p>
+        </polylist>
+      </mesh>
+      <extra><technique profile="MAYA"><double_sided>1</double_sided></technique></extra>
+    </geometry>
+  </library_geometries>
+  <library_visual_scenes>
+    <visual_scene id="Scene" name="Scene">
+      <node id="Cube" type="NODE">
+        <translate sid="location">0 0 0</translate>
+        <rotate sid="rotationZ">0 0 1 0</rotate>
+        <rotate sid="rotationY">0 1 0 0</rotate>
+        <rotate sid="rotationX">1 0 0 0</rotate>
+        <scale sid="scale">0.04101324 0.04101324 0.04101324</scale>
+        <instance_geometry url="#Cube_001-mesh">
+          <bind_material>
+            <technique_common>
+              <instance_material symbol="Material1" target="#Material-material"/>
+              <instance_material symbol="Material_0012" target="#Material_001-material"/>
+            </technique_common>
+          </bind_material>
+        </instance_geometry>
+      </node>
+    </visual_scene>
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#Scene"/>
+  </scene>
+</COLLADA>
+

Controllers/Packages/cititruck_description/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>cititruck_description</name>
+  <version>1.1.7</version>
+  <description>URDF description of the cititruck robot</description>
+
+  <maintainer email="martin.guenther@dfki.de">Martin Günther</maintainer>
+  <author email="martin.guenther@dfki.de">Martin Günther</author>
+
+  <license>BSD</license>
+
+  <url type="website">https://github.com/DFKI-NI/cititruck_robot</url>
+  <url type="repository">https://github.com/DFKI-NI/cititruck_robot</url>
+  <url type="bugtracker">https://github.com/DFKI-NI/cititruck_robot/issues</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roslaunch</build_depend>
+
+  <exec_depend>diff_drive_controller</exec_depend>
+  <exec_depend>gazebo_plugins</exec_depend>
+  <exec_depend>gazebo_ros_control</exec_depend>
+  <exec_depend>hector_gazebo_plugins</exec_depend>
+  <exec_depend>joint_state_controller</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>position_controllers</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>rviz</exec_depend>
+  <exec_depend>urdf</exec_depend>
+  <exec_depend>xacro</exec_depend>
+</package>

Controllers/Packages/cititruck_description/rviz/cititruck_description.rviz

@@ -0,0 +1,242 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 81
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 546
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        drive_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fixed_wheel_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fixed_wheel_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        left_laser_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_laser_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        steer_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        steer_link_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_footprint:
+          Value: true
+        base_link:
+          Value: true
+        drive_wheel_link:
+          Value: true
+        fixed_wheel_left_link:
+          Value: true
+        fixed_wheel_right_link:
+          Value: true
+        imu_frame:
+          Value: true
+        imu_link:
+          Value: true
+        left_laser_link:
+          Value: true
+        right_laser_link:
+          Value: true
+        steer_link:
+          Value: true
+        steer_link_2:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        base_footprint:
+          base_link:
+            fixed_wheel_left_link:
+              {}
+            fixed_wheel_right_link:
+              {}
+            imu_link:
+              imu_frame:
+                {}
+            left_laser_link:
+              {}
+            right_laser_link:
+              {}
+            steer_link:
+              drive_wheel_link:
+                {}
+              steer_link_2:
+                {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: base_footprint
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 5.068544387817383
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0.023150593042373657
+        Y: -0.03417559340596199
+        Z: -2.6775524020195007e-09
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.0303977727890015
+      Target Frame: <Fixed Frame>
+      Yaw: 0.26539814472198486
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000340000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 2125
+  Y: 0