update

Controllers/Packages/amr_control/CMakeLists.txt

@@ -23,8 +23,10 @@ find_package(catkin REQUIRED COMPONENTS
   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)
@@ -40,7 +42,7 @@ find_package(PkgConfig)
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
   INCLUDE_DIRS include
-  LIBRARIES amr_control 
+  LIBRARIES amr_control amr_control_converter amr_control_publisher amr_control_subscriber
   CATKIN_DEPENDS 
     geometry_msgs  
     loc_core 
@@ -63,9 +65,22 @@ 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
@@ -75,18 +90,14 @@ add_library(${PROJECT_NAME}
   src/amr_make_plan_with_order.cpp
 )
 
-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})
-
 ## 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
@@ -123,14 +134,16 @@ set_target_properties(${PROJECT_NAME}_node PROPERTIES
 ## 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}
+  ${TF3_LIBRARY}
 )
 
 target_link_libraries(${PROJECT_NAME}_node
   ${PROJECT_NAME}
-  ${PROJECT_NAME}_converter
   ${catkin_LIBRARIES}
   ${Boost_LIBRARIES}
 )
@@ -158,7 +171,7 @@ install(TARGETS ${PROJECT_NAME}_node
 
 ## 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

@@ -28,6 +28,8 @@
 #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 <boost/dll/import.hpp>
 
@@ -146,6 +148,8 @@ namespace amr_control
     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_;

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

@@ -1,6 +1,7 @@
 #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>
@@ -10,8 +11,6 @@
 #include <robot_sensor_msgs/PointCloud2.h>
 #include <robot_geometry_msgs/Point.h>
 #include <robot_geometry_msgs/PoseStamped.h>
-#include <robot/console.h>
-#include <robot/time.h>
 #include <move_base_core/navigation.h>
 #include <memory>
 

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

@@ -2,8 +2,7 @@
 #define __TF_CONVERTER_H_INCLUDED_
 
 #include <ros/ros.h>
-#include <robot/console.h>
-#include <robot/time.h>
+#include <robot/robot.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf3/buffer_core.h>
 #include <tf3/exceptions.h>

Controllers/Packages/amr_control/package.xml

@@ -91,14 +91,15 @@
   <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,10 +1,10 @@
 #include "amr_control/amr_control.h"
 
-#include <robot/plugin_loader_helper.h>
-#include <robot/console.h>  
-#include <robot/node_handle.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
 {
@@ -76,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)
@@ -234,18 +243,13 @@ namespace amr_control
       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);
+        path_file_so, "MoveBase", boost::dll::load_mode::append_decorations);
       
       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_);
-      
-      // 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);
-      
+
       ros::Rate r(3);
       do
       {
@@ -253,15 +257,99 @@ namespace amr_control
         ros::spinOnce();
       } while (ros::ok() && !move_base_ptr_->getFeedback()->is_ready);
 
+      // 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)

Controllers/Packages/amr_control/src/amr_opc_ua_server_api.cpp

@@ -8,7 +8,7 @@
 #include <angles/angles.h>
 
 // pnkx core
-#include <robot/time.h>
+#include <robot/robot.h>
 #include <robot_nav_2d_utils/conversions.h>
 #include <robot_nav_2d_msgs/Twist2D.h>
 

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
 {
@@ -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());
-        // goal = path.poses.back();
+        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::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,23 +223,27 @@ 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 = robot::Time::now();
         goal.header.frame_id = "map";
         goal.pose.position.x = position.x;
         goal.pose.position.y = position.y;
-        // tf3::Transform transform;
-        // transform.setRotation(tf3::Quaternion(0.0, 0.0, position.theta));
-        // goal = tf3::toMsg(transform);
+        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);
+        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_;
@@ -248,7 +257,6 @@ namespace amr_control
       robot_geometry_msgs::Vector3 angular;
       angular.z = cmd_vel_max_.theta;
       VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular);
- 
     }
   }
 
@@ -385,8 +393,6 @@ namespace amr_control
       robot_geometry_msgs::Vector3 angular;
       angular.z = cmd_vel_max_.theta;
       VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular);
-
-
     }
   }
 

Controllers/Packages/amr_control/src/sensor_converter.cpp

@@ -75,7 +75,7 @@ void amr_control::SensorConverter::convertAndAddMap(const nav_msgs::OccupancyGri
     map.info.origin.orientation.w = nav_map.info.origin.orientation.w;
     
     map.data = nav_map.data;
-    move_base_ptr_->addStaticMap("navigation_map", map);
+    move_base_ptr_->addStaticMap("/map", map);
   }
 }
 
@@ -104,7 +104,7 @@ void amr_control::SensorConverter::laserFScanCallback(const sensor_msgs::LaserSc
     robot_scan.ranges = msg->ranges;
     robot_scan.intensities = msg->intensities;
     
-    move_base_ptr_->addLaserScan("f_scan", robot_scan);
+    move_base_ptr_->addLaserScan("/f_scan", robot_scan);
   }
 }
 
@@ -127,6 +127,6 @@ void amr_control::SensorConverter::laserBScanCallback(const sensor_msgs::LaserSc
     robot_scan.ranges = msg->ranges;
     robot_scan.intensities = msg->intensities;
     
-    move_base_ptr_->addLaserScan("b_scan", robot_scan);
+    move_base_ptr_->addLaserScan("/b_scan", robot_scan);
   }
 }

Controllers/Packages/amr_control/src/tf_converter.cpp

@@ -5,7 +5,7 @@ amr_control::TfConverter::TfConverter(std::shared_ptr<tf2_ros::Buffer> tf)
 {
   tf3_buffer_ = std::make_shared<tf3::BufferCore>();
   tf3_buffer_->setUsingDedicatedThread(true);
-  tf_thread_ = std::thread([this]() { this->tfWorker(); });
+  tf_thread_ = std::thread(&amr_control::TfConverter::tfWorker, this);
 }
 
 amr_control::TfConverter::~TfConverter()
@@ -16,8 +16,8 @@ amr_control::TfConverter::~TfConverter()
 void amr_control::TfConverter::tfWorker()
 {
   struct TFEdge {
-      std::string parent;
-      std::string child;
+    std::string parent;
+    std::string child;
   }; 
   std::vector<TFEdge> edges;
   tf2_ros::Buffer tfBuffer;
@@ -27,20 +27,30 @@ void amr_control::TfConverter::tfWorker()
   std::string tree;
   std::string line;
 
+  int count_tf_receive_done = 0;
   ros::Rate rate(20);
   while (ros::ok() && !stop_tf_thread_)
   {
-      tree = tfBuffer.allFramesAsString();
+    ros::spinOnce();
+    tree = tfBuffer.allFramesAsString();
+    if (!tree.empty() && tree == last_tree)
+    {
+      count_tf_receive_done++;
+    }
+    else
+    {
+      count_tf_receive_done = 0;
+    }
 
-      if (!tree.empty() && tree == last_tree)
-      {
-          // ROS_WARN("TF tree stabilized: \n%s", tree.c_str());
-          break;
-      }
+    if(count_tf_receive_done > 2)
+    {
+      ROS_WARN("TF tree stabilized: \n%s", tree.c_str());
+      break;
+    }
 
-      last_tree = tree;
-      ros::spinOnce();
-      rate.sleep();
+    last_tree = tree;
+    
+    rate.sleep();
   }
 
   std::istringstream iss(tree);
@@ -104,7 +114,8 @@ void amr_control::TfConverter::tfWorker()
         catch (tf2::TransformException &ex)
         {
             ROS_WARN("TF %s -> %s failed: %s",
-                    e.parent.c_str(),e.child.c_str(),
+                    e.parent.c_str(),
+                    e.child.c_str(),
                     ex.what());
         }
       }

Controllers/Packages/amr_startup/config/costmap_global_params_plugins_no_virtual_walls.yaml

@@ -1,4 +1,5 @@
 global_costmap:
+    library_path: libplugins
     frame_id: map
     plugins:
         - {name: navigation_map, type: "costmap_2d::StaticLayer" }

Controllers/Packages/amr_startup/config/costmap_local_params_plugins_no_virtual_walls.yaml

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

Controllers/Packages/amr_startup/config/mqtt_general.yaml

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

Controllers/Packages/amr_startup/rviz/navigation.rviz

@@ -7,13 +7,10 @@ Panels:
         - /Global Options1
         - /Grid1/Offset1
         - /TF1/Frames1
-        - /Global Map1/Straight Path1
         - /Local Map1
-        - /Local Map1/Trajectory1
-        - /Local Map1/Trajectory1/transform plan1
-        - /Pose1
+        - /Local Map1/Plan1
       Splitter Ratio: 0.37295082211494446
-    Tree Height: 422
+    Tree Height: 682
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -70,18 +67,118 @@ Visualization Manager:
     - Alpha: 0.5
       Class: rviz/RobotModel
       Collision Enabled: true
-      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
+        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: false
+      Value: true
       Visual Enabled: true
     - Class: rviz/TF
       Enabled: true
@@ -260,7 +357,7 @@ Visualization Manager:
           Draw Behind: true
           Enabled: true
           Name: Costmap
-          Topic: /amr_node/global_costmap/costmap
+          Topic: /global_costmap/costmap
           Unreliable: false
           Use Timestamp: false
           Value: true
@@ -268,13 +365,13 @@ Visualization Manager:
           Buffer Length: 1
           Class: rviz/Path
           Color: 25; 255; 0
-          Enabled: false
+          Enabled: true
           Head Diameter: 0.30000001192092896
           Head Length: 0.20000000298023224
           Length: 0.20000000298023224
           Line Style: Lines
           Line Width: 0.05000000074505806
-          Name: Full Plan
+          Name: Plan
           Offset:
             X: 0
             Y: 0
@@ -285,101 +382,29 @@ Visualization Manager:
           Radius: 0.019999999552965164
           Shaft Diameter: 0.10000000149011612
           Shaft Length: 0.10000000149011612
-          Topic: /amr_node/SBPLLatticePlanner/plan
+          Topic: /global_costmap/costmap/plan
           Unreliable: false
-          Value: false
+          Value: true
         - Alpha: 1
           Class: rviz/Polygon
           Color: 255; 0; 0
           Enabled: true
           Name: Footprint
           Queue Size: 10
-          Topic: move_base_node/global_costmap/footprint
+          Topic: /global_costmap/costmap/footprint
           Unreliable: 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.00800000037997961
-          Line Style: Billboards
-          Line Width: 0.009999999776482582
-          Name: CustomPath
-          Offset:
-            X: 0
-            Y: 0
-            Z: 0.8999999761581421
-          Pose Color: 255; 85; 255
-          Pose Style: None
-          Queue Size: 10
-          Radius: 0.004999999888241291
-          Shaft Diameter: 0.10000000149011612
-          Shaft Length: 0.10000000149011612
-          Topic: /amr_node/CustomPlanner/plan
-          Unreliable: false
-          Value: true
-        - Alpha: 1
-          Buffer Length: 1
-          Class: rviz/Path
-          Color: 0; 0; 0
-          Enabled: false
-          Head Diameter: 0.30000001192092896
-          Head Length: 0.20000000298023224
-          Length: 0.30000001192092896
-          Line Style: Lines
-          Line Width: 0.029999999329447746
-          Name: Straight Path
-          Offset:
-            X: 0
-            Y: 0
-            Z: 0
-          Pose Color: 255; 85; 255
-          Pose Style: Axes
-          Queue Size: 10
-          Radius: 0.029999999329447746
-          Shaft Diameter: 0.10000000149011612
-          Shaft Length: 0.10000000149011612
-          Topic: /amr_node/LocalPlannerAdapter/MKTGoStraightLocalPlanner/global_plan
-          Unreliable: false
-          Value: false
       Enabled: true
       Name: Global Map
     - Class: rviz/Group
       Displays:
-        - Alpha: 1
-          Buffer Length: 1
-          Class: rviz/Path
-          Color: 252; 175; 62
-          Enabled: false
-          Head Diameter: 0.30000001192092896
-          Head Length: 0.20000000298023224
-          Length: 0.03999999910593033
-          Line Style: Lines
-          Line Width: 0.029999999329447746
-          Name: Global Path
-          Offset:
-            X: 0
-            Y: 0
-            Z: 0
-          Pose Color: 255; 85; 255
-          Pose Style: Axes
-          Queue Size: 10
-          Radius: 0.009999999776482582
-          Shaft Diameter: 0.10000000149011612
-          Shaft Length: 0.10000000149011612
-          Topic: /amr_node/LocalPlannerAdapter/global_plan
-          Unreliable: false
-          Value: false
         - Alpha: 0.699999988079071
           Class: rviz/Map
           Color Scheme: map
           Draw Behind: false
           Enabled: true
           Name: Costmap
-          Topic: /amr_node/local_costmap/costmap
+          Topic: /local_costmap/costmap
           Unreliable: false
           Use Timestamp: false
           Value: true
@@ -389,61 +414,33 @@ Visualization Manager:
           Enabled: true
           Name: Footprint
           Queue Size: 10
-          Topic: /amr_node/local_costmap/footprint
+          Topic: /local_costmap/costmap/footprint
           Unreliable: false
           Value: true
         - Alpha: 1
           Buffer Length: 1
           Class: rviz/Path
-          Color: 138; 226; 52
-          Enabled: false
+          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: Local Plan
+          Name: Plan
           Offset:
             X: 0
             Y: 0
-            Z: 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: /amr_node/LocalPlannerAdapter/transformed_global_plan
+          Topic: /local_costmap/costmap/plan
           Unreliable: false
-          Value: false
-        - Alpha: 1
-          Autocompute Intensity Bounds: true
-          Autocompute Value Bounds:
-            Max Value: 10
-            Min Value: -10
-            Value: true
-          Axis: Z
-          Channel Name: intensity
-          Class: rviz/PointCloud2
-          Color: 255; 255; 255
-          Color Transformer: Intensity
-          Decay Time: 0
-          Enabled: false
-          Invert Rainbow: false
-          Max Color: 255; 255; 255
-          Min Color: 0; 0; 0
-          Name: PointCloud2
-          Position Transformer: XYZ
-          Queue Size: 10
-          Selectable: true
-          Size (Pixels): 3
-          Size (m): 0.009999999776482582
-          Style: Flat Squares
-          Topic: /amr_node/PredictiveTrajectory/cost_cloud
-          Unreliable: false
-          Use Fixed Frame: true
-          Use rainbow: true
-          Value: false
+          Value: true
         - Class: rviz/Group
           Displays:
             - Alpha: 1
@@ -597,7 +594,7 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Angle: -3.135005474090576
+      Angle: -3.150002956390381
       Class: rviz/TopDownOrtho
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
@@ -607,10 +604,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Scale: -359.3964538574219
+      Scale: -191.32327270507812
       Target Frame: base_link
-      X: -0.49439820647239685
-      Y: 0.196189746260643
+      X: 0.8287617564201355
+      Y: 0.9812669157981873
     Saved:
       - Angle: -34.55989074707031
         Class: rviz/TopDownOrtho
@@ -629,10 +626,10 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 573
+  Height: 833
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000001e3fc0200000008fb000000100044006900730070006c006100790073000000003d000001e3000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d0065000000000000000752000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000418000001e300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001fa000002e7fc0200000008fb000000100044006900730070006c006100790073010000003d000002e7000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d00650000000000000007520000041800fffffffb0000000800540069006d0065010000000000000450000000000000000000000235000002e700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -641,6 +638,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1048
+  Width: 1077
   X: 0
-  Y: 21
+  Y: 0

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

Controllers/Packages/cititruck_description/urdf/cititruck.urdf.xacro

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<robot name="cititruck" xmlns:xacro="http://ros.org/wiki/xacro" >
+
+  <!-- cititruck base -->
+  <xacro:include filename="$(find cititruck_description)/urdf/include/cititruck_v1.urdf.xacro" />
+  <xacro:include filename="$(find cititruck_description)/urdf/include/common.gazebo.xacro" />
+
+  <xacro:arg name="tf_prefix" default="" />
+  <xacro:property name="tf_prefix_" value="$(arg tf_prefix)" />
+  <xacro:if value="${tf_prefix_ == ''}">
+    <xacro:property name="prefix" value="" />
+  </xacro:if>
+  <xacro:unless value="${tf_prefix_ == ''}">
+    <xacro:property name="prefix" value="${tf_prefix_}/" />
+  </xacro:unless>
+
+  <xacro:cititruck prefix="${prefix}" />
+  <xacro:controller_plugin_gazebo robot_namespace=""/>
+
+  <!-- instead of the controller_plugin_gazebo, you can use the steerdrive controller like this: -->
+  <!-- <xacro:steer_controller_plugin_gazebo 
+  prefix="${prefix}"
+  steer_joint = "base2steer_joint"
+  steerdrive_wheel_joint = "steer2sd_wheel_joint"
+  wheel_radius = "0.105"/> -->
+</robot>

Controllers/Packages/cititruck_description/urdf/include/check_xacro_order.sh

@@ -0,0 +1,3 @@
+rosrun xacro xacro cititruck.xacro > /tmp/old.xml
+rosrun xacro xacro --inorder cititruck.xacro > /tmp/new.xml
+diff /tmp/old.xml /tmp/new.xml

Controllers/Packages/cititruck_description/urdf/include/cititruck.gazebo

@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+<robot  xmlns:xacro="http://ros.org/wiki/xacro">
+  
+  <!-- Arguments used in cititruck.gazebo -->
+  <xacro:arg name="controller_prefix" default=""/>
+  <xacro:arg name="sensor_prefix" default=""/>
+  <xacro:arg name="sick_name" default="safety"/>
+  <xacro:arg name="ls2000_name" default="nav"/>   
+  <xacro:arg name="kinect2_name" default=""/>
+  <xacro:arg name="kinect1_name" default="kinect"/>
+  <xacro:arg name="use_gpu" default="true"/>
+
+  <!-- ros_control plugin -->
+  <gazebo>
+    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+     <robotNamespace>/cititruck</robotNamespace>
+    </plugin>
+  </gazebo>
+
+  <gazebo>
+    <plugin name="steer_drive_controller" filename="libgazebo_ros_steer_drive.so">
+      <legacyMode>false</legacyMode>
+      <alwaysOn>true</alwaysOn>
+      <updateRate>100</updateRate>
+      <steerJoint>base2steer_joint</steerJoint>
+      <driveJoint>steer2sd_wheel_joint</driveJoint>
+      <fixedWheelLeftJoint>base2fixed_left_wheel_joint</fixedWheelLeftJoint>
+      <fixedWheelRightJoint>base2fixed_right_wheel_joint</fixedWheelRightJoint>
+
+      <wheelDiameter>0.16</wheelDiameter>
+      <steeringFixWheelDistanceX>1.190</steeringFixWheelDistanceX>
+      <steeringFixWheelDistanceY>0.0</steeringFixWheelDistanceY>
+      
+      <steerTorque>1000</steerTorque>
+      <driveTorque>1000</driveTorque>
+
+      <commandTopic>$(arg controller_prefix)cmd_vel</commandTopic>
+      <odometryTopic>odom</odometryTopic>
+      <odometryFrame>/world</odometryFrame>
+      <robotBaseFrame>base_footprint</robotBaseFrame>
+
+      <odomEncSteeringAngleOffset>0.01</odomEncSteeringAngleOffset>
+ 
+      <!--odometrySource>encoder</odometrySource-->
+      
+      <publishWheelJointState>true</publishWheelJointState>
+    </plugin>
+  </gazebo>
+
+  <gazebo reference="base_link" >
+    <material>Gazebo/Red</material>
+  </gazebo>
+
+  <gazebo reference="left_fork" >
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="right_fork" >
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="fixed_left_wheel_link" >
+    <material>Gazebo/Black</material>
+  </gazebo>
+
+  <gazebo reference="fixed_right_wheel_link" >
+    <material>Gazebo/Black</material>
+  </gazebo>
+
+  <gazebo reference="steerdrive_wheel_link" >
+    <material>Gazebo/Black</material>
+  </gazebo>
+     
+</robot>

Controllers/Packages/cititruck_description/urdf/include/cititruck.gazebo.xacro

@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="steer_controller_plugin_gazebo" params="prefix steer_joint steerdrive_wheel_joint wheel_radius">
+    <gazebo>
+      <plugin name="steer_drive_controller" filename="libgazebo_ros_steer_drive.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100</updateRate>
+        <steerJoint>${prefix}base2steer_joint</steerJoint>
+        <driveJoint>${prefix}steer2sd_wheel_joint</driveJoint>
+        <fixedWheelLeftJoint>${prefix}fixed_left_wheel_joint</fixedWheelLeftJoint>
+        <fixedWheelRightJoint>${prefix}fixed_right_wheel_joint</fixedWheelRightJoint>
+  
+        <wheelDiameter>1.05</wheelDiameter>
+        <steeringFixWheelDistanceX>1.1</steeringFixWheelDistanceX>
+        <steeringFixWheelDistanceY>0.0</steeringFixWheelDistanceY>
+        
+        <steerTorque>1000</steerTorque>
+        <driveTorque>1000</driveTorque>
+  
+        <commandTopic>mobile_base_controller/cmd_vel</commandTopic>
+        <odometryTopic>mobile_base_controller/odom</odometryTopic>
+        <odometryFrame>world</odometryFrame>
+        <robotBaseFrame>base_footprint</robotBaseFrame>
+  
+        <odomEncSteeringAngleOffset>0.01</odomEncSteeringAngleOffset>
+   
+        <!--odometrySource>encoder</odometrySource-->
+        <publishWheelTF>false</publishWheelTF>
+        <publishWheelJointState>true</publishWheelJointState>
+
+      </plugin>
+    </gazebo>
+  </xacro:macro>
+
+  <xacro:macro name="set_wheel_friction" params="link friction">
+    <gazebo reference="${link}">
+      <mu1 value="${friction}"/>
+      <mu2 value="${friction}"/>
+      <kp value="10000000.0"/>
+      <kd value="1.0"/>
+      <minDepth>0.01</minDepth>
+    </gazebo>
+  </xacro:macro>
+
+  <xacro:macro name="set_all_wheel_frictions" params="prefix">
+    <xacro:set_wheel_friction link="${prefix}steer_link" friction="200"/>
+    <xacro:set_wheel_friction link="${prefix}drive_wheel_link" friction="200"/>
+    <xacro:set_wheel_friction link="${prefix}fixed_wheel_left_link" friction="1"/>
+    <xacro:set_wheel_friction link="${prefix}fixed_wheel_right_link" friction="1"/>
+  </xacro:macro>
+
+  <xacro:macro name="p3d_base_controller" params="prefix">
+    <gazebo>
+      <plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>50.0</updateRate>
+        <bodyName>${prefix}base_footprint</bodyName>
+        <topicName>base_pose_ground_truth</topicName>
+        <gaussianNoise>0.01</gaussianNoise>
+        <frameName>map</frameName>
+        <xyzOffsets>0 0 0</xyzOffsets>
+        <rpyOffsets>0 0 0</rpyOffsets>
+      </plugin>
+    </gazebo>
+  </xacro:macro>
+</robot>

Controllers/Packages/cititruck_description/urdf/include/cititruck.transmission.xacro

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="cititruck_wheel_transmission" params="prefix locationprefix">
+    <transmission name="${prefix}${locationprefix}_wheel_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}${locationprefix}_wheel_joint">
+        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}${locationprefix}_wheel_motor">
+        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+  </xacro:macro>
+
+  <xacro:macro name="cititruck_steer_transmission" params="prefix locationprefix">
+    <transmission name="${prefix}${locationprefix}_wheel_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}${locationprefix}_joint">
+        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}${locationprefix}_motor">
+        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+  </xacro:macro>
+
+</robot>

Controllers/Packages/cititruck_description/urdf/include/cititruck_v1.urdf.xacro

@@ -0,0 +1,251 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:arg name="robot_type" default="demo" />     
+  <xacro:property name="robot_type" value="$(arg robot_type)" />  <!-- necessary because args cannot be accessed inside ${} expressions -->
+
+  <xacro:include filename="$(find cititruck_description)/urdf/include/common_properties.urdf.xacro" />
+  <xacro:include filename="$(find cititruck_description)/urdf/include/cititruck.gazebo.xacro" />
+  <xacro:include filename="$(find cititruck_description)/urdf/include/cititruck.transmission.xacro" />
+  <xacro:include filename="$(find cititruck_description)/urdf/include/imu.gazebo.urdf.xacro" />
+  <xacro:include filename="$(find cititruck_description)/urdf/include/sick_s300.urdf.xacro" />
+  
+  <xacro:property name="deg_to_rad" value="0.017453293" />
+
+  <!-- The inertia for the MiR platform is intentionally chosen to be smaller than
+  the bounding box and also shifted a bit to the back, because most of the mass
+  is in the lower center back (because of the batteries). -->
+
+  <xacro:macro name="actuated_wheel" params="prefix locationprefix locationright">
+    <joint name="fixed_${locationprefix}_wheel_joint" type="continuous">
+      <parent link="${prefix}base_link"/>
+      <child link="${prefix}fixed_wheel_${locationprefix}_link"/>
+      <origin xyz="${locationright}" rpy="0 0 0" />
+      <axis xyz="0 1 0" />
+    </joint>
+
+    <link name="${prefix}fixed_wheel_${locationprefix}_link">
+      <inertial>
+        <mass value="1"/>
+        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
+        <origin/>
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="1.57 0 0" />
+        <geometry>
+          <cylinder radius="0.05" length="0.08" />
+        </geometry>
+        <xacro:insert_block name="material_black" />
+      </visual>
+
+      <collision>
+        <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
+        <geometry>
+          <cylinder radius="0.04" length="0.08" />
+        </geometry>
+      </collision>
+    </link>
+
+    <gazebo reference="${prefix}fixed_wheel_${locationprefix}_link">
+      <material>Gazebo/FlatBlack</material>
+    </gazebo>
+  </xacro:macro>
+  
+  <xacro:macro name="fork" params="locationprefix locationright"> 
+      <xacro:box_inertial mass="5" x="1" y="0" z="0">
+        <origin xyz="0 0 0" rpy="0 0 0" />
+      </xacro:box_inertial>
+      <visual>
+        <origin xyz="${locationright}" rpy="0 0 0" /> 
+        <geometry>
+          <box size="1.35 0.15 0.04" />
+        </geometry>
+          <xacro:insert_block name="material_grey" />
+      </visual>
+  </xacro:macro>
+
+<!-- robot -->
+  <xacro:macro name="cititruck" params="prefix">
+
+  <link name="${prefix}base_footprint" />
+
+  <joint name="${prefix}base_joint" type="fixed">
+    <parent link="${prefix}base_footprint" />
+    <child link="${prefix}base_link" />
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </joint>
+
+  <!-- Base Link -->
+  <link name="${prefix}base_link">
+    <xacro:box_inertial mass="30" x="1" y="0" z="0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="${1.3268 - 0.21 + 0.4/2} 0 ${0.8/2 + 0.3337 }" rpy="0 0 0" />
+      <geometry>
+        <box size="0.40 0.65 0.80" />
+      </geometry>
+        <xacro:insert_block name="material_white" />
+    </visual>
+
+    <visual>
+      <origin xyz="${1.3268 - 0.21 + 0.4/2} 0 ${0.3 - 0.01}" rpy="0 0 0" />
+      <geometry>
+        <box size="0.40 0.65 0.02" />
+      </geometry>
+        <xacro:insert_block name="material_grey" />
+    </visual>
+
+    <visual>
+      <origin xyz="${1.3268 - 0.21 + 0.02/2} 0 ${0.3 - (0.3 - 0.01)/2}" rpy="0 0 0" />
+      <geometry>
+        <box size="0.02 0.65 ${0.3 - 0.01}" />
+      </geometry>
+        <xacro:insert_block name="material_grey" />
+    </visual>
+
+    <collision>
+      <origin xyz="${1.3268 - 0.21 + 0.4/2} 0 ${0.8/2 + 0.3337 }" rpy="0 0 0" />
+      <geometry>
+        <box size="0.40 0.70 0.80" />
+      </geometry>
+    </collision>
+
+    <xacro:fork locationprefix="left" locationright="${1.12 - 1.35/2} 0.205 ${0.05/2 +  0.3 - 0.29}"/>
+    <xacro:fork locationprefix="right" locationright="${1.12 - 1.35/2} -0.205 ${0.05/2 +  0.3 - 0.29}"/>
+
+  </link>
+
+  <gazebo reference="${prefix}base_link">
+    <material>Gazebo/White</material>
+  </gazebo>
+
+  <!-- <xacro:fork prefix="${prefix}" locationprefix="left" parent="base_link" locationright="${0.9 - 1.15/2} 0.205 ${0.05/2 +  0.3 - 0.29}"/> -->
+  <!-- <xacro:fork prefix="${prefix}" locationprefix="right" parent="base_link" locationright="${0.9 - 1.15/2} -0.205 ${0.05/2 +  0.3 - 0.29}"/> -->
+
+  <joint name="${prefix}base2steer_joint" type="revolute">
+    <parent link="${prefix}base_link"/>
+    <child link="${prefix}steer_link"/>
+    <limit effort="10000.0" lower="-1.8" upper="1.8" velocity="2500"/>
+    <origin xyz="1.3268 0 0.125" rpy="0 0 0" />
+    <axis xyz="0 0 1" />
+  </joint>
+  <xacro:cititruck_steer_transmission prefix="${prefix}" locationprefix="base2steer"/>
+
+  <link name="${prefix}steer_link">
+    <inertial>
+      <mass value="45"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001" />
+      <origin/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="1.57 0 0" />
+      <geometry>
+        <cylinder radius="0.09" length="0.1" />
+      </geometry>
+      <xacro:insert_block name="material_dark_grey" />
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="1.57 0 0" />
+      <geometry>
+        <cylinder radius="0.09" length="0.1" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="${prefix}steer_link" >
+    <material>Gazebo/FlatBlack</material>
+  </gazebo> 
+
+  <link name="${prefix}steer_link_2" />
+
+  <joint name="${prefix}steer2steer_2_joint" type="fixed">
+    <parent link="${prefix}steer_link" />
+    <child link="${prefix}steer_link_2" />
+    <origin xyz="0 0 1.02" rpy="0 0 0" />
+  </joint>
+
+  <joint name="${prefix}steer2sd_wheel_joint" type="continuous">
+    <parent link="${prefix}steer_link"/>
+    <child link="${prefix}drive_wheel_link"/>
+    <limit effort="10000.0" velocity="100"/>
+    <origin xyz="0 0 0" rpy="0 0 0" />
+    <axis xyz="0 1 0" />
+  </joint>
+  <xacro:cititruck_wheel_transmission prefix="${prefix}" locationprefix="steer2sd"/>
+
+  <link name="${prefix}drive_wheel_link">
+    <inertial>
+      <mass value="10"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
+      <origin/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="1.57 0 0" />
+      <geometry>
+        <cylinder radius="0.125" length="0.08" />
+      </geometry>
+      <xacro:insert_block name="material_yellow" />
+    </visual>
+    
+    <collision>
+      <origin xyz="0 0 0" rpy="1.57 0 0" />
+      <geometry>
+        <cylinder radius="0.125" length="0.08" />
+      </geometry>
+    </collision>
+    
+  </link>
+
+  <gazebo reference="${prefix}drive_wheel_link" >
+    <material>Gazebo/FlatBlack</material>
+  </gazebo>
+
+  <xacro:actuated_wheel prefix="${prefix}" locationprefix="left" locationright="0.0 0.205 ${0.05}"/>
+  <xacro:actuated_wheel prefix="${prefix}" locationprefix="right" locationright="0.0 -0.205 ${0.05}" />
+
+  <!-- IMU -->
+  <joint name="${prefix}base_link_to_imu_joint" type="fixed">
+    <parent link="${prefix}base_link" />
+    <child link="${prefix}imu_link" />
+    <origin xyz="${1.3268 -0.15} 0.0 0.3" rpy="0 0 0" />  <!-- same as real MiR -->
+  </joint>
+
+  <link name="${prefix}imu_link" />
+
+  <xacro:imu_gazebo link="${prefix}imu_link" imu_topic="imu_data" update_rate="50.0" />
+
+  <!-- Create an alias for imu_link. This is necessary because the real MiR's
+        TF has imu_link, but the imu_data topic is published in the imu_frame
+        frame. -->
+  <joint name="${prefix}imu_link_to_imu_frame_joint" type="fixed">
+    <parent link="${prefix}imu_link" />
+    <child link="${prefix}imu_frame" />
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </joint>
+
+  <link name="${prefix}imu_frame" />
+
+
+  <!-- Laser scanners -->
+  <joint name="${prefix}base_link_to_left_laser_joint" type="fixed">
+    <parent link="${prefix}base_link" />
+    <child link="${prefix}left_laser_link" />
+    <origin xyz="1.5447 0.2953 0.1743" rpy="0.0 0.0 ${0.25 * pi}" />
+  </joint>
+  <xacro:sick_s300 prefix="${prefix}" link="left_laser_link" topic="l_scan" />
+
+  <joint name="${prefix}base_link_to_right_laser_joint" type="fixed">
+    <parent link="${prefix}base_link" />
+    <child link="${prefix}right_laser_link" />
+    <origin xyz="1.5447 -0.2953 0.1743" rpy="0.0 0.0 ${-0.25 * pi}" />
+  </joint>
+
+  <xacro:sick_s300 prefix="${prefix}" link="right_laser_link" topic="r_scan" />
+
+  <!-- set the gazebo friction parameters for the wheels -->
+  <xacro:set_all_wheel_frictions prefix="${prefix}"/>
+
+  <xacro:p3d_base_controller prefix="${prefix}" />
+</xacro:macro>
+
+</robot>

Controllers/Packages/cititruck_description/urdf/include/common.gazebo.xacro

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="controller_plugin_gazebo" params="robot_namespace">
+    <gazebo>
+      <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+        <xacro:unless value="${robot_namespace == ''}">
+          <robotNamespace>${robot_namespace}</robotNamespace>
+        </xacro:unless>
+        <controlPeriod>0.001</controlPeriod>
+        <legacyModeNS>false</legacyModeNS>
+      </plugin>
+    </gazebo>
+  </xacro:macro>
+</robot>

Controllers/Packages/cititruck_description/urdf/include/common_properties.urdf.xacro

@@ -0,0 +1,84 @@
+<?xml version="1.0" ?>
+<!--
+  Various useful properties such as constants and materials
+ -->
+<robot name="xacro_properties" xmlns:xacro="http://ros.org/wiki/xacro">
+  <!-- #################### RViz materials #################### -->
+  <xacro:property name="material_white">
+    <material name="white">
+      <color rgba="1 1 1 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_yellow">
+    <material name="yellow">
+      <color rgba="${255/255} ${226/255} ${0/255} 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_almost_white">
+    <material name="almost_white">
+      <color rgba="0.9 0.9 0.9 1" />
+    </material>
+  </xacro:property>
+  <xacro:property name="material_grey">
+    <material name="grey">
+      <color rgba="0.5 0.5 0.5 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_light_grey">
+    <material name="light_grey">
+      <color rgba="0.6 0.6 0.6 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_dark_grey">
+    <material name="dark_grey">
+      <color rgba="0.3 0.3 0.3 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_black">
+    <material name="black">
+      <color rgba="0.1 0.1 0.1 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_aluminum">
+    <material name="aluminum">
+      <color rgba="0.8 0.8 0.8 1" />
+    </material>
+  </xacro:property>
+  <xacro:property name="material_silver">
+    <material name="silver">
+      <color rgba="0.79 0.82 0.93 1" />
+    </material>
+  </xacro:property>
+
+  <!-- #################### inertials with origin #################### -->
+  <!-- see https://secure.wikimedia.org/wikipedia/en/wiki/List_of_moment_of_inertia_tensors -->
+  <xacro:macro name="sphere_inertial" params="radius mass *origin">
+    <inertial>
+      <mass value="${mass}" />
+      <xacro:insert_block name="origin" />
+      <inertia ixx="${0.4 * mass * radius * radius}" ixy="0.0" ixz="0.0"
+        iyy="${0.4 * mass * radius * radius}" iyz="0.0"
+        izz="${0.4 * mass * radius * radius}" />
+    </inertial>
+  </xacro:macro>
+
+  <xacro:macro name="cylinder_inertial" params="radius length mass *origin">
+    <inertial>
+      <mass value="${mass}" />
+      <xacro:insert_block name="origin" />
+      <inertia ixx="${0.0833333 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0"
+        iyy="${0.0833333 * mass * (3 * radius * radius + length * length)}" iyz="0.0"
+        izz="${0.5 * mass * radius * radius}" />
+    </inertial>
+  </xacro:macro>
+
+  <xacro:macro name="box_inertial" params="x y z mass *origin">
+    <inertial>
+      <mass value="${mass}" />
+      <xacro:insert_block name="origin" />
+      <inertia ixx="${0.0833333 * mass * (y*y + z*z)}" ixy="0.0" ixz="0.0"
+        iyy="${0.0833333 * mass * (x*x + z*z)}" iyz="0.0"
+        izz="${0.0833333 * mass * (x*x + y*y)}" />
+    </inertial>
+  </xacro:macro>
+</robot>

Controllers/Packages/cititruck_description/urdf/include/imu.gazebo.urdf.xacro

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <!-- If tf_prefix is given, use "frame tf_prefix/imu_frame", else "imu_frame" -->
+  <xacro:arg name="tf_prefix" default="" />
+  <xacro:property name="tf_prefix" value="$(arg tf_prefix)" />
+  <xacro:if value="${tf_prefix == ''}">
+      <xacro:property name="imu_frame" value="imu_frame" />
+  </xacro:if>
+  <xacro:unless value="${tf_prefix == ''}">
+      <xacro:property name="imu_frame" value="$(arg tf_prefix)/imu_frame" />
+  </xacro:unless>
+
+
+  <xacro:macro name="imu_gazebo" params="link imu_topic update_rate">
+    <gazebo>
+      <plugin name="imu_plugin" filename="libhector_gazebo_ros_imu.so">
+        <updateRate>${update_rate}</updateRate>
+        <bodyName>${link}</bodyName>
+        <frameId>${imu_frame}</frameId>  <!-- from real MiR -->
+        <topicName>${imu_topic}</topicName>
+        <accelDrift>0.0 0.0 0.0</accelDrift>
+        <accelGaussianNoise>${sqrt(5e-05)} ${sqrt(0.0001)} ${sqrt(0.00013)}</accelGaussianNoise>    <!-- real MiR linear_acceleration_covariance: [5e-05, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.00013] -->
+        <rateDrift>0.0 0.0 0.0</rateDrift>
+        <rateGaussianNoise>${sqrt(8e-06)} ${sqrt(8e-06)} ${sqrt(3e-07)}</rateGaussianNoise>         <!-- real MiR angular_velocity_covariance: [8e-06, 0.0, 0.0, 0.0, 8e-06, 0.0, 0.0, 0.0, 3e-07] -->
+        <yawDrift>0.0</yawDrift>
+        <yawGaussianNoise>${sqrt(0.1)}</yawGaussianNoise>                                           <!-- real MiR orientation_covariance: [0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1] -->
+      </plugin>
+    </gazebo>
+  </xacro:macro>
+</robot>

Controllers/Packages/cititruck_description/urdf/include/materials.xacro

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<robot>
+
+  <material name="black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+
+  <material name="blue">
+    <color rgba="0.0 0.0 0.8 1.0"/>
+  </material>
+
+  <material name="green">
+    <color rgba="0.0 0.8 0.0 1.0"/>
+  </material>
+
+  <material name="grey">
+    <color rgba="0.2 0.2 0.2 1.0"/>
+  </material>
+
+  <material name="grey2">
+    <color rgba="0.4 0.4 0.4 1.0"/>
+  </material>
+
+  <material name="orange">
+    <color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
+  </material>
+
+  <material name="brown">
+    <color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
+  </material>
+
+  <material name="red">
+    <color rgba="0.8 0.0 0.0 1.0"/>
+  </material>
+
+  <material name="white">
+    <color rgba="1.0 1.0 1.0 1.0"/>
+  </material>
+
+</robot>

Controllers/Packages/cititruck_description/urdf/include/sensor_calibration_assembly.xacro

@@ -0,0 +1,185 @@
+<?xml version="1.0"?>
+
+<!--
+# (c) 2018 EU H2020 Project ILIAD, iliad-project.eu
+# The following code in this file is licensed under the MIT license:
+# 
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+-->
+
+<!-- This xacro contains a macro for spawning a sensor calibration assembly inside a robot's URDF file.
+ The idea is that on the final robot, the assembly just consists of two fixed joints, which have the origin and rpy values set properly
+ to reflect the sensor's extrinsic calibration values.
+
+ Instead, during the calibration phase, these two fixed joints are replaced by a chain of nine prismatic+revolute joints, such that
+ each dimension can be tweaked individually using the sliders in the joint state publisher GUI.
+
+ Once calibrated, calibration values should be saved via save_calibration.py.
+-->
+
+<robot name="sensor_calibration_assembly_macro" xmlns:xacro="http://ros.org/wiki/xacro">
+
+     <!-- Command-line arguments -->
+    <xacro:arg name="use_fixed_joints" default="true"/> <!-- Set to false for calibration -->
+    <xacro:arg name="calib_file" default=""/> <!-- Same file name should be passed to joint state publisher. Should be set up like this:
+        zeros:
+                velodyne_calib_pre_x: 0.1
+                velodyne_calib_pre_y: -0.2
+                ...
+                velodyne_calib_roll: 0.10
+                velodyne_calib_pitch: -0.03
+                ..
+                velodyne_calib_post_x: 0.05
+            -->
+
+    <!-- Helper macro -->
+    <xacro:macro name="simple_link" params="name">
+        <link name="${name}">
+            <inertia ixx="3.64583333333e-07" ixy="0.0" ixz="0.0" iyy="3.64583333333e-07" iyz="0.0" izz="3.125e-07"/>
+        </link>
+    </xacro:macro>
+
+    <!-- Invoke this macro to create a sensor calibration assembly (XYZ offset + rotation + XYZ post-offset). -->
+    <xacro:macro name="sensor_calibration_assembly" params="sensor_id parent_link create_final_link:=true final_link_suffix:='link'">
+
+        <!-- Lighter version with a single fixed pre-calibrated joint per sensor (for use in Gazebo and on the actual robot) -->
+        <xacro:if value="$(arg use_fixed_joints)">
+            <!-- Load resulting transforms exported by save_calibration.py, and use them for the fixed offset and rpy -->
+            <xacro:property name="all_sensor_tfs" value="${load_yaml('$(arg calib_file)').get('resulting_transforms', dict())}"/>
+            <xacro:property name="sensor_tf" value="${all_sensor_tfs.get(sensor_id, [0.0]*6)}"/>
+            <xacro:property name="xyz" value="${' '.join(str(x) for x in sensor_tf[0:3] )}"/>
+            <xacro:property name="rpy" value="${' '.join(str(x) for x in sensor_tf[3:6] )}"/>
+
+            <!-- Resulting transform -->
+            <joint name="${sensor_id}_resulting_transform_joint" type="fixed">
+                <origin xyz="${xyz}" rpy="${rpy}"/>
+                <parent link="${parent_link}"/>
+                <child link="${sensor_id}_${final_link_suffix}"/>
+            </joint>
+        </xacro:if>
+
+        <!-- Version with movable joints (only for calib use with joint state publisher GUI) -->
+        <xacro:unless value="$(arg use_fixed_joints)">
+            <!-- Dynamic joint limits -->
+            <xacro:property name="max_trans" value="1.0"/>
+            <xacro:property name="max_rot" value="3.14159"/>
+
+            <!-- Zero values will be loaded by joint state publisher! Joints should be 1D because only 1D values are supported in the GUI -->
+
+            <!-- Initial dummy link (zero transform) that starts the chain -->
+            <!-- Do not remove, required for automatic saving! -->
+            <simple_link name="${sensor_id}_calib_start_link"/>
+            <joint name="${sensor_id}_calib_start" type="fixed">
+                <parent link="${parent_link}"/>
+                <child link="${sensor_id}_calib_start_link"/>
+            </joint>
+
+            <!-- Pre-translation -->
+            <simple_link name="${sensor_id}_calib_pre_x_link"/>
+            <joint name="${sensor_id}_calib_pre_x" type="prismatic">
+                <axis xyz="1 0 0"/>
+                <limit lower="-${max_trans}" upper="+${max_trans}" effort="1" velocity="1"/>
+                <parent link="${sensor_id}_calib_start_link"/>
+                <child link="${sensor_id}_calib_pre_x_link"/>
+            </joint>
+            
+            <simple_link name="${sensor_id}_calib_pre_y_link"/>
+            <joint name="${sensor_id}_calib_pre_y" type="prismatic">
+                <axis xyz="0 1 0"/>
+                <limit lower="-${max_trans}" upper="+${max_trans}" effort="1" velocity="1"/>
+                <parent link="${sensor_id}_calib_pre_x_link"/>
+                <child link="${sensor_id}_calib_pre_y_link"/>
+            </joint>
+            
+            <simple_link name="${sensor_id}_calib_pre_z_link"/>
+            <joint name="${sensor_id}_calib_pre_z" type="prismatic">
+                <axis xyz="0 0 1"/>
+                <limit lower="-${max_trans}" upper="+${max_trans}" effort="1" velocity="1"/>
+                <parent link="${sensor_id}_calib_pre_y_link"/>
+                <child link="${sensor_id}_calib_pre_z_link"/>
+            </joint>
+
+            <!-- Rotation -->
+            <simple_link name="${sensor_id}_calib_roll_link"/>
+            <joint name="${sensor_id}_calib_roll" type="revolute">
+                <axis xyz="1 0 0"/>
+                <limit lower="-${max_rot}" upper="+${max_rot}" effort="1" velocity="1"/>
+                <parent link="${sensor_id}_calib_pre_z_link"/>
+                <child link="${sensor_id}_calib_roll_link"/>
+            </joint>
+            
+            <simple_link name="${sensor_id}_calib_pitch_link"/>
+            <joint name="${sensor_id}_calib_pitch" type="revolute">
+                <axis xyz="0 1 0"/>
+                <limit lower="-${max_rot}" upper="+${max_rot}" effort="1" velocity="1"/>
+                <parent link="${sensor_id}_calib_roll_link"/>
+                <child link="${sensor_id}_calib_pitch_link"/>
+            </joint>
+            
+            <simple_link name="${sensor_id}_calib_yaw_link"/>
+            <joint name="${sensor_id}_calib_yaw" type="revolute">
+                <axis xyz="0 0 1"/>
+                <limit lower="-${max_rot}" upper="+${max_rot}" effort="1" velocity="1"/>
+                <parent link="${sensor_id}_calib_pitch_link"/>
+                <child link="${sensor_id}_calib_yaw_link"/>
+            </joint>
+
+            <!-- Post-translation (after rotation) -->
+            <simple_link name="${sensor_id}_calib_post_x_link"/>
+            <joint name="${sensor_id}_calib_post_x" type="prismatic">
+                <axis xyz="1 0 0"/>
+                <limit lower="-${max_trans}" upper="+${max_trans}" effort="1" velocity="1"/>
+                <parent link="${sensor_id}_calib_yaw_link"/>
+                <child link="${sensor_id}_calib_post_x_link"/>
+            </joint>
+            
+            <simple_link name="${sensor_id}_calib_post_y_link"/>
+            <joint name="${sensor_id}_calib_post_y" type="prismatic">
+                <axis xyz="0 1 0"/>
+                <limit lower="-${max_trans}" upper="+${max_trans}" effort="1" velocity="1"/>
+                <parent link="${sensor_id}_calib_post_x_link"/>
+                <child link="${sensor_id}_calib_post_y_link"/>
+            </joint>
+
+            <simple_link name="${sensor_id}_calib_post_z_link"/>
+            <joint name="${sensor_id}_calib_post_z" type="prismatic">
+                <axis xyz="0 0 1"/>
+                <limit lower="-${max_trans}" upper="+${max_trans}" effort="1" velocity="1"/>
+                <parent link="${sensor_id}_calib_post_y_link"/>
+                <child link="${sensor_id}_calib_post_z_link"/>
+            </joint>
+
+            <!-- Final dummy link (zero transform) that ends the chain -->
+            <!-- Do not remove, required for automatic saving! -->
+            <joint name="${sensor_id}_calib_end_link" type="fixed">
+                <parent link="${sensor_id}_calib_post_z_link"/>
+                <child link="${sensor_id}_${final_link_suffix}"/>
+            </joint>
+
+            <!-- End of version with movable joints for calib -->  
+        </xacro:unless>
+
+        <!-- Final link -->
+        <xacro:if value="${create_final_link}">
+            <simple_link name="${sensor_id}_${final_link_suffix}"/>
+        </xacro:if>
+        
+    </xacro:macro>
+
+</robot>

Controllers/Packages/cititruck_description/urdf/include/sick_s300.urdf.xacro

@@ -0,0 +1,72 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find cititruck_description)/urdf/include/common_properties.urdf.xacro" />
+
+  <xacro:property name="laser_x" value="0.156" />
+  <xacro:property name="laser_y" value="0.155" />
+  <xacro:property name="laser_z" value="0.185" />
+  <xacro:property name="laser_mass" value="1.2" />
+
+  <xacro:macro name="sick_s300" params="link topic prefix">
+    <link name="${prefix}${link}">
+      <visual>
+        <origin xyz="0.0 0.0 0.0" rpy="${pi} 0 0" />
+        <geometry>
+          <mesh filename="package://cititruck_description/meshes/visual/sick_lms-100.stl" />
+        </geometry>
+        <!-- <xacro:insert_block name="material_yellow" /> -->
+        <xacro:insert_block name="material_black" />
+      </visual>
+      <collision>
+        <origin xyz="0.0 0.0 0.0" rpy="${pi} 0 0" />
+        <geometry>
+          <mesh filename="package://cititruck_description/meshes/visual/sick_lms-100.stl" />
+        </geometry>
+      </collision>
+      <xacro:box_inertial x="${laser_x}" y="${laser_y}" z="${laser_z}" mass="${laser_mass}">
+        <origin xyz="0 0 0" />
+      </xacro:box_inertial>
+    </link>
+
+    <gazebo reference="${prefix}${link}">
+      <!-- <material value="Gazebo/Yellow" /> -->
+      <material value="Gazebo/FlatBlack" />
+
+      <sensor type="ray" name="${prefix}${link}">
+        <pose>0 0 0 0 0 0</pose>
+        <visualize>false</visualize>
+        <update_rate>12.5</update_rate>
+        <ray>
+          <scan>
+            <horizontal>
+              <samples>541</samples>
+              <resolution>1</resolution>  <!-- has to be 1; actual resolution will be computed from number of samples + min_angle/max_angle -->
+              <!-- <min_angle>-2.35619449615</min_angle>
+              <max_angle>2.35619449615</max_angle> -->
+              <min_angle>-2.094395102</min_angle>
+              <max_angle>2.181661565</max_angle>
+            </horizontal>
+          </scan>
+          <range>
+            <min>0.05</min>
+            <max>29.0</max>
+            <resolution>0.01</resolution>
+          </range>
+          <noise>
+            <type>gaussian</type>
+            <!-- Noise parameters based on published spec for S300 achieving
+                 "+-29mm" accuracy at range < 3m (~0.01 of the range) at
+                 1 sigma. -->
+            <mean>0.0</mean>
+            <stddev>0.01</stddev>
+          </noise>
+        </ray>
+        <plugin name="gazebo_ros_${link}_controller" filename="libgazebo_ros_laser.so">
+          <frameName>${prefix}${link}</frameName>
+          <topicName>${topic}</topicName>
+        </plugin>
+      </sensor>
+    </gazebo>
+  </xacro:macro>
+</robot>

Controllers/Packages/robot_gazebo_plugins/cititruck_driver/CMakeLists.txt

@@ -0,0 +1,63 @@
+cmake_minimum_required(VERSION 3.5.1)
+project(cititruck_driver)
+
+find_package(catkin REQUIRED COMPONENTS
+  actionlib_msgs
+  diagnostic_msgs
+  dynamic_reconfigure
+  geometry_msgs
+  move_base_msgs
+  nav_msgs
+  rosgraph_msgs
+  roslaunch
+  rospy
+  rospy_message_converter
+  sensor_msgs
+  std_msgs
+  tf
+  visualization_msgs
+)
+
+catkin_python_setup()
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  CATKIN_DEPENDS
+    actionlib_msgs
+    diagnostic_msgs
+    dynamic_reconfigure
+    geometry_msgs
+    move_base_msgs
+    nav_msgs
+    rosgraph_msgs
+    rospy_message_converter
+    sensor_msgs
+    std_msgs
+    tf
+    visualization_msgs
+)
+
+#############
+## Install ##
+#############
+
+catkin_install_python(PROGRAMS
+  nodes/fake_cititruck_joint_publisher.py
+  nodes/cititruck_bridge.py
+  nodes/rep117_filter.py
+  nodes/tf_remove_child_frames.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+# Mark other files for installation (e.g. launch and bag files, etc.)
+install(DIRECTORY
+  launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+roslaunch_add_file_check(launch)

Controllers/Packages/robot_gazebo_plugins/cititruck_driver/launch/mir.launch

@@ -0,0 +1,85 @@
+<launch>
+  <arg name="mir_type" default="mir_250" doc="The MiR variant. Can be 'mir_100' or 'mir_250' for now." />
+
+  <arg name="tf_prefix" default="" doc="TF prefix to use for all of MiR's TF frames"/>
+
+  <arg name="mir_hostname" default="192.168.12.20" />
+
+  <arg name="disable_map" default="false" doc="Disable the map topic and map -> odom TF transform from the MiR" />
+
+  <param name="tf_prefix" type="string" value="$(arg tf_prefix)"/>
+
+  <!-- URDF -->
+  <include file="$(find cititruck_description)/launch/upload_cititruck_urdf.launch">
+    <arg name="mir_type" value="$(arg mir_type)" />
+  </include>
+
+  <!-- Robot state publisher -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
+    <remap from="/tf"        to="tf_rss" />
+    <remap from="/tf_static" to="tf_static_rss" />
+  </node>
+
+  <!-- remove those TFs that are also published by the MiR to avoid conflicts -->
+  <node name="tf_remove_state_publisher_frames" pkg="cititruck_driver" type="tf_remove_child_frames.py" output="screen">
+    <remap from="tf_in"         to="tf_rss" />
+    <remap from="tf_out"        to="/tf" />
+    <remap from="tf_static_in"  to="tf_static_rss" />
+    <remap from="tf_static_out" to="/tf_static" />
+    <rosparam param="remove_frames">
+      - base_link
+      - front_laser_link
+      - back_laser_link
+      - camera_top_link
+      - camera_top_depth_optical_frame
+      - camera_floor_link
+      - camera_floor_depth_optical_frame
+      - imu_link
+    </rosparam>
+  </node>
+
+  <!-- MiR base -->
+  <group if="$(arg disable_map)">
+    <node name="cititruck_bridge" pkg="cititruck_driver" type="cititruck_bridge.py" output="screen">
+      <param name="hostname" value="$(arg mir_hostname)" />
+      <param name="tf_prefix" value="$(arg tf_prefix)" />
+      <remap from="map" to="map_mir" />
+      <remap from="map_metadata" to="map_metadata_mir" />
+      <remap from="rosout" to="/rosout" />
+      <remap from="rosout_agg" to="/rosout_agg" />
+      <remap from="tf" to="tf_mir" />
+    </node>
+
+    <!-- remove the map -> odom TF transform -->
+    <node name="tf_remove_mir_map_frame" pkg="cititruck_driver" type="tf_remove_child_frames.py" output="screen">
+      <remap from="tf_in"         to="tf_mir" />
+      <remap from="tf_out"        to="/tf" />
+      <rosparam param="remove_frames">
+        - odom
+      </rosparam>
+    </node>
+  </group>
+  <group unless="$(arg disable_map)">
+    <node name="cititruck_bridge" pkg="cititruck_driver" type="cititruck_bridge.py" output="screen">
+      <param name="hostname" value="$(arg mir_hostname)" />
+      <param name="tf_prefix" value="$(arg tf_prefix)" />
+      <remap from="map" to="/map" />
+      <remap from="map_metadata" to="/map_metadata" />
+      <remap from="rosout" to="/rosout" />
+      <remap from="rosout_agg" to="/rosout_agg" />
+      <remap from="tf" to="/tf" />
+    </node>
+  </group>
+
+  <node name="b_rep117_laser_filter" pkg="cititruck_driver" type="rep117_filter.py" output="screen">
+    <remap from="scan" to="b_scan" />
+    <remap from="scan_filtered" to="b_scan_rep117" />
+  </node>
+
+  <node name="f_rep117_laser_filter" pkg="cititruck_driver" type="rep117_filter.py" output="screen">
+    <remap from="scan" to="f_scan" />
+    <remap from="scan_filtered" to="f_scan_rep117" />
+  </node>
+
+  <node name="fake_mir_joint_publisher" pkg="cititruck_driver" type="fake_mir_joint_publisher.py" output="screen" />
+</launch>

Controllers/Packages/robot_gazebo_plugins/cititruck_driver/nodes/cititruck_bridge.py

@@ -0,0 +1,529 @@
+#!/usr/bin/env python3
+# Copyright (c) 2018-2022, Martin Günther (DFKI GmbH) and contributors
+#
+# 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 the copyright holder 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 HOLDER 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.
+#
+# Author: Martin Günther
+
+import rospy
+
+import copy
+import sys
+from collections.abc import Iterable
+
+from cititruck_driver import rosbridge
+from rospy_message_converter import message_converter
+
+from actionlib import SimpleActionClient
+import actionlib_msgs.msg
+import diagnostic_msgs.msg
+import dynamic_reconfigure.msg
+import geometry_msgs.msg
+import cititruck_actions.msg
+import cititruck_msgs.msg
+import move_base_msgs.msg
+import nav_msgs.msg
+import rosgraph_msgs.msg
+import sdc21x0.msg
+import sensor_msgs.msg
+import std_msgs.msg
+import tf2_msgs.msg
+import visualization_msgs.msg
+
+from collections import OrderedDict
+
+tf_prefix = ''
+static_transforms = OrderedDict()
+
+
+class TopicConfig(object):
+    def __init__(self, topic, topic_type, latch=False, dict_filter=None):
+        self.topic = topic
+        self.topic_type = topic_type
+        self.latch = latch
+        self.dict_filter = dict_filter
+
+
+# remap cititruck_actions/cititruckMoveBaseAction => move_base_msgs/MoveBaseAction
+def _move_base_goal_dict_filter(msg_dict):
+    filtered_msg_dict = copy.deepcopy(msg_dict)
+    filtered_msg_dict['goal']['move_task'] = cititruck_actions.msg.cititruckMoveBaseGoal.GLOBAL_MOVE
+    filtered_msg_dict['goal']['goal_dist_threshold'] = 0.25
+    filtered_msg_dict['goal']['clear_costmaps'] = True
+    return filtered_msg_dict
+
+
+def _move_base_feedback_dict_filter(msg_dict):
+    # filter out slots from the dict that are not in our message definition
+    # e.g., cititruckMoveBaseFeedback has the field "state", which MoveBaseFeedback doesn't
+    filtered_msg_dict = copy.deepcopy(msg_dict)
+    filtered_msg_dict['feedback'] = {
+        key: msg_dict['feedback'][key] for key in move_base_msgs.msg.MoveBaseFeedback.__slots__
+    }
+    return filtered_msg_dict
+
+
+def _move_base_result_dict_filter(msg_dict):
+    filtered_msg_dict = copy.deepcopy(msg_dict)
+    filtered_msg_dict['result'] = {key: msg_dict['result'][key] for key in move_base_msgs.msg.MoveBaseResult.__slots__}
+    return filtered_msg_dict
+
+
+def _cmd_vel_dict_filter(msg_dict):
+    """
+    Convert Twist to TwistStamped.
+
+    Convert a geometry_msgs/Twist message dict (as sent from the ROS side) to
+    a geometry_msgs/TwistStamped message dict (as expected by the cititruck on
+    software version >=2.7).
+    """
+    header = std_msgs.msg.Header(frame_id='', stamp=rospy.Time.now())
+    filtered_msg_dict = {
+        'header': message_converter.convert_ros_message_to_dictionary(header),
+        'twist': copy.deepcopy(msg_dict),
+    }
+    return filtered_msg_dict
+
+
+def _tf_dict_filter(msg_dict):
+    filtered_msg_dict = copy.deepcopy(msg_dict)
+    for transform in filtered_msg_dict['transforms']:
+        transform['child_frame_id'] = tf_prefix + '/' + transform['child_frame_id'].strip('/')
+    return filtered_msg_dict
+
+
+def _tf_static_dict_filter(msg_dict):
+    """
+    Cache tf_static messages (simulate latching).
+
+    The tf_static topic needs special handling. Publishers on tf_static are *latched*, which means that the ROS master
+    caches the last message that was sent by each publisher on that topic, and will forward it to new subscribers.
+    However, since the cititruck_driver node appears to the ROS master as a single node with a single publisher on tf_static,
+    and there are multiple actual publishers hiding behind it on the cititruck side, only one of those messages will be
+    cached. Therefore, we need to implement the caching ourselves and make sure that we always publish the full set of
+    transforms as a single message.
+    """
+    global static_transforms
+
+    # prepend tf_prefix
+    filtered_msg_dict = _tf_dict_filter(msg_dict)
+
+    # The following code was copied + modified from https://github.com/tradr-project/static_transform_mux .
+
+    # Process the incoming transforms, merge them with our cache.
+    for transform in filtered_msg_dict['transforms']:
+        key = transform['child_frame_id']
+        rospy.loginfo(
+            "[%s] tf_static: updated transform %s->%s.",
+            rospy.get_name(),
+            transform['header']['frame_id'],
+            transform['child_frame_id'],
+        )
+        static_transforms[key] = transform
+
+    # Return the cached messages.
+    filtered_msg_dict['transforms'] = static_transforms.values()
+    rospy.loginfo(
+        "[%s] tf_static: sent %i transforms: %s",
+        rospy.get_name(),
+        len(static_transforms),
+        str(static_transforms.keys()),
+    )
+    return filtered_msg_dict
+
+
+def _prepend_tf_prefix_dict_filter(msg_dict):
+    # filtered_msg_dict = copy.deepcopy(msg_dict)
+    if not isinstance(msg_dict, dict):  # can happen during recursion
+        return
+    for (key, value) in msg_dict.items():
+        if key == 'header':
+            try:
+                # prepend frame_id
+                frame_id = value['frame_id'].strip('/')
+                if frame_id != 'map':
+                    # prepend tf_prefix, then remove leading '/' (e.g., when tf_prefix is empty)
+                    value['frame_id'] = (tf_prefix + '/' + frame_id).strip('/')
+                else:
+                    value['frame_id'] = frame_id
+
+            except TypeError:
+                pass  # value is not a dict
+            except KeyError:
+                pass  # value doesn't have key 'frame_id'
+        elif isinstance(value, dict):
+            _prepend_tf_prefix_dict_filter(value)
+        elif isinstance(value, Iterable):  # an Iterable other than dict (e.g., a list)
+            for item in value:
+                _prepend_tf_prefix_dict_filter(item)
+    return msg_dict
+
+
+def _remove_tf_prefix_dict_filter(msg_dict):
+    # filtered_msg_dict = copy.deepcopy(msg_dict)
+    if not isinstance(msg_dict, dict):  # can happen during recursion
+        return
+    for (key, value) in msg_dict.items():
+        if key == 'header':
+            try:
+                # remove frame_id
+                s = value['frame_id'].strip('/')
+                if s.find(tf_prefix) == 0:
+                    value['frame_id'] = (s[len(tf_prefix) :]).strip('/')  # strip off tf_prefix, then strip leading '/'
+            except TypeError:
+                pass  # value is not a dict
+            except KeyError:
+                pass  # value doesn't have key 'frame_id'
+        elif isinstance(value, dict):
+            _remove_tf_prefix_dict_filter(value)
+        elif isinstance(value, Iterable):  # an Iterable other than dict (e.g., a list)
+            for item in value:
+                _remove_tf_prefix_dict_filter(item)
+    return msg_dict
+
+
+# topics we want to publish to ROS (and subscribe to from the cititruck)
+PUB_TOPICS = [
+    # TopicConfig('LightCtrl/bms_data', cititruck_msgs.msg.BMSData),
+    # TopicConfig('LightCtrl/charging_state', cititruck_msgs.msg.ChargingState),
+    TopicConfig('LightCtrl/us_list', sensor_msgs.msg.Range),
+    # TopicConfig('MC/battery_currents', cititruck_msgs.msg.BatteryCurrents),
+    # TopicConfig('MC/battery_voltage', std_msgs.msg.Float64),
+    TopicConfig('MC/currents', sdc21x0.msg.MotorCurrents),
+    # TopicConfig('MC/encoders', sdc21x0.msg.StampedEncoders),
+    TopicConfig('MissionController/CheckArea/visualization_marker', visualization_msgs.msg.Marker),
+    # TopicConfig('MissionController/goal_position_guid', std_msgs.msg.String),
+    # TopicConfig('MissionController/prompt_user', cititruck_msgs.msg.UserPrompt),
+    TopicConfig('SickPLC/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    TopicConfig('SickPLC/parameter_updates', dynamic_reconfigure.msg.Config),
+    # TopicConfig('active_mapping_guid', std_msgs.msg.String),
+    TopicConfig('amcl_pose', geometry_msgs.msg.PoseWithCovarianceStamped),
+    TopicConfig('b_raw_scan', sensor_msgs.msg.LaserScan),
+    TopicConfig('b_scan', sensor_msgs.msg.LaserScan),
+    TopicConfig('camera_floor/background', sensor_msgs.msg.PointCloud2),
+    TopicConfig('camera_floor/depth/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    TopicConfig('camera_floor/depth/parameter_updates', dynamic_reconfigure.msg.Config),
+    TopicConfig('camera_floor/depth/points', sensor_msgs.msg.PointCloud2),
+    TopicConfig('camera_floor/filter/visualization_marker', visualization_msgs.msg.Marker),
+    TopicConfig('camera_floor/floor', sensor_msgs.msg.PointCloud2),
+    TopicConfig('camera_floor/obstacles', sensor_msgs.msg.PointCloud2),
+    TopicConfig('check_area/polygon', geometry_msgs.msg.PolygonStamped),
+    # TopicConfig('check_pose_area/polygon', geometry_msgs.msg.PolygonStamped),
+    # TopicConfig('data_events/area_events', cititruck_data_msgs.msg.AreaEventEvent),
+    # TopicConfig('data_events/maps', cititruck_data_msgs.msg.MapEvent),
+    # TopicConfig('data_events/positions', cititruck_data_msgs.msg.PositionEvent),
+    # TopicConfig('data_events/registers', cititruck_data_msgs.msg.PLCRegisterEvent),
+    # TopicConfig('data_events/sounds', cititruck_data_msgs.msg.SoundEvent),
+    TopicConfig('diagnostics', diagnostic_msgs.msg.DiagnosticArray),
+    TopicConfig('diagnostics_agg', diagnostic_msgs.msg.DiagnosticArray),
+    TopicConfig('diagnostics_toplevel_state', diagnostic_msgs.msg.DiagnosticStatus),
+    TopicConfig('f_raw_scan', sensor_msgs.msg.LaserScan),
+    TopicConfig('f_scan', sensor_msgs.msg.LaserScan),
+    TopicConfig('imu_data', sensor_msgs.msg.Imu),
+    TopicConfig('laser_back/driver/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    TopicConfig('laser_back/driver/parameter_updates', dynamic_reconfigure.msg.Config),
+    TopicConfig('laser_front/driver/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    TopicConfig('laser_front/driver/parameter_updates', dynamic_reconfigure.msg.Config),
+    # TopicConfig('localization_score', std_msgs.msg.Float64),
+    TopicConfig('/map', nav_msgs.msg.OccupancyGrid, latch=True),
+    TopicConfig('/map_metadata', nav_msgs.msg.MapMetaData),
+    # TopicConfig('marker_tracking_node/feedback', cititruck_marker_tracking.msg.MarkerTrackingActionFeedback),
+    # TopicConfig(
+    #     'marker_tracking_node/laser_line_extract/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription
+    # ),
+    # TopicConfig('marker_tracking_node/laser_line_extract/parameter_updates', dynamic_reconfigure.msg.Config),
+    # TopicConfig('marker_tracking_node/laser_line_extract/visualization_marker', visualization_msgs.msg.Marker),
+    # TopicConfig('marker_tracking_node/result', cititruck_marker_tracking.msg.MarkerTrackingActionResult),
+    # TopicConfig('marker_tracking_node/status', actionlib_msgs.msg.GoalStatusArray),
+    # TopicConfig('cititruckEventTrigger/events', cititruck_msgs.msg.Events),
+    TopicConfig('cititruck_amcl/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    TopicConfig('cititruck_amcl/parameter_updates', dynamic_reconfigure.msg.Config),
+    TopicConfig('cititruck_amcl/selected_points', sensor_msgs.msg.PointCloud2),
+    TopicConfig('cititruck_log', rosgraph_msgs.msg.Log),
+    # TopicConfig('cititruck_sound/sound_event', cititruck_msgs.msg.SoundEvent),
+    TopicConfig('cititruck_status_msg', std_msgs.msg.String),
+    # TopicConfig('cititruckspawn/node_events', cititruckSpawn.msg.LaunchItem),
+    TopicConfig('cititruckwebapp/grid_map_metadata', cititruck_msgs.msg.LocalMapStat),
+    TopicConfig('cititruckwebapp/laser_map_metadata', cititruck_msgs.msg.LocalMapStat),
+    # TopicConfig('cititruckwebapp/web_path', cititruck_msgs.msg.WebPath),
+    # really cititruck_actions/cititruckMoveBaseActionFeedback:
+    TopicConfig(
+        'move_base/feedback', move_base_msgs.msg.MoveBaseActionFeedback, dict_filter=_move_base_feedback_dict_filter
+    ),
+    # really cititruck_actions/cititruckMoveBaseActionResult:
+    TopicConfig('move_base/result', move_base_msgs.msg.MoveBaseActionResult, dict_filter=_move_base_result_dict_filter),
+    TopicConfig('move_base/status', actionlib_msgs.msg.GoalStatusArray),
+    # TopicConfig('move_base_node/cititruckPlannerROS/cost_cloud', sensor_msgs.msg.PointCloud2),
+    # TopicConfig('move_base_node/cititruckPlannerROS/global_plan', nav_msgs.msg.Path),
+    # TopicConfig('move_base_node/cititruckPlannerROS/len_to_goal', std_msgs.msg.Float64),
+    TopicConfig('move_base_node/cititruckPlannerROS/local_plan', nav_msgs.msg.Path),
+    # TopicConfig('move_base_node/cititruckPlannerROS/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    # TopicConfig('move_base_node/cititruckPlannerROS/parameter_updates', dynamic_reconfigure.msg.Config),
+    TopicConfig('move_base_node/cititruckPlannerROS/updated_global_plan', cititruck_msgs.msg.PlanSegments),
+    # TopicConfig('move_base_node/cititruckPlannerROS/visualization_marker', visualization_msgs.msg.MarkerArray),
+    TopicConfig('move_base_node/SBPLLatticePlanner/plan', nav_msgs.msg.Path),
+    # TopicConfig(
+    #   'move_base_node/SBPLLatticePlanner/sbpl_lattice_planner_stats', sbpl_lattice_planner.msg.SBPLLatticePlannerStats
+    # ),
+    # TopicConfig('move_base_node/SBPLLatticePlanner/visualization_marker', visualization_msgs.msg.MarkerArray),
+    TopicConfig('move_base_node/current_goal', geometry_msgs.msg.PoseStamped),
+    # TopicConfig('move_base_node/global_costmap/inflated_obstacles', nav_msgs.msg.GridCells),
+    # TopicConfig('move_base_node/global_costmap/obstacles', nav_msgs.msg.GridCells),
+    # TopicConfig('move_base_node/global_costmap/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    # TopicConfig('move_base_node/global_costmap/parameter_updates', dynamic_reconfigure.msg.Config),
+    # TopicConfig('move_base_node/global_costmap/robot_footprint', geometry_msgs.msg.PolygonStamped),
+    # TopicConfig('move_base_node/global_costmap/unknown_space', nav_msgs.msg.GridCells),
+    # TopicConfig('move_base_node/global_plan', nav_msgs.msg.Path),
+    TopicConfig('move_base_node/local_costmap/inflated_obstacles', nav_msgs.msg.GridCells),
+    TopicConfig('move_base_node/local_costmap/obstacles', nav_msgs.msg.GridCells),
+    # TopicConfig('move_base_node/local_costmap/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    # TopicConfig('move_base_node/local_costmap/parameter_updates', dynamic_reconfigure.msg.Config),
+    TopicConfig('move_base_node/local_costmap/robot_footprint', geometry_msgs.msg.PolygonStamped),
+    # TopicConfig('move_base_node/local_costmap/safety_zone', geometry_msgs.msg.PolygonStamped),
+    # TopicConfig('move_base_node/local_costmap/unknown_space', nav_msgs.msg.GridCells),
+    # TopicConfig('move_base_node/cititruck_escape_recovery/visualization_marker', visualization_msgs.msg.Marker),
+    # TopicConfig('move_base_node/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    # TopicConfig('move_base_node/parameter_updates', dynamic_reconfigure.msg.Config),
+    TopicConfig('move_base_node/time_to_coll', std_msgs.msg.Float64),
+    TopicConfig('move_base_node/traffic_costmap/inflated_obstacles', nav_msgs.msg.GridCells),
+    TopicConfig('move_base_node/traffic_costmap/obstacles', nav_msgs.msg.GridCells),
+    TopicConfig('move_base_node/traffic_costmap/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    TopicConfig('move_base_node/traffic_costmap/parameter_updates', dynamic_reconfigure.msg.Config),
+    TopicConfig('move_base_node/traffic_costmap/robot_footprint', geometry_msgs.msg.PolygonStamped),
+    TopicConfig('move_base_node/traffic_costmap/unknown_space', nav_msgs.msg.GridCells),
+    TopicConfig('move_base_node/visualization_marker', visualization_msgs.msg.Marker),
+    TopicConfig('move_base_simple/visualization_marker', visualization_msgs.msg.Marker),
+    TopicConfig('odom', nav_msgs.msg.Odometry),
+    TopicConfig('odom_enc', nav_msgs.msg.Odometry),
+    # TopicConfig('one_way_map', nav_msgs.msg.OccupancyGrid),
+    # TopicConfig('param_update', std_msgs.msg.String),
+    # TopicConfig('particlevizmarker', visualization_msgs.msg.MarkerArray),
+    # TopicConfig('resource_tracker/needed_resources', cititruck_msgs.msg.ResourcesState),
+    TopicConfig('robot_mode', cititruck_msgs.msg.RobotMode),
+    TopicConfig('robot_pose', geometry_msgs.msg.Pose),
+    TopicConfig('robot_state', cititruck_msgs.msg.RobotState),
+    # TopicConfig('robot_status', cititruck_msgs.msg.RobotStatus),
+    TopicConfig('/rosout', rosgraph_msgs.msg.Log),
+    TopicConfig('/rosout_agg', rosgraph_msgs.msg.Log),
+    TopicConfig('scan', sensor_msgs.msg.LaserScan),
+    # TopicConfig('scan_filter/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    # TopicConfig('scan_filter/parameter_updates', dynamic_reconfigure.msg.Config),
+    TopicConfig('scan_filter/visualization_marker', visualization_msgs.msg.Marker),
+    # TopicConfig('session_importer_node/info', cititruckSessionImporter.msg.SessionImportInfo),
+    # TopicConfig('set_mc_PID', std_msgs.msg.Float64MultiArray),
+    TopicConfig('/tf', tf2_msgs.msg.TFMessage, dict_filter=_tf_dict_filter),
+    TopicConfig('/tf_static', tf2_msgs.msg.TFMessage, dict_filter=_tf_static_dict_filter, latch=True),
+    # TopicConfig('traffic_map', nav_msgs.msg.OccupancyGrid),
+    # TopicConfig('wifi_diagnostics', diagnostic_msgs.msg.DiagnosticArray),
+    # TopicConfig('wifi_diagnostics/cur_ap', cititruck_wifi_msgs.msg.APInfo),
+    # TopicConfig('wifi_diagnostics/roam_events', cititruck_wifi_msgs.msg.WifiRoamEvent),
+    # TopicConfig('wifi_diagnostics/wifi_ap_interface_stats', cititruck_wifi_msgs.msg.WifiInterfaceStats),
+    # TopicConfig('wifi_diagnostics/wifi_ap_rssi', cititruck_wifi_msgs.msg.APRssiStats),
+    # TopicConfig('wifi_diagnostics/wifi_ap_time_stats', cititruck_wifi_msgs.msg.APTimeStats),
+    # TopicConfig('wifi_watchdog/ping', cititruck_wifi_msgs.msg.APPingStats),
+]
+
+# topics we want to subscribe to from ROS (and publish to the cititruck)
+SUB_TOPICS = [
+    # really geometry_msgs.msg.TwistStamped:
+    TopicConfig('cmd_vel', geometry_msgs.msg.Twist, dict_filter=_cmd_vel_dict_filter),
+    TopicConfig('initialpose', geometry_msgs.msg.PoseWithCovarianceStamped),
+    TopicConfig('light_cmd', std_msgs.msg.String),
+    TopicConfig('cititruck_cmd', std_msgs.msg.String),
+    TopicConfig('move_base/cancel', actionlib_msgs.msg.GoalID),
+    # really cititruck_actions/cititruckMoveBaseActionGoal:
+    TopicConfig('move_base/goal', move_base_msgs.msg.MoveBaseActionGoal, dict_filter=_move_base_goal_dict_filter),
+]
+
+
+class PublisherWrapper(rospy.SubscribeListener):
+    def __init__(self, topic_config, robot):
+        self.topic_config = topic_config
+        self.robot = robot
+        self.connected = False
+        # Use topic_config.topic directly here. If it does not have a leading slash, it will use the private namespace.
+        self.pub = rospy.Publisher(
+            name=topic_config.topic,
+            data_class=topic_config.topic_type,
+            subscriber_listener=self,
+            latch=topic_config.latch,
+            queue_size=10,
+        )
+        rospy.loginfo(
+            "[%s] publishing topic '%s' [%s]", rospy.get_name(), topic_config.topic, topic_config.topic_type._type
+        )
+        # latched topics must be subscribed immediately
+        if topic_config.latch:
+            self.peer_subscribe("", None, None)
+
+    def peer_subscribe(self, topic_name, topic_publish, peer_publish):
+        if not self.connected:
+            self.connected = True
+            rospy.loginfo("[%s] starting to stream messages on topic '%s'", rospy.get_name(), self.topic_config.topic)
+            absolute_topic = '/' + self.topic_config.topic.lstrip('/')  # ensure exactly 1 leading slash for cititruck comm
+            self.robot.subscribe(topic=absolute_topic, callback=self.callback)
+
+    def peer_unsubscribe(self, topic_name, num_peers):
+        pass
+        # doesn't work: once ubsubscribed, robot doesn't let us subscribe again
+        # if self.connected and self.pub.get_num_connections() == 0 and not self.topic_config.latch:
+        #     self.connected = False
+        #     rospy.loginfo("[%s] stopping to stream messages on topic '%s'", rospy.get_name(), self.topic_config.topic)
+        #     absolute_topic = '/' + self.topic_config.topic.lstrip('/')  # ensure exactly 1 leading slash for cititruck comm
+        #     self.robot.unsubscribe(topic=absolute_topic)
+
+    def callback(self, msg_dict):
+        msg_dict = _prepend_tf_prefix_dict_filter(msg_dict)
+        if self.topic_config.dict_filter is not None:
+            msg_dict = self.topic_config.dict_filter(msg_dict)
+        msg = message_converter.convert_dictionary_to_ros_message(self.topic_config.topic_type._type, msg_dict)
+        self.pub.publish(msg)
+
+
+class SubscriberWrapper(object):
+    def __init__(self, topic_config, robot):
+        self.topic_config = topic_config
+        self.robot = robot
+        # Use topic_config.topic directly here. If it does not have a leading slash, it will use the private namespace.
+        self.sub = rospy.Subscriber(
+            name=topic_config.topic, data_class=topic_config.topic_type, callback=self.callback, queue_size=10
+        )
+        rospy.loginfo(
+            "[%s] subscribing to topic '%s' [%s]", rospy.get_name(), topic_config.topic, topic_config.topic_type._type
+        )
+
+    def callback(self, msg):
+        msg_dict = message_converter.convert_ros_message_to_dictionary(msg)
+        msg_dict = _remove_tf_prefix_dict_filter(msg_dict)
+        if self.topic_config.dict_filter is not None:
+            msg_dict = self.topic_config.dict_filter(msg_dict)
+        absolute_topic = '/' + self.topic_config.topic.lstrip('/')  # ensure exactly 1 leading slash for cititruck comm
+        self.robot.publish(absolute_topic, msg_dict)
+
+
+class cititruckBridge(object):
+    def __init__(self):
+        try:
+            hostname = rospy.get_param('~hostname')
+        except KeyError:
+            rospy.logfatal('[%s] parameter "hostname" is not set!', rospy.get_name())
+            sys.exit(-1)
+        port = rospy.get_param('~port', 9090)
+
+        global tf_prefix
+        tf_prefix = rospy.get_param('~tf_prefix', '').strip('/')
+
+        rospy.loginfo('[%s] trying to connect to %s:%i...', rospy.get_name(), hostname, port)
+        self.robot = rosbridge.RosbridgeSetup(hostname, port)
+
+        r = rospy.Rate(10)
+        i = 1
+        while not self.robot.is_connected():
+            if rospy.is_shutdown():
+                sys.exit(0)
+            if self.robot.is_errored():
+                rospy.logfatal('[%s] connection error to %s:%i, giving up!', rospy.get_name(), hostname, port)
+                sys.exit(-1)
+            if i % 10 == 0:
+                rospy.logwarn('[%s] still waiting for connection to %s:%i...', rospy.get_name(), hostname, port)
+            i += 1
+            r.sleep()
+
+        rospy.loginfo('[%s] ... connected.', rospy.get_name())
+
+        topics = self.get_topics()
+        published_topics = [topic_name for (topic_name, _, has_publishers, _) in topics if has_publishers]
+        subscribed_topics = [topic_name for (topic_name, _, _, has_subscribers) in topics if has_subscribers]
+
+        for pub_topic in PUB_TOPICS:
+            PublisherWrapper(pub_topic, self.robot)
+            absolute_topic = '/' + pub_topic.topic.lstrip('/')  # ensure exactly 1 leading slash for cititruck comm
+            if absolute_topic not in published_topics:
+                rospy.logwarn("[%s] topic '%s' is not published by the cititruck!", rospy.get_name(), pub_topic.topic)
+
+        for sub_topic in SUB_TOPICS:
+            SubscriberWrapper(sub_topic, self.robot)
+            absolute_topic = '/' + sub_topic.topic.lstrip('/')  # ensure exactly 1 leading slash for cititruck comm
+            if absolute_topic not in subscribed_topics:
+                rospy.logwarn("[%s] topic '%s' is not yet subscribed to by the cititruck!", rospy.get_name(), sub_topic.topic)
+
+        # At least with software version 2.8 there were issues when forwarding a simple goal to the robot
+        # This workaround converts it into an action. Check https://github.com/DFKI-NI/cititruck_robot/issues/60 for details.
+        self._move_base_client = SimpleActionClient('move_base', move_base_msgs.msg.MoveBaseAction)
+        rospy.Subscriber("move_base_simple/goal", geometry_msgs.msg.PoseStamped, self._move_base_simple_goal_callback)
+
+    def get_topics(self):
+        srv_response = self.robot.callService('/rosapi/topics', msg={})
+        topic_names = sorted(srv_response['topics'])
+        topics = []
+
+        for topic_name in topic_names:
+            srv_response = self.robot.callService("/rosapi/topic_type", msg={'topic': topic_name})
+            topic_type = srv_response['type']
+
+            srv_response = self.robot.callService("/rosapi/publishers", msg={'topic': topic_name})
+            has_publishers = True if len(srv_response['publishers']) > 0 else False
+
+            srv_response = self.robot.callService("/rosapi/subscribers", msg={'topic': topic_name})
+            has_subscribers = True if len(srv_response['subscribers']) > 0 else False
+
+            topics.append([topic_name, topic_type, has_publishers, has_subscribers])
+
+        print('Publishers:')
+        for (topic_name, topic_type, has_publishers, has_subscribers) in topics:
+            if has_publishers:
+                print((' * %s [%s]' % (topic_name, topic_type)))
+
+        print('\nSubscribers:')
+        for (topic_name, topic_type, has_publishers, has_subscribers) in topics:
+            if has_subscribers:
+                print((' * %s [%s]' % (topic_name, topic_type)))
+
+        return topics
+
+    def _move_base_simple_goal_callback(self, msg):
+        if not self._move_base_client.wait_for_server(rospy.Duration(2)):
+            rospy.logwarn("Could not connect to 'move_base' server after two seconds. Dropping goal.")
+            rospy.logwarn("Did you activate 'planner' in the cititruck web interface?")
+            return
+        goal = move_base_msgs.msg.MoveBaseGoal()
+        goal.target_pose.header = copy.deepcopy(msg.header)
+        goal.target_pose.pose = copy.deepcopy(msg.pose)
+        self._move_base_client.send_goal(goal)
+
+
+def main():
+    rospy.init_node('cititruck_bridge')
+    cititruckBridge()
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    try:
+        main()
+    except rospy.ROSInterruptException:
+        pass

Controllers/Packages/robot_gazebo_plugins/cititruck_driver/nodes/fake_cititruck_joint_publisher.py

@@ -0,0 +1,61 @@
+#!/usr/bin/env python3
+# Copyright (c) 2018-2022, Martin Günther (DFKI GmbH) and contributors
+#
+# 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 the copyright holder 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 HOLDER 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.
+#
+# Author: Martin Günther
+
+import rospy
+from sensor_msgs.msg import JointState
+
+
+def fake_cititruck_joint_publisher():
+    rospy.init_node('fake_cititruck_joint_publisher')
+    prefix = rospy.get_param('~prefix', '')
+    pub = rospy.Publisher('joint_states', JointState, queue_size=10)
+    r = rospy.Rate(50.0)
+    while not rospy.is_shutdown():
+        js = JointState()
+        js.header.stamp = rospy.Time.now()
+        js.name = [
+            prefix + 'base2steer_joint',
+            prefix + 'steer2sd_wheel_joint',
+            prefix + 'base2fixed_left_wheel_joint',
+            prefix + 'base2fixed_right_wheel_joint',
+        ]
+        js.position = [0.0 for _ in js.name]
+        js.velocity = [0.0 for _ in js.name]
+        js.effort = [0.0 for _ in js.name]
+        pub.publish(js)
+        r.sleep()
+
+
+if __name__ == '__main__':
+    try:
+        fake_cititruck_joint_publisher()
+    except rospy.ROSInterruptException:
+        pass

Controllers/Packages/robot_gazebo_plugins/cititruck_driver/nodes/rep117_filter.py

@@ -0,0 +1,76 @@
+#!/usr/bin/env python3
+
+# Copyright (c) 2018-2022, Martin Günther (DFKI GmbH) and contributors
+#
+# 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 the copyright holder 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 HOLDER 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.
+#
+# Author: Martin Günther
+
+import rospy
+from sensor_msgs.msg import LaserScan
+
+pub = None
+
+
+def callback(msg):
+    """
+    Convert laser scans to REP 117 standard.
+
+    See http://www.ros.org/reps/rep-0117.html
+    """
+    ranges_out = []
+    for dist in msg.ranges:
+        if dist < msg.range_min:
+            # assume "reading too close to measure",
+            # although it could also be "reading invalid" (nan)
+            ranges_out.append(float("-inf"))
+
+        elif dist > msg.range_max:
+            # assume "reading of no return (outside sensor range)",
+            # although it could also be "reading invalid" (nan)
+            ranges_out.append(float("inf"))
+        else:
+            ranges_out.append(dist)
+
+    msg.ranges = ranges_out
+    pub.publish(msg)
+
+
+def main():
+    global pub
+    rospy.init_node('rep117_filter')
+
+    pub = rospy.Publisher('scan_filtered', LaserScan, queue_size=10)
+    rospy.Subscriber('scan', LaserScan, callback)
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    try:
+        main()
+    except rospy.ROSInterruptException:
+        pass

Controllers/Packages/robot_gazebo_plugins/cititruck_driver/nodes/tf_remove_child_frames.py

@@ -0,0 +1,51 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+# Copyright 2016 The Cartographer Authors
+# Copyright 2018 DFKI GmbH
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#    http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import rospy
+from tf.msg import tfMessage
+
+
+def main():
+    rospy.init_node('tf_remove_child_frames')
+    remove_frames = rospy.get_param('~remove_frames', [])
+
+    # filter tf_in topic
+    tf_pub = rospy.Publisher('tf_out', tfMessage, queue_size=1)
+
+    def tf_cb(msg):
+        msg.transforms = [t for t in msg.transforms if t.child_frame_id.lstrip('/') not in remove_frames]
+        if len(msg.transforms) > 0:
+            tf_pub.publish(msg)
+
+    rospy.Subscriber('tf_in', tfMessage, tf_cb)
+
+    # filter tf_static_in topic
+    tf_static_pub = rospy.Publisher('tf_static_out', tfMessage, queue_size=1, latch=True)
+
+    def tf_static_cb(msg):
+        msg.transforms = [t for t in msg.transforms if t.child_frame_id.lstrip('/') not in remove_frames]
+        if len(msg.transforms) > 0:
+            tf_static_pub.publish(msg)
+
+    rospy.Subscriber('tf_static_in', tfMessage, tf_static_cb)
+
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    main()

Controllers/Packages/robot_gazebo_plugins/cititruck_driver/package.xml

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>cititruck_driver</name>
+  <version>1.1.7</version>
+  <description>A reverse ROS bridge for 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>
+  <license>Apache 2.0</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>
+
+  <depend>actionlib_msgs</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>dynamic_reconfigure</depend>
+  <depend>geometry_msgs</depend>
+  <depend>move_base_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>python3-websocket</depend>
+  <depend>rosgraph_msgs</depend>
+  <depend>rospy</depend>
+  <depend>rospy_message_converter</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf</depend>
+  <depend>visualization_msgs</depend>
+
+  <exec_depend>cititruck_description</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+</package>

Controllers/Packages/robot_gazebo_plugins/cititruck_driver/setup.py

@@ -0,0 +1,9 @@
+# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup(packages=['cititruck_driver'], package_dir={'': 'src'})
+
+setup(**setup_args)

Controllers/Packages/robot_gazebo_plugins/cititruck_driver/src/cititruck_driver/rosbridge.py

@@ -0,0 +1,214 @@
+# Copyright (c) 2018-2022, Martin Günther (DFKI GmbH) and contributors
+#
+# 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 the copyright holder 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 HOLDER 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 websocket
+import threading
+
+import json
+import traceback
+import time
+
+import string
+import random
+
+
+class RosbridgeSetup:
+    def __init__(self, host, port):
+        self.callbacks = {}
+        self.service_callbacks = {}
+        self.resp = None
+        self.connection = RosbridgeWSConnection(host, port)
+        self.connection.registerCallback(self.onMessageReceived)
+
+    def publish(self, topic, obj):
+        pub = {"op": "publish", "topic": topic, "msg": obj}
+        self.send(pub)
+
+    def subscribe(self, topic, callback, throttle_rate=-1):
+        if self.addCallback(topic, callback):
+            sub = {"op": "subscribe", "topic": topic}
+            if throttle_rate > 0:
+                sub['throttle_rate'] = throttle_rate
+
+            self.send(sub)
+
+    def unhook(self, callback):
+        keys_for_deletion = []
+        for key, values in self.callbacks.items():
+            for value in values:
+                if callback == value:
+                    print("Found!")
+                    values.remove(value)
+                    if len(values) == 0:
+                        keys_for_deletion.append(key)
+
+        for key in keys_for_deletion:
+            self.unsubscribe(key)
+            self.callbacks.pop(key)
+
+    def unsubscribe(self, topic):
+        unsub = {"op": "unsubscribe", "topic": topic}
+        self.send(unsub)
+
+    def callService(self, serviceName, callback=None, msg=None):
+        id = self.generate_id()
+        call = {"op": "call_service", "id": id, "service": serviceName}
+        if msg is not None:
+            call['args'] = msg
+
+        if callback is None:
+            self.resp = None
+
+            def internalCB(msg):
+                self.resp = msg
+                return None
+
+            self.addServiceCallback(id, internalCB)
+            self.send(call)
+
+            while self.resp is None:
+                time.sleep(0.01)
+
+            return self.resp
+
+        self.addServiceCallback(id, callback)
+        self.send(call)
+        return None
+
+    def send(self, obj):
+        try:
+            self.connection.sendString(json.dumps(obj))
+        except Exception:
+            traceback.print_exc()
+            raise
+
+    def generate_id(self, chars=16):
+        return ''.join(random.SystemRandom().choice(string.ascii_letters + string.digits) for _ in range(chars))
+
+    def addServiceCallback(self, id, callback):
+        self.service_callbacks[id] = callback
+
+    def addCallback(self, topic, callback):
+        if topic in self.callbacks:
+            self.callbacks[topic].append(callback)
+            return False
+
+        self.callbacks[topic] = [callback]
+        return True
+
+    def is_connected(self):
+        return self.connection.connected
+
+    def is_errored(self):
+        return self.connection.errored
+
+    def onMessageReceived(self, message):
+        try:
+            # Load the string into a JSON object
+            obj = json.loads(message)
+            # print "Received: ", obj
+
+            if 'op' in obj:
+                option = obj['op']
+                if option == "publish":  # A message from a topic we have subscribed to..
+                    topic = obj["topic"]
+                    msg = obj["msg"]
+                    if topic in self.callbacks:
+                        for callback in self.callbacks[topic]:
+                            try:
+                                callback(msg)
+                            except Exception:
+                                print("exception on callback", callback, "from", topic)
+                                traceback.print_exc()
+                                raise
+                elif option == "service_response":
+                    if "id" in obj:
+                        id = obj["id"]
+                        values = obj["values"]
+                        if id in self.service_callbacks:
+                            try:
+                                # print 'id:', id, 'func:', self.service_callbacks[id]
+                                self.service_callbacks[id](values)
+                            except Exception:
+                                print("exception on callback ID:", id)
+                                traceback.print_exc()
+                                raise
+                    else:
+                        print("Missing ID!")
+                else:
+                    print("Recieved unknown option - it was: ", option)
+            else:
+                print("No OP key!")
+        except Exception:
+            print("exception in onMessageReceived")
+            print("message", message)
+            traceback.print_exc()
+            raise
+
+
+class RosbridgeWSConnection:
+    def __init__(self, host, port):
+        self.ws = websocket.WebSocketApp(
+            ("ws://%s:%d/" % (host, port)), on_message=self.on_message, on_error=self.on_error, on_close=self.on_close
+        )
+        self.ws.on_open = self.on_open
+        self.run_thread = threading.Thread(target=self.run)
+        self.run_thread.start()
+        self.connected = False
+        self.errored = False
+        self.callbacks = []
+
+    def on_open(self):
+        print("### ROS bridge connected ###")
+        self.connected = True
+
+    def sendString(self, message):
+        if not self.connected:
+            print("Error: not connected, could not send message")
+            # TODO: throw exception
+        else:
+            self.ws.send(message)
+
+    def on_error(self, error):
+        self.errored = True
+        print("Error: %s" % error)
+
+    def on_close(self):
+        self.connected = False
+        print("### ROS bridge closed ###")
+
+    def run(self, *args):
+        self.ws.run_forever()
+
+    def on_message(self, message):
+        # Call the handlers
+        for callback in self.callbacks:
+            callback(message)
+
+    def registerCallback(self, callback):
+        self.callbacks.append(callback)

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/CHANGELOG.rst

@@ -0,0 +1,104 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package mir_gazebo
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.1.7 (2023-01-20)
+------------------
+* Don't set cmake_policy CMP0048
+* Contributors: Martin Günther
+
+1.1.6 (2022-06-02)
+------------------
+* Add arg mir_type to launch files and urdfs
+* Rename mir_100 -> mir
+  This is in preparation of mir_250 support.
+* Contributors: Martin Günther
+
+1.1.5 (2022-02-11)
+------------------
+
+1.1.4 (2021-12-10)
+------------------
+* Remove outdated comment
+* 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)
+------------------
+* mir_gazebo: Add model_name arg
+* Move joint_state_publisher to mir_gazebo_common.launch
+* Add optional namespace to launch files
+* 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
+* Contributors: Martin Günther
+
+1.1.0 (2020-06-30)
+------------------
+* Initial release into noetic
+* Contributors: Martin Günther
+
+1.0.6 (2020-06-30)
+------------------
+* Set cmake_policy CMP0048 to fix warning
+* Contributors: Martin Günther
+
+1.0.5 (2020-05-01)
+------------------
+
+1.0.4 (2019-05-06)
+------------------
+* Fix gazebo launch file
+  Before this commit, the mobile base plugin couldn't initialize, because
+  subst_value didn't work.
+* Contributors: Martin Günther
+
+1.0.3 (2019-03-04)
+------------------
+* Add hector_mapping
+* fake_localization.launch: Add frame id args
+* 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
+* 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
+* Fix translation error in odom_comb (`#12 <https://github.com/DFKI-NI/mir_robot/issues/12>`_)
+  Previously, the ekf localization only computed a correct orientation, but the translation still followed the pure odometry data. This led to strange errors where the robot would move sideways (despite only having a diff drive).
+  This PR changes the ekf configuration to not use any position information from the odometry, but to integrate the velocities, which fixes this problem.
+* Split scan_rep117 topic into two separate topics
+  This fixes the problem that the back laser scanner was ignored in the
+  navigation costmap in Gazebo (probably because in Gazebo, both laser
+  scanners have the exact same timestamp).
+* Contributors: Martin Günther, Nils Niemann
+
+1.0.2 (2018-07-30)
+------------------
+* mir_gazebo: Install config directory
+* Contributors: Martin Günther
+
+1.0.1 (2018-07-17)
+------------------
+* gazebo: Replace robot_pose_ekf with robot_localization
+  robot_pose_ekf is deprecated, and has been removed from the navigation
+  stack starting in melodic.
+* gazebo: Adjust ekf.yaml
+* gazebo: Copy robot_localization/ekf_template.yaml
+  ... for modification.
+* Contributors: Martin Günther
+
+1.0.0 (2018-07-12)
+------------------
+* Initial release
+* Contributors: Martin Günther

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.5.1)
+project(cititruck_gazebo)
+
+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
+  maps
+  sdf
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+roslaunch_add_file_check(launch)

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/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: 40
+
+# 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/robot_gazebo_plugins/cititruck_gazebo/launch/cititruck_empty_world.launch

@@ -0,0 +1,64 @@
+<?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="cititruck-01" />
+  <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)/cititruck/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" value="true" />
+        <arg name="gui" value="$(arg gui)" />
+      </include>
+    </group>
+
+    <group ns="$(arg namespace)">
+      <!-- spawn robot and bring up controllers etc. -->
+      <include file="$(find cititruck_gazebo)/launch/cititruck_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="cititruck/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" value="true" />
+        <arg name="gui" value="$(arg gui)" />
+      </include>
+    </group>
+
+    <!-- spawn robot and bring up controllers etc. -->
+    <include file="$(find cititruck_gazebo)/launch/cititruck_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/robot_gazebo_plugins/cititruck_gazebo/launch/cititruck_gazebo_common.launch

@@ -0,0 +1,71 @@
+<?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="cititruck-01" />
+  
+  <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="cititruck" doc="Name of the Gazebo robot model (needs to be different for each robot)" />
+
+
+  <!-- Load 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>
+
+  <!-- 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) " respawn="false"/>
+
+<!-- Load ros_control controller configurations -->
+  <rosparam file="$(find cititruck_description)/config/joint_state_controller.yaml" command="load" />
+  <rosparam file="$(find cititruck_description)/config/steerdrive_controller.yaml" command="load" subst_value="true" />
+
+  <!-- Start the controllers -->
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" output="screen"
+    args="joint_state_controller mobile_base_controller"/>
+
+  <!-- EKF -->
+  <include file="$(find cititruck_gazebo)/launch/includes/ekf.launch.xml">
+      <arg name="tf_prefix" value="$(arg prefix)" />
+  </include>
+
+  <!-- Add passive + mimic joints to joint_states topic -->
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" output="screen" >
+    <rosparam param="source_list">[cititruck/joint_states]</rosparam>
+    <param name="rate" value="200.0" />
+  </node>
+
+  <!-- Robot state publisher -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" />
+
+  <!-- Load teleop -->
+  <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.57079" />
+    <param name="default_vw_min" value="-1.57079" />
+  </node>
+
+  <!-- create combined scan topic (like on real cititruck) -->
+  <node pkg="topic_tools" type="relay" name="l_scan_relay" args="l_scan scan"/>
+  <node pkg="topic_tools" type="relay" name="r_scan_relay" args="r_scan scan"/>
+
+  <node name="l_rep117_laser_filter" pkg="cititruck_driver" type="rep117_filter.py" output="screen">
+    <remap from="scan" to="l_scan" />
+    <remap from="scan_filtered" to="l_scan_rep117" />
+  </node>
+
+  <node name="r_rep117_laser_filter" pkg="cititruck_driver" type="rep117_filter.py" output="screen">
+    <remap from="scan" to="r_scan" />
+    <remap from="scan_filtered" to="r_scan_rep117" />
+  </node>
+</launch>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/launch/cititruck_maze_world.launch

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="gui" default="true" />
+  <arg name="robot_type" default="cititruck-01" />
+  <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
+  <arg name="robot_x"   default="0.0" />
+  <arg name="robot_y"   default="0.0" />
+  <arg name="robot_yaw" default="0.0" />
+
+  <include file="$(find cititruck_gazebo)/launch/cititruck_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)" />
+    <arg name="robot_x"   value="$(arg robot_x)" />
+    <arg name="robot_y"   value="$(arg robot_y)" />
+    <arg name="robot_yaw" value="$(arg robot_yaw)" />
+  </include>
+
+  <include file="$(find cititruck_gazebo)/launch/includes/spawn_maze.launch.xml" />
+</launch>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/launch/fake_localization.launch

@@ -0,0 +1,17 @@
+<?xml version="1.0" ?>
+
+<launch>
+  <arg name="delta_x" default="0.0" />
+  <arg name="delta_y" default="0.0" />
+  <arg name="delta_yaw" default="0.0" />
+  <arg name="odom_frame_id" default="odom"/>
+  <arg name="base_frame_id" default="base_footprint"/>
+
+  <node name="fake_localization" pkg="fake_localization" type="fake_localization" output="screen">
+    <param name="odom_frame_id" value="$(arg odom_frame_id)"/>
+    <param name="base_frame_id" value="$(arg base_frame_id)"/>
+    <param name="delta_x"       value="$(arg delta_x)" />
+    <param name="delta_y"       value="$(arg delta_y)" />
+    <param name="delta_yaw"     value="$(arg delta_yaw)" />
+  </node>
+</launch>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/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 cititruck_gazebo)/config/ekf.yaml" subst_value="true" />
+  </node>
+</launch>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/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 cititruck_gazebo)/sdf/maze/model.sdf -model walls" output="screen" />
+</launch>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/maps_new/maze.yaml

@@ -0,0 +1,6 @@
+image: maze.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/maps_new/maze_virtual_walls.yaml

@@ -0,0 +1,6 @@
+image: maze_virtual_walls.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/maze.yaml

@@ -0,0 +1,6 @@
+image: maze.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196