增加初始化代码,实现多机启动,rviz加载地图,amcl定位待调试

mkr2018_mrc/robot_coordination/CMakeLists.txt

@@ -0,0 +1,193 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(robot_coordination)
+
+add_definitions('-std=c++11')
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED
+  COMPONENTS
+  nav_msgs
+  roscpp
+  sensor_msgs
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+add_message_files(
+  FILES
+  Waypoint.msg
+)
+
+## Generate services in the 'srv' folder
+add_service_files(
+  FILES
+  AddPath.srv
+  StartMovement.srv
+  StopMovement.srv
+)
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+ generate_messages(
+   DEPENDENCIES
+   geometry_msgs
+   #   sensor_msgs#   std_msgs
+ )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES robot
+#  CATKIN_DEPENDS nav_msgs roscpp sensor_msgs std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(robot
+#   src/${PROJECT_NAME}/robot.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(robot ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+ add_executable(robot_node src/node.cpp src/robot.cpp src/waypoint.cpp)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(robot_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+ target_link_libraries(robot_node
+   ${catkin_LIBRARIES}
+ )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS robot robot_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_robot.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

mkr2018_mrc/robot_coordination/include/robot/robot.h

@@ -0,0 +1,75 @@
+#include <list>
+
+#include <nav_msgs/Odometry.h>
+#include <ros/ros.h>
+
+#include <robot/waypoint.h>
+#include <robot_coordination/AddPath.h>
+#include <robot_coordination/StartMovement.h>
+#include <robot_coordination/StopMovement.h>
+
+/** Control a robot with a follow-the-carrot strategy
+ *
+ * Plan is loaded directly with setPath() or through a service call of type AddPath.
+ * Movement is started with a call to startExecutePath() or throught a service call of type StartMovement.
+ * Movement is stopped with a call to stopExecutePath() or throught a service call of type SoptMovement.
+ *
+ * Not thread-safe.
+ */
+class Robot {
+  public:
+
+    Robot();
+
+    /** Callback function to process odometry messages.
+     *  This function is called every time the nav_msgs/Odometry message is received on odom topic.
+     */
+    void odometryCallback(const nav_msgs::Odometry::ConstPtr& msg);
+
+    /** Service callback functions to process service request
+     */
+    bool addPathService(robot_coordination::AddPath::Request& req, robot_coordination::AddPath::Response& res);
+    bool startMovementService(robot_coordination::StartMovement::Request& req, robot_coordination::StartMovement::Response& res);
+    bool stopMovementService(robot_coordination::StopMovement::Request& req, robot_coordination::StopMovement::Response& res);
+
+    /** Set the path to follow by the robot.
+     *  This command rewrite the actual planned path of the robot with the Path class.
+     */
+    void setPath(const std::list<Waypoint>& path);
+
+    /** The robot starts to move along the planned path.
+     * The robot moves along the set of way-points using the "follow-the-carrot" control law.
+     * As soon as the motion is finished, the state of the robot is changed to Waiting.
+     */
+    void startExecutePath();
+    /** Stop the plan execution.
+     * The robot stops at the spot and the state of the robot is changed to Waiting.
+     */
+    void stopExecutePath();
+
+    /** Set the publisher created outside of the class*/
+    void setCommandPublisher(ros::Publisher* pub);
+
+    /** Create the publisher inside of the class*/
+    void createCommandPublisher(ros::NodeHandle& nh);
+
+  private:
+    /** Implementation of the follow-the-carrot control law.
+     * This function is to be called in the odometry callback function.
+     *
+     * @return true when robot finishes the plan, false otherwise.
+     * */
+    bool followTheCarrot();
+
+    ros::Publisher* command_publisher_;
+
+    ros::Time execute_start_time_;
+
+    std::list<Waypoint> plan_;
+
+    geometry_msgs::Pose robot_pose_;
+
+    double threshold_;
+
+    bool execute_;
+};

mkr2018_mrc/robot_coordination/include/robot/waypoint.h

@@ -0,0 +1,12 @@
+#include "geometry_msgs/Pose.h"
+#include "ros/ros.h"
+class Waypoint {
+  public:
+  geometry_msgs::Pose pose;
+  ros::Duration  timepoint;
+  Waypoint();
+  Waypoint (geometry_msgs::Pose, ros::Duration);
+  double distanceTo(geometry_msgs::Pose);
+  double angleTo(geometry_msgs::Pose);
+  
+};

mkr2018_mrc/robot_coordination/msg/Waypoint.msg

@@ -0,0 +1,3 @@
+geometry_msgs/Pose pose
+duration timepoint
+

mkr2018_mrc/robot_coordination/package.xml

@@ -0,0 +1,58 @@
+<?xml version="1.0"?>
+<package>
+  <name>robot_coordination</name>
+  <version>0.0.0</version>
+  <description>The MKR robot coordination assignment</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="karel@todo.todo">karel</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/robot</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <build_depend>message_generation</build_depend>
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <run_depend>message_runtime</run_depend>
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

mkr2018_mrc/robot_coordination/scripts/example_robot_control.py

@@ -0,0 +1,127 @@
+#!/usr/bin/env python
+# -*- encoding: utf-8 -*-
+# Example of script to send a trajectory to a robot, start and stop motion.
+
+import time
+
+from geometry_msgs.msg import Pose
+from robot_coordination.msg import Waypoint
+from robot_coordination.srv import AddPath
+from robot_coordination.srv import StartMovement
+from robot_coordination.srv import StopMovement
+import rospy
+
+# Trajectory as a tuple of (x, y, time)
+g_trajectory = (
+    (1.8, 6.6, 0),
+    (2.8, 6.6, 4),
+    (2.8, 7.6, 8),
+    (1.8, 7.6, 12),
+    (1.8, 6.6, 16),
+)
+
+
+def wait_for_service(server_ns, srv_name):
+    while not rospy.is_shutdown():
+        try:
+            rospy.wait_for_service(server_ns + '/' + srv_name, 5)
+            return True
+        except rospy.ROSException:
+            rospy.logwarn('Could not connect to service {}/{}, trying again'.format(
+                server_ns, srv_name))
+        except (rospy.ROSInterruptException, KeyboardInterrupt):
+            return False
+
+
+def start_movement(server_ns):
+    """Start the robot motion by calling the service 'start_movement'"""
+    srv_name = 'start_movement'
+    if not wait_for_service(server_ns, srv_name):
+        return False
+    try:
+        start_movement_srv = rospy.ServiceProxy(server_ns + '/' + srv_name, StartMovement)
+        reply = start_movement_srv()
+        return reply.ack
+    except rospy.ServiceException as e:
+        rospy.logerr('Service call failed: {}'.format(e))
+    return False
+
+
+def stop_movement(server_ns):
+    """Stop the robot motion by calling the service 'stop_movement'"""
+    srv_name = 'stop_movement'
+    if not wait_for_service(server_ns, srv_name):
+        return False
+    try:
+        stop_movement_srv = rospy.ServiceProxy(server_ns + '/' + srv_name, StopMovement)
+        reply = stop_movement_srv()
+        return reply.ack
+    except rospy.ServiceException as e:
+        rospy.logerr('Service call failed: {}'.format(e))
+    return False
+
+
+def waypoint_list_from_tuple(traj):
+    """Return a list of Waypoint instances from a list of (x, y, timepoint)"""
+    waypoint_list = []
+    for x, y, timepoint in traj:
+        waypoint = Waypoint()
+        waypoint.pose = Pose()
+        waypoint.pose.position.x = x
+        waypoint.pose.position.y = y
+        waypoint.timepoint = rospy.Duration.from_sec(timepoint)
+        waypoint_list.append(waypoint)
+    return waypoint_list
+
+
+def add_path(server_ns, trajectory):
+    """Add a path with data from trajectory"""
+    waypoint_list = waypoint_list_from_tuple(trajectory)
+    srv_name = 'add_path'
+    if not wait_for_service(server_ns, srv_name):
+        return False
+    try:
+        add_path_srv = rospy.ServiceProxy(server_ns + '/' + srv_name, AddPath)
+        reply = add_path_srv(waypoint_list)
+        return reply.result
+    except rospy.ServiceException as e:
+        rospy.logerr('Service call failed: {}'.format(e))
+    return False
+
+
+def main():
+    rospy.init_node('example_robot_control')
+
+    server_ns_parameter_name = '~server_namespace'
+    try:
+        server_ns = rospy.get_param(server_ns_parameter_name)
+    except KeyError:
+        rospy.logerr('Parameter {} must be set'.format(
+            rospy.resolve_name(server_ns_parameter_name)))
+        return
+
+    if not add_path(server_ns, g_trajectory):
+        rospy.logerr('Could not add path, exiting')
+        return
+    rospy.loginfo('Path added')
+
+    if not start_movement(server_ns):
+        rospy.logerr('Could not start motion, exiting')
+        return
+    rospy.loginfo('Movement started')
+
+    time.sleep(20)
+
+    if not stop_movement(server_ns):
+        rospy.logerr('Could not stop motion, exiting')
+        return
+    rospy.loginfo('Movement stopped')
+
+    if not add_path(server_ns, []):
+        rospy.logerr('Could not clear path, exiting')
+        return
+    rospy.loginfo('Path cleared')
+
+
+if __name__ == '__main__':
+    main()

mkr2018_mrc/robot_coordination/src/node.cpp

@@ -0,0 +1,44 @@
+#include "ros/ros.h"
+#include "robot/robot.h"
+#include <sstream>
+
+int main(int argc, char **argv)
+{
+  /*initialize ROS node*/
+  ros::init(argc, argv, "robot");
+  /*create node handle*/
+  ros::NodeHandle nh;
+  /*create an instance of a robot */
+  Robot robot;
+  /*create a plan as list of waypoints*/
+  std::list<Waypoint> path;
+  /* create waypoints on the circle with step of 1 degree, diameter of 1 meter and center on [5,4]*/
+/*  for (int i = 0 ; i < 360; i++) {
+    Waypoint p1;
+    p1.pose.position.x =5+cos(i*M_PI/180);
+    p1.pose.position.y =4+sin(i*M_PI/180);
+    p1.timepoint = ros::Duration(i*0.1+1);
+    path.push_back(p1);
+  }
+  */
+  /*assign a plan to the robot*/
+//  robot.setPath(path);
+  
+  /*set the subscriber of the odometry to the robot*/
+  ros::Subscriber odometry_subscriber = nh.subscribe("odom", 100, &Robot::odometryCallback, &robot);
+  /*create a publiser of commands*/
+  ros::Publisher cmd_pub = nh.advertise<geometry_msgs::Twist> ("cmd_vel",10);
+  /*advertise the service to add a path for a robot*/
+  ros::ServiceServer service_add_path = nh.advertiseService("add_path", &Robot::addPathService, &robot);
+  /*advertise the service to start the execution of the plan*/
+  ros::ServiceServer service_start = nh.advertiseService("start_movement", &Robot::startMovementService, &robot);
+  /*advertise the service to stop execution of the plan*/
+  ros::ServiceServer service_stop = nh.advertiseService("stop_movement", &Robot::stopMovementService, &robot);
+  /*set the publisher to the robot*/
+  robot.setCommandPublisher(&cmd_pub);
+//  robot.startExecutePath();
+  /*start processing messages*/
+  ros::spin(); 
+  return 0;
+}
+

mkr2018_mrc/robot_coordination/src/robot.cpp

@@ -0,0 +1,126 @@
+#include <cmath>  // For M_PI.
+
+#include "ros/ros.h"
+#include "tf/tf.h"
+#include "robot/robot.h"
+#include "geometry_msgs/Twist.h"
+
+
+Robot::Robot() :
+  threshold_(0.1) {
+}
+
+void Robot::odometryCallback (const nav_msgs::Odometry::ConstPtr& msg) {
+  ROS_DEBUG("got odometry");
+    robot_pose_ = msg->pose.pose;
+  if (execute_) {
+    followTheCarrot();
+  }
+}
+
+void Robot::startExecutePath() {
+  execute_start_time_ = ros::Time::now();
+  execute_ = true;
+}
+
+void Robot::stopExecutePath() {
+  geometry_msgs::Twist command;
+  command.angular.z = 0;
+  command.linear.x = 0;
+  command_publisher_->publish(command);
+  execute_ = false;
+}
+
+bool Robot::addPathService(robot_coordination::AddPath::Request& req, robot_coordination::AddPath::Response& res) {
+  ROS_INFO("service addPath called");
+  plan_.clear();
+  ROS_INFO_STREAM("Plan has " << req.waypoints.size() << " points");
+  for (int i =0; i < req.waypoints.size(); i++) {
+    plan_.push_back(Waypoint(req.waypoints[i].pose,req.waypoints[i].timepoint));
+    ROS_DEBUG_STREAM("added waypoint (" <<
+        req.waypoints[i].pose.position.x <<
+        ", " <<
+        req.waypoints[i].pose.position.y <<
+        ")");
+  }
+  res.result = true;
+
+  return true;
+}
+
+bool Robot::startMovementService(robot_coordination::StartMovement::Request& req, robot_coordination::StartMovement::Response& res) {
+   startExecutePath();
+   res.ack = true;
+   return true;
+}
+
+bool Robot::stopMovementService(robot_coordination::StopMovement::Request& req, robot_coordination::StopMovement::Response& res) {
+   stopExecutePath();
+   res.ack = true;
+   return true;
+}
+
+void Robot::setPath(const std::list<Waypoint>& path) {
+  plan_ = path;
+}
+
+void Robot::setCommandPublisher(ros::Publisher * cmdpub) {
+  command_publisher_ = cmdpub;
+}
+
+bool Robot::followTheCarrot() {
+  if (plan_.empty()) {
+    geometry_msgs::Twist command;
+    command.angular.z = 0;
+    command.linear.x = 0;
+    command_publisher_->publish(command);
+    return true;
+  } else {
+    while (!plan_.empty() && plan_.front().distanceTo(robot_pose_) < threshold_) {
+      plan_.pop_front();
+    }
+
+    Waypoint actual_goal = plan_.front();
+    const double dist = actual_goal.distanceTo(robot_pose_);
+    const double angle = actual_goal.angleTo(robot_pose_);
+    tf::Quaternion q(
+        robot_pose_.orientation.x,
+        robot_pose_.orientation.y,
+        robot_pose_.orientation.z,
+        robot_pose_.orientation.w);
+    tf::Matrix3x3 m(q);
+    double roll;
+    double pitch;
+    double yaw;
+    m.getRPY(roll, pitch, yaw);
+    ROS_INFO("distance %f and angle %f to goal", dist,angle);
+    ROS_INFO("robot roll %f , pitch %f yaw %f", roll, pitch, yaw);
+    geometry_msgs::Twist command;
+    double delta = angle - yaw;
+    if (delta > M_PI ) {
+      delta -= 2 * M_PI;
+    }
+    if (delta < -M_PI ) {
+      delta += 2 * M_PI;
+    }
+    ROS_INFO_STREAM(ros::Time::now() << " " << actual_goal.timepoint);
+    const double time_to_goal = ((execute_start_time_ + actual_goal.timepoint) - ros::Time::now()).toSec();
+
+    ROS_INFO("time to goal %f ", time_to_goal);
+    double speed;
+    if (time_to_goal > 0) {
+      speed = dist/time_to_goal ;
+    } else {
+     speed = 1;
+    }
+    command.angular.z = 2 * (sin(delta) - 0.333 * sin(3 * delta));
+    command.angular.z = delta;
+    command.linear.x = speed * (cos(delta) + 0.333 * cos(3 * delta));
+    if (command.linear.x < 0) {
+      command.linear.x = 0;
+    }
+    command_publisher_->publish(command);
+
+    return false;
+  }
+}

mkr2018_mrc/robot_coordination/src/waypoint.cpp

@@ -0,0 +1,26 @@
+#include "robot/waypoint.h"
+
+Waypoint::Waypoint() {
+};
+
+Waypoint::Waypoint(geometry_msgs::Pose p, ros::Duration d) {
+  pose = p;
+  timepoint = d;
+}
+
+/** return distance to given pose*distance to given pose*/
+double Waypoint::distanceTo(geometry_msgs::Pose p) {
+  double d_x = pose.position.x - p.position.x;
+  double d_y = pose.position.y - p.position.y;
+  double dist = sqrt(d_x*d_x + d_y*d_y);
+  return dist; 
+}
+
+/** return relative angle to the given pose*/
+double Waypoint::angleTo(geometry_msgs::Pose p) {
+  double d_x = pose.position.x - p.position.x;
+  double d_y = pose.position.y - p.position.y;
+  double angle = atan2(d_y, d_x);
+  return angle;
+
+}

mkr2018_mrc/robot_coordination/srv/AddPath.srv

@@ -0,0 +1,3 @@
+Waypoint[] waypoints
+---
+bool result

mkr2018_mrc/robot_coordination/srv/StartMovement.srv

@@ -0,0 +1,2 @@
+---
+bool ack

mkr2018_mrc/robot_coordination/srv/StopMovement.srv

@@ -0,0 +1,2 @@
+---
+bool ack

mkr2018_mrc/simulator_e130/CMakeLists.txt

@@ -0,0 +1,195 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(simulator_e130)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES simulator_e130
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/simulator_e130.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/simulator_e130_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_simulator_e130.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

mkr2018_mrc/simulator_e130/launch/gui.launch

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+
+<launch>
+
+	<include file="$(find stdr_gui)/launch/stdr_gui.launch" />
+
+</launch>

mkr2018_mrc/simulator_e130/launch/robots.launch

@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+
+<launch>
+
+	<node name="robot0_spawner" pkg="stdr_robot" type="robot_handler" args="add $(find turtlebot_stdr)/robot/turtlebot.yaml 1.8 6.6 -0.1" /> 
+	<node name="robot1_spawner" pkg="stdr_robot" type="robot_handler" args="add $(find turtlebot_stdr)/robot/turtlebot.yaml 1.4 1.0 1.0" /> 
+
+</launch>

mkr2018_mrc/simulator_e130/launch/server.launch

@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+
+<launch>
+	<include file="$(find stdr_robot)/launch/robot_manager.launch" />
+
+	<node type="stdr_server_node" pkg="stdr_server" name="stdr_server" args="$(find simulator_e130)/maps/floorplan_e130_e129.yaml"/>
+
+	<node pkg="tf" type="static_transform_publisher" name="world2map" args="0 0 0 0 0 0  world map 100" />
+
+</launch>

mkr2018_mrc/simulator_e130/launch/simulator.launch

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+
+<launch>
+
+	<include file="$(find simulator_e130)/launch/server.launch" />
+
+	<include file="$(find simulator_e130)/launch/gui.launch" />
+
+	<include file="$(find simulator_e130)/launch/robots.launch" />
+  
+    <!-- startup move_base node -->
+    <include file="$(find stdr_move_base)/launch/stdr_move_base.launch" />
+    
+    <!-- load home map -->
+    <node pkg="map_server" type="map_server" name="stdr_load_map" args="$(find simulator_e130)/maps/e129_e130.yaml">
+        <remap from="map" to="/amcl/map" />
+    </node>
+
+    <!-- startup amcl node -->
+    <include file="$(find stdr_amcl)/launch/stdr_amcl.launch" />
+
+</launch>
+

mkr2018_mrc/simulator_e130/maps/e129_e130.yaml

@@ -0,0 +1,7 @@
+image: e129_e130.pgm
+resolution: 0.050000
+origin: [-17.000000, -18.600000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

mkr2018_mrc/simulator_e130/maps/floorplan_e130_e129.yaml

@@ -0,0 +1,6 @@
+image: floorplan_e130_e129.png
+resolution: 0.01
+origin: [0.0, 0.0, 0.0]
+occupied_thresh: 0.6
+free_thresh: 0.3
+negate: 0

mkr2018_mrc/simulator_e130/package.xml

@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>simulator_e130</name>
+  <version>0.1.0</version>
+  <description>Configuration and maps to simulate turtlebots in CTU's KN:E-130</description>
+
+  <maintainer email="gael.ecorchard@cvut.cz">Gaël Écorchard</maintainer>
+
+  <license>MIT</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/simulator_e130</url> -->
+
+
+  <author email="gael.ecorchard@cvut.cz">Gaël Écorchard</author>
+
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <exec_depend>stdr_gui</exec_depend>
+  <exec_depend>stdr_robot</exec_depend>
+  <exec_depend>stdr_server</exec_depend>
+  <exec_depend>tf</exec_depend>
+  <exec_depend>turtlebot_stdr</exec_depend>
+
+
+  <export>
+
+  </export>
+</package>

mkr2018_mrc/stdr_amcl/CMakeLists.txt

@@ -0,0 +1,195 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(stdr_amcl)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES stdr_amcl
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/stdr_amcl.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/stdr_amcl_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_stdr_amcl.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

mkr2018_mrc/stdr_amcl/launch/home_map_amcl.launch

@@ -0,0 +1,70 @@
+<!--
+  Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description:
+    启动amcl节点，这样就可以在全局地图上对机器人进行定位了。
+  History:
+    20180528: initial this file.
+    20190102: 删除加载地图的launch文件,在自动导航时来启动加载地图的launch文件.
+-->
+<launch>
+  <!-- startup rviz node to show amcl process -->
+  <node name="amcl_rviz" type="rviz" pkg="rviz" args="-d $(find stdr_amcl)/rviz/stdr_amcl_config.rviz" />
+
+  <!-- Run amcl -->
+  <node pkg="amcl" type="amcl" name="stdr_amcl_node" output="screen">
+    <!-- amcl default subscribe scan topic, now remap to /robot0/laser_0 -->
+    <remap from="scan" to="/robot0/laser_0"/>
+
+    <!-- amcl default subscribe map topic, now remap to /amcl/map -->
+    <remap from="map" to="/amcl/map" />
+
+    <!-- overall filter parameters -->
+    <param name="min_particles" value="400"/>
+    <param name="max_particles" value="5000"/>
+    <param name="kld_err"       value="0.01"/>
+    <param name="kld_z"         value="0.99"/>
+    <param name="update_min_d"  value="0.05"/>
+    <param name="update_min_a"  value="0.05"/>
+    <param name="resample_interval"   value="2"/>
+    <param name="transform_tolerance" value="0.5"/>
+    <param name="recovery_alpha_slow" value="0.001"/>
+    <param name="recovery_alpha_fast" value="0.1"/>
+    <!-- set particles init pose to robot pose -->
+    <param name="initial_pose_x" value="5.0"/>
+    <param name="initial_pose_y" value="10.0"/>
+    <param name="initial_pose_a" value="-1.57"/>
+    <param name="initial_cov_xx" value="0.25"/>
+    <param name="initial_cov_yy" value="0.25"/>
+    <param name="initial_cov_aa" value="0.068538917"/> <!-- pi/12 * pi/12 -->
+    <param name="gui_publish_rate" value="-1.0"/>
+    <param name="save_pose_rate"   value="0.5"/>
+    <param name="use_map_topic"    value="false"/>
+    <param name="first_map_only"   value="false"/>
+
+    <!-- laser model parameters -->
+    <param name="laser_min_range" value="0.05"/>
+    <param name="laser_max_range" value="4.0"/>
+    <param name="laser_max_beams" value="60"/>
+    <param name="laser_z_hit"     value="0.95"/>
+    <param name="laser_z_short"   value="0.1"/>
+    <param name="laser_z_max"     value="0.05"/>
+    <param name="laser_z_rand"    value="0.05"/>
+    <param name="laser_sigma_hit"    value="0.2"/>
+    <param name="laser_lambda_short" value="0.1"/>
+    <param name="laser_likelihood_max_dist" value="2.0"/>
+    <param name="laser_model_type" value="likelihood_field"/>
+
+    <!-- Odometry model parameters -->
+    <param name="odom_model_type" value="diff"/>
+    <param name="odom_alpha1"     value="0.03"/>
+    <param name="odom_alpha2"     value="0.03"/>
+    <param name="odom_alpha3"     value="0.03"/>
+    <param name="odom_alpha4"     value="0.03"/>
+    <param name="odom_alpha5"     value="0.0"/> <!--only used if model is "omni"-->
+    <param name="odom_frame_id"   value="/map_static"/>
+    <param name="base_frame_id"   value="/robot0"/>
+    <param name="global_frame_id" value="/map"/>
+    <param name="tf_broadcast"    value="true"/>
+  </node>
+</launch>

mkr2018_mrc/stdr_amcl/launch/stdr_amcl.launch

@@ -0,0 +1,70 @@
+<!--
+  Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description:
+    启动amcl节点，这样就可以在全局地图上对机器人进行定位了。
+  History:
+    20180528: initial this file.
+    20190102: 删除加载地图的launch文件,在自动导航时来启动加载地图的launch文件.
+-->
+<launch>
+  <!-- startup rviz node to show amcl process -->
+  <node name="amcl_rviz" type="rviz" pkg="rviz" args="-d $(find stdr_amcl)/rviz/stdr_amcl_config.rviz" />
+
+  <!-- Run amcl -->
+  <node pkg="amcl" type="amcl" name="stdr_amcl_node" output="screen">
+    <!-- amcl default subscribe scan topic, now remap to /robot0/laser_0 -->
+    <remap from="scan" to="/robot0/laser_0"/>
+
+    <!-- amcl default subscribe map topic, now remap to /amcl/map -->
+    <remap from="map" to="/amcl/map" />
+
+    <!-- overall filter parameters -->
+    <param name="min_particles" value="400"/>
+    <param name="max_particles" value="5000"/>
+    <param name="kld_err"       value="0.01"/>
+    <param name="kld_z"         value="0.99"/>
+    <param name="update_min_d"  value="0.05"/>
+    <param name="update_min_a"  value="0.05"/>
+    <param name="resample_interval"   value="2"/>
+    <param name="transform_tolerance" value="0.5"/>
+    <param name="recovery_alpha_slow" value="0.001"/>
+    <param name="recovery_alpha_fast" value="0.1"/>
+    <!-- set particles init pose to robot pose -->
+    <param name="initial_pose_x" value="1.0"/>
+    <param name="initial_pose_y" value="2.0"/>
+    <param name="initial_pose_a" value="0.0"/>
+    <param name="initial_cov_xx" value="0.25"/>
+    <param name="initial_cov_yy" value="0.25"/>
+    <param name="initial_cov_aa" value="0.068538917"/> <!-- pi/12 * pi/12 -->
+    <param name="gui_publish_rate" value="-1.0"/>
+    <param name="save_pose_rate"   value="0.5"/>
+    <param name="use_map_topic"    value="false"/>
+    <param name="first_map_only"   value="false"/>
+
+    <!-- laser model parameters -->
+    <param name="laser_min_range" value="0.05"/>
+    <param name="laser_max_range" value="4.0"/>
+    <param name="laser_max_beams" value="60"/>
+    <param name="laser_z_hit"     value="0.95"/>
+    <param name="laser_z_short"   value="0.1"/>
+    <param name="laser_z_max"     value="0.05"/>
+    <param name="laser_z_rand"    value="0.05"/>
+    <param name="laser_sigma_hit"    value="0.2"/>
+    <param name="laser_lambda_short" value="0.1"/>
+    <param name="laser_likelihood_max_dist" value="2.0"/>
+    <param name="laser_model_type" value="likelihood_field"/>
+
+    <!-- Odometry model parameters -->
+    <param name="odom_model_type" value="diff"/>
+    <param name="odom_alpha1"     value="0.03"/>
+    <param name="odom_alpha2"     value="0.03"/>
+    <param name="odom_alpha3"     value="0.03"/>
+    <param name="odom_alpha4"     value="0.03"/>
+    <param name="odom_alpha5"     value="0.0"/> <!--only used if model is "omni"-->
+    <param name="odom_frame_id"   value="/map_static"/>
+    <param name="base_frame_id"   value="/robot0"/>
+    <param name="global_frame_id" value="/map"/>
+    <param name="tf_broadcast"    value="true"/>
+  </node>
+</launch>

mkr2018_mrc/stdr_amcl/package.xml

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

mkr2018_mrc/stdr_amcl/rviz/stdr_amcl_config.rviz

@@ -0,0 +1,363 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Odometry1/Covariance1
+      Splitter Ratio: 0.522222221
+    Tree Height: 359
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.400000006
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 5
+        Y: 5
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 40
+      Reference Frame: map
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        map:
+          Value: true
+        map_static:
+          Value: true
+        robot0:
+          Value: true
+        robot0_laser_0:
+          Value: false
+        robot0_sonar_0:
+          Value: false
+        robot0_sonar_1:
+          Value: false
+        robot0_sonar_2:
+          Value: false
+        robot0_sonar_3:
+          Value: false
+        robot0_sonar_4:
+          Value: false
+        world:
+          Value: false
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        world:
+          map:
+            map_static:
+              robot0:
+                robot0_laser_0:
+                  {}
+                robot0_sonar_0:
+                  {}
+                robot0_sonar_1:
+                  {}
+                robot0_sonar_2:
+                  {}
+                robot0_sonar_3:
+                  {}
+                robot0_sonar_4:
+                  {}
+      Update Interval: 0.100000001
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: x
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: true
+      Max Color: 255; 255; 255
+      Max Intensity: 8.80745792
+      Min Color: 0; 0; 0
+      Min Intensity: 2.19338179
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.100000001
+      Style: Flat Squares
+      Topic: /robot0/laser_0
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Arrow Length: 0.100000001
+      Axes Length: 0.300000012
+      Axes Radius: 0.00999999978
+      Class: rviz/PoseArray
+      Color: 139; 205; 248
+      Enabled: true
+      Head Length: 0.0700000003
+      Head Radius: 0.0299999993
+      Name: PoseArray
+      Shaft Length: 0.230000004
+      Shaft Radius: 0.00999999978
+      Shape: Arrow (Flat)
+      Topic: /particlecloud
+      Unreliable: false
+      Value: true
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: true
+      Enabled: true
+      Name: Map
+      Topic: /amcl/map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 0.600000024
+      Buffer Length: 20
+      Class: rviz/Path
+      Color: 57; 70; 255
+      Enabled: true
+      Head Diameter: 0.0299999993
+      Head Length: 0.0500000007
+      Length: 0.300000012
+      Line Style: Lines
+      Line Width: 0.0299999993
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 0; 85; 255
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.00999999978
+      Shaft Length: 0.0299999993
+      Topic: /stdr_move_base/GlobalPlanner/plan
+      Unreliable: false
+      Value: true
+    - Alpha: 0.800000012
+      Buffer Length: 20
+      Class: rviz/Path
+      Color: 250; 232; 28
+      Enabled: true
+      Head Diameter: 0.0299999993
+      Head Length: 0.0299999993
+      Length: 0.300000012
+      Line Style: Billboards
+      Line Width: 0.00499999989
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 250; 232; 28
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.00999999978
+      Shaft Length: 0.00999999978
+      Topic: /stdr_move_base/DWAPlannerROS/global_plan
+      Unreliable: false
+      Value: true
+    - Alpha: 0.5
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 255; 23; 112
+      Enabled: true
+      Head Diameter: 0.300000012
+      Head Length: 0.200000003
+      Length: 0.300000012
+      Line Style: Lines
+      Line Width: 0.0299999993
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.100000001
+      Shaft Length: 0.100000001
+      Topic: /stdr_move_base/DWAPlannerROS/local_plan
+      Unreliable: false
+      Value: true
+    - Angle Tolerance: 0.100000001
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.300000012
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 110
+      Name: Odometry
+      Position Tolerance: 0.100000001
+      Shape:
+        Alpha: 0.699999988
+        Axes Length: 1
+        Axes Radius: 0.100000001
+        Color: 206; 255; 206
+        Head Length: 0.100000001
+        Head Radius: 0.100000001
+        Shaft Length: 0.200000003
+        Shaft Radius: 0.0500000007
+        Value: Arrow
+      Topic: /robot0/odom
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 255; 24; 105
+      Enabled: true
+      Name: Polygon
+      Topic: /stdr_move_base/local_costmap/footprint
+      Unreliable: false
+      Value: true
+    - Alpha: 0.300000012
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /stdr_move_base/local_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 0.699999988
+      Axes Length: 1
+      Axes Radius: 0.100000001
+      Class: rviz/Pose
+      Color: 239; 76; 35
+      Enabled: true
+      Head Length: 0.200000003
+      Head Radius: 0.200000003
+      Name: Pose
+      Shaft Length: 0.5
+      Shaft Radius: 0.0500000007
+      Shape: Arrow
+      Topic: /move_base_simple/goal
+      Unreliable: false
+      Value: true
+    - Alpha: 0.400000006
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /stdr_move_base/global_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 20
+  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
+      Topic: /initialpose
+    - 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: 15.2291393
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 8.06713963
+        Y: 5.65849018
+        Z: 2.00396991
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 1.20480001
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.68818998
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: true
+  Height: 1056
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000022400000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000073f0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1855
+  X: 65
+  Y: 24

mkr2018_mrc/stdr_move_base/CMakeLists.txt

@@ -0,0 +1,195 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(stdr_move_base)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES stdr_move_base
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/stdr_move_base.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/stdr_move_base_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_stdr_move_base.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

mkr2018_mrc/stdr_move_base/config/costmap_common_params.yaml

@@ -0,0 +1,36 @@
+#FileName: costmap_common_params.yaml
+#Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+#Author: corvin
+#Description:
+# 代价地图通用参数配置文件,就是全局代价地图和局部代价地图
+# 共同都需要配置的参数,各参数意义如下:
+# robot_radius: 机器人的半径
+#
+#History:
+# 20180613: initial this file.
+
+robot_radius: 0.2
+
+obstacle_layer:
+  enabled: true
+  combination_method: 1
+  track_unknown_space: true
+  obstacle_range: 2.5
+  raytrace_range: 3.0
+  observation_sources: laser_scan_sensor
+  laser_scan_sensor: {
+    sensor_frame: /robot0_laser_0,
+    data_type: LaserScan,
+    topic: /robot0/laser_0,
+    marking: true,
+    clearing: true
+  }
+
+inflation_layer:
+  enabled: true
+  cost_scaling_factor: 5.0
+  inflation_radius: 0.36
+
+static_layer:
+  enabled: true
+

mkr2018_mrc/stdr_move_base/config/dwa_local_planner_params.yaml

@@ -0,0 +1,112 @@
+#FileName: dwa_local_planner_params.yaml
+#Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+#Author: corvin
+#Description:
+#  dwa_local_planner提供一个能够驱动底座的控制器,该控制器连接了路径规划器和机器人.
+#  使用地图,规划器产生从起点到目标点的运动轨迹,在移动时,规划器在机器人周围产生一个函数,
+#  用网格地图表示。控制器的工作就是利用这个函数来确定发送给机器人的速度dx, dy, dtheta
+#
+#       >> DWA算法的基本思想 <<
+#  1.在机器人控制空间离散采样(dx, dy, dtheta)
+#  2.对每一个采样的速度进行前向模拟,看看在当前状态下,使用该采样速度移动一小段时间后会发生什么.
+#  3.评价前向模拟得到的每个轨迹,是否接近障碍物,是否接近目标,是否接近全局路径以及速度等等.舍弃非法路径
+#  4.选择得分最高的路径,发送对应的速度给底座
+#
+#  DWA与Trajectory Rollout的区别主要是在机器人的控制空间采样差异.Trajectory Rollout采样点来源于整个
+#  前向模拟阶段所有可用速度集合,而DWA采样点仅仅来源于一个模拟步骤中的可用速度集合.这意味着相比之下
+#  DWA是一种更加有效算法,因为其使用了更小采样空间;然而对于低加速度的机器人来说可能Trajectory Rollout更好,
+#  因为DWA不能对常加速度做前向模拟。
+#
+#  下面来依次介绍下每个参数的意义:
+#   * acc_lim_x:x方向的加速度绝对值
+#   * acc_lim_y:y方向的加速度绝对值,该值只有全向移动的机器人才需配置.
+#   * acc_lim_th:旋转加速度的绝对值.
+#
+#   * max_trans_vel:平移速度最大值绝对值
+#   * min_trans_vel:平移速度最小值的绝对值
+#
+#   * max_vel_x:x方向最大速度的绝对值
+#   * min_vel_x:x方向最小值绝对值,如果为负值表示可以后退.
+#   * max_vel_y:y方向最大速度的绝对值.
+#   * min_vel_y:y方向最小速度的绝对值.
+#
+#   * max_rot_vel:最大旋转速度的绝对值.
+#   * min_rot_vel:最小旋转速度的绝对值.
+#   *
+#   * yaw_goal_tolerance:到达目标点时偏行角允许的误差，单位弧度.
+#   * xy_goal_tolerance:到达目标点时,在xy平面内与目标点的距离误差.
+#   * latch_xy_goal_tolerance:设置为true,如果到达容错距离内,机器人就会原地
+#        旋转,即使转动是会跑出容错距离外.
+#
+#   * sim_time:向前仿真轨迹的时间.
+#   * sim_granularity:步长,轨迹上采样点之间的距离,轨迹上点的密集程度.
+#   * vx_samples:x方向速度空间的采样点数.
+#   * vy_samples:y方向速度空间采样点数.
+#   * vth_samples:旋转方向的速度空间采样点数.
+#   * controller_frequency:发送给底盘控制移动指令的频率.
+#
+#   * path_distance_bias:定义控制器与给定路径接近程度.
+#   * goal_distance_bias:定义控制器与局部目标点的接近程度
+#   * occdist_scale:定义控制器躲避障碍物的程度.
+#   * stop_time_buffer:为防止碰撞,机器人必须提前停止的时间长度.
+#   * scaling_speed:启动机器人底盘的速度.
+#   * max_scaling_factor:最大缩放参数.
+#
+#   * publish_cost_grid:是否发布规划器在规划路径时的代价网格.如果设置为true,
+#       那么就会在~/cost_cloud话题上发布sensor_msgs/PointCloud2类型消息.
+#       每个点云代表代价网格,并且每个单独的评价函数都有一个字段及其每个单元
+#       的总代价，并考虑评分参数.
+#   
+#   * oscillation_reset_dist:机器人运动多远距离才会重置振荡标记.
+#   
+#   * prune_plan:机器人前进是是否清楚身后1m外的轨迹.
+#History:
+#  20180613: initial this file.
+#  20180727: update comment and parameters.
+
+DWAPlannerROS:
+# Robot Configuration Parameters - stdr robot
+  acc_lim_x: 0.2
+  acc_lim_y: 0.0
+  acc_lim_th: 0.3
+
+  max_trans_vel: 0.3 #choose slightly less than the base's capability
+  min_trans_vel: 0.1 #this is the min trans velocity when there is negligible rotational velocity
+
+  max_vel_x: 0.3
+  min_vel_x: -0.1
+  max_vel_y: 0.0  #diff drive robot,don't need set vel_y
+  min_vel_y: 0.0
+
+  max_rot_vel: 0.5  #choose slightly less than the base's capability
+  min_rot_vel: 0.1  #this is the min angular velocity when there is negligible translational velocity
+
+# Goal Tolerance Parameters
+  yaw_goal_tolerance: 0.1  # 0.1 rad = 5.7 degree
+  xy_goal_tolerance: 0.14
+  latch_xy_goal_tolerance: false
+
+# Forward Simulation Parameters
+  sim_time: 2.0    # 1.7
+  sim_granularity: 0.025
+  vx_samples: 6    # default 3
+  vy_samples: 1    # diff drive robot, there is only one sample
+  vth_samples: 20  # 20
+  controller_frequency: 5.0
+
+# Trajectory Scoring Parameters
+  path_distance_bias: 52.0  # 32.0  -weighting for how much it should stick to the global path plan
+  goal_distance_bias: 12.0  # 24.0  -wighting for how much it should attempt to reach its goal
+  occdist_scale: 0.3        # 0.01  -weighting for how much the controller should avoid obstacles
+  forward_point_distance: 0.325 # 0.325 -how far along to place an additional scoring point
+  stop_time_buffer: 0.2     # 0.2   -amount of time a robot must stop in before colliding for a valid traj.
+  scaling_speed: 0.20       # 0.25  -absolute velocity at which to start scaling the robot's footprint
+  max_scaling_factor: 0.2   # 0.2   -how much to scale the robot's footprint when at speed.
+  publish_cost_grid: false
+
+# Oscillation Prevention Parameters
+  oscillation_reset_dist: 0.05  # default 0.05
+
+# Global Plan Parameters
+  prune_plan: false
+

mkr2018_mrc/stdr_move_base/config/global_costmap_params.yaml

@@ -0,0 +1,22 @@
+#FileName: global_costmap_params.yaml
+#Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+#Author: corvin
+#Description:
+#  全局代价地图参数配置文件,各参数的意义如下：
+#  global_frame:在全局代价地图中的全局坐标系;
+#  robot_base_frame:机器人的基坐标系;
+#
+#History:
+#  20180613: initial this file.
+global_costmap:
+  global_frame: /map
+  robot_base_frame: /robot0
+  update_frequency: 0.5
+  static_map: true
+  rolling_window: false
+  transform_tolerance: 1.0
+  plugins:
+    - {name: static_layer,    type: "costmap_2d::StaticLayer"}
+    - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
+    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+

mkr2018_mrc/stdr_move_base/config/global_planner_params.yaml

@@ -0,0 +1,50 @@
+#FileName: global_planner_params.yaml
+#Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+#Author: corvin
+#Description:
+#  这个包为导航实现了一种快速,内插值的全局路径规划器,
+#  继承了nav_core包中nav_core::BaseGlobalPlanner接口,
+#  该实现相比navfn使用更加灵活.更多内容可以查看网址:
+#  http://wiki.ros.org/global_planner,下面介绍各参数意义:
+#   * allow_unknown:是否允许规划器规划穿过未知区域的路径,
+#        只设计该参数为true还不行,还要在costmap_commons_params.yaml
+#        中设置track_unknown_space参数也为true才行.
+#   * default_tolerance:当设置的目的地被障碍物占据时,需要以该参数
+#        为半径寻找到最近的点作为新目的地点.
+#   * visualize_potential:是否显示从PointCloud2计算得到的势区域.
+#   * use_dijkstra:设置为true,将使用dijkstra算法,否则使用A*算法.
+#   * use_quadratic:设置为true,将使用二次函数近似函数,否则使用更加
+#      简单的计算方式,这样节省硬件计算资源.
+#   * use_grid_path:如果设置为true,则会规划一条沿着网格边界的路径,
+#      偏向于直线穿越网格,否则将使用梯度下降算法,路径更为光滑点.
+#   * old_navfn_behavior:若在某些情况下,想让global_planner完全复制navfn
+#      的功能,那就设置为true,但是需要注意navfn是非常旧的ROS系统中使用的,
+#      现在已经都用global_planner代替navfn了,所以不建议设置为true.
+#   * lethal_cost:致命代价值,默认是设置为253,可以动态来配置该参数.
+#   * neutral_cost:中等代价值,默认设置是50,可以动态配置该参数.
+#   * cost_factor:代价地图与每个代价值相乘的因子.
+#   * publish_potential:是否发布costmap的势函数.
+#   * orientation_mode: 如何设置每个点的方向（None = 0,Forward = 1,
+#       Interpolate = 2,ForwardThenInterpolate = 3,Backward = 4,
+#       Leftward = 5,Rightward = 6）（可动态重新配置）
+#   * orientation_window_size:根据orientation_mode指定的位置积分来得到
+#      使用窗口的方向.默认值1,可以动态重新配置.
+#History:
+#  20180727: initial this comment.
+#
+GlobalPlanner:
+  allow_unknown: false
+  default_tolerance: 0.2
+  visualize_potential: false
+  use_dijkstra: true
+  use_quadratic: true
+  use_grid_path: false
+  old_navfn_behavior: false
+
+  lethal_cost: 253
+  neutral_cost: 50
+  cost_factor: 3.0
+  publish_potential: true
+  orientation_mode: 0
+  orientation_window_size: 1
+

mkr2018_mrc/stdr_move_base/config/local_costmap_params.yaml

@@ -0,0 +1,26 @@
+#FileName: local_costmap_params.yaml
+#Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+#Author: corvin
+#Description:
+#  本地代价地图需要配置的参数，各参数意义如下:
+#  global_frame:在本地代价地图中的全局坐标系;
+#  robot_base_frame:机器人本体的基坐标系;
+#
+#History:
+#  20180613: initial this file.
+
+local_costmap:
+  global_frame: /map_static
+  robot_base_frame: /robot0
+  update_frequency: 5.0
+  publish_frequency: 3.0
+  static_map: false
+  rolling_window: true
+  width: 4.0
+  height: 4.0
+  resolution: 0.05
+  transform_tolerance: 0.5
+  plugins:
+    - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
+    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+

mkr2018_mrc/stdr_move_base/config/move_base_params.yaml

@@ -0,0 +1,32 @@
+#FileName: move_base_params.yaml
+#Copyright: 2016-2018 ROS小课堂www.corvin.cn
+#Author: corvin
+#Description:
+# move_base软件包的通用配置参数,现在依次解释每个参数意义:
+#   shutdown_costmaps:当move_base在不活动状态时,是否关掉costmap.
+#   controller_frequency:向底盘控制移动话题cmd_vel发送命令的频率.
+#   controller_patience:在空间清理操作执行前,控制器花多长时间等有效控制下发.
+#   planner_frequency:全局规划操作的执行频率.如果设置为0.0,则全局规划器仅
+#       在接收到新的目标点或者局部规划器报告路径堵塞时才会重新执行规划操作.
+#   planner_patience:在空间清理操作执行前,留给规划器多长时间来找出一条有效规划.
+#   oscillation_timeout:执行修复机制前,允许振荡的时长.
+#   oscillation_distance:来回运动在多大距离以上不会被认为是振荡.
+#   base_local_planner:指定用于move_base的局部规划器名称.
+#   base_global_planner:指定用于move_base的全局规划器插件名称.
+#History:
+# 20180726: initial this comment.
+#
+shutdown_costmaps: false
+
+controller_frequency: 5.0
+controller_patience: 3.0
+
+planner_frequency: 1.4
+planner_patience: 5.0
+
+oscillation_timeout: 8.0
+oscillation_distance: 0.3
+
+base_local_planner: "dwa_local_planner/DWAPlannerROS"
+base_global_planner: "global_planner/GlobalPlanner"
+

mkr2018_mrc/stdr_move_base/launch/stdr_move_base.launch

@@ -0,0 +1,45 @@
+<!--
+  FileName: stdr_move_base.launch
+  Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description:
+    启动move_base节点，加载各个配置文件。
+  History:
+    20180528: initial this file.
+    20180530: add arg param to rename all kinds of frames,topics,add load params file.
+-->
+<launch>
+  <arg name="odom_frame_id"   default="/map_static"/>
+  <arg name="base_frame_id"   default="/robot0"/>
+  <arg name="global_frame_id" default="/map"/>
+
+  <arg name="odom_topic"      default="/robot0/odom"/>
+  <arg name="cmd_vel_topic"   default="/robot0/cmd_vel"/>
+  <arg name="map_topic"       default="/amcl/map"/>
+
+  <node pkg="move_base" type="move_base" name="stdr_move_base" output="screen">
+    <rosparam file="$(find stdr_move_base)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find stdr_move_base)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find stdr_move_base)/config/local_costmap_params.yaml"  command="load" />
+    <rosparam file="$(find stdr_move_base)/config/global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find stdr_move_base)/config/move_base_params.yaml"            command="load" />
+    <rosparam file="$(find stdr_move_base)/config/global_planner_params.yaml"       command="load" />
+    <rosparam file="$(find stdr_move_base)/config/dwa_local_planner_params.yaml"    command="load" />
+
+   <param name="global_costmap/global_frame"     value="$(arg global_frame_id)"/>
+   <param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
+   <param name="local_costmap/global_frame"      value="$(arg odom_frame_id)"/>
+   <param name="local_costmap/robot_base_frame"  value="$(arg base_frame_id)"/>
+   <param name="DWAPlannerROS/global_frame_id"   value="$(arg odom_frame_id)"/>
+
+    <!-- move base default publish cmd to /cmd_vel topic,now remap to /robot0/cmd_vel -->
+    <remap from="/cmd_vel" to="$(arg cmd_vel_topic)"/>
+
+    <!-- move_base default subscribe odom topic,now remap to /robot0/odom -->
+    <remap from="/odom" to="$(arg odom_topic)"/>
+
+    <!-- move_base default subscribe map topic,now remap to /amcl/map -->
+    <remap from="/map" to="$(arg map_topic)"/>
+  </node>
+</launch>
+

mkr2018_mrc/stdr_move_base/package.xml

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