Cleaning Robot development note - memo

于 2017-05-10 10:40:14 发布
阅读量2.1k 收藏 2
点赞数
分类专栏： ROS
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/lightrain0/article/details/71515236
版权

A type of cleaning robot:

  • Requirement

Realize the functionality of cleaning room by covering all free spaces and reach the level of cleaning grade of XiaoMi Robot.

    • Record
      • Two tf listener methods
        1.  lookupTransforms(A,B, ,transform) - Get transform from B to A;
        2. Transformpoint("framename",Stamppoint_A,Stampedpoint_B) - Transform A in the "framename" frame to B ;
      • TF check methods
      • Fake_localization
        • cmd_vel -> fake_localization ->odometry & tf
      • Get all points on the global planner path
        • published topic : /plan
        • message type: nav_msgs/Path -> std_msgs/header  & geometry_msgs/posestamped[] 
      • Rosbag : Stored the messages from laser scan and other topics published from sensors.
        • Setup a new map through rosbag
          1. We can download a bag and rosrun gmapping. Then running 'rosbag play' to publish all messages from bag to gmapping to setup a new map. After a setup of map , we also could use ' 
            rosrun map_server map_saver -f mymap ' to save map as a pgm file.
          2. We can operate robot with joystick when publishing all message from laser scan , then we run 'rosbag record' to save a bag which can be used to setup a new map through step 1.
      • Footprint:
        • There are two places where we can set the footprint of robot , one is located in the configuration file of teb_local_planner represented as 'footprint_model' which is utilized in the processing of planning; another is located in the file of 'costmap_common_params' showing in the section of 'footprint layer' which is used to check the passing feasibility when avoiding obstacles.
      • GUI parameters configuration dynamically
        • rosrun rqt_reconfigure rqt_reconfigure 
      • Teb_local_planner
        • Using 'via-point' to get close to the global planner path with minimized distance to viapoint.
          • params
            1. weight_viapoint : Optimization weight for minimzing the distance to via-points.
            2. viapoint_all_candidates : true-all candidate trajectories to minimize distance; false- just the trajectory belongs to the topology of global plan is optimized.
            3. global_plan_via_point_sep: positive value enable via-points with minimum sepration between two consecutive via-points along the global plan(in meters)
            4. selection_viapoint_cost_scale: Extra scaling of via-point cost terms just for selecting the 'best' candidate.
          • test_optim_node
            1.   We can download the source code of teb_local_planner on thewebsite . 
            2.  After catkin_make this package, we run with command 'rosrun teb_local_planner test_optim_node.launch'
            3.  Now we can modify the code of test_optim_node.cpp to test the result if we modify the parameter for optimization like adding viapoints on the plan path by clicking point in the rviz.
          • Questions
            1.  This planner need install an extra package called 'ros_indigo_teb_local_planner' compared to the DWA and Trajectory planner which have already installed in the ROS .
            2. If you want to plan a straight line from start to goal ,you could set the orientation same both in the start and goal position . After that ,sending goal or making a plan.
        • Costmap_conversion to convert occupied costmap_2d cells to geometry primitives , which could reduce the calculation for optimization usingcostmap_converter plugin .
          • topics related to obstacles
            1.  subscribed topics:~<name>/obstacles(teb_local_planner/ObstacleMsg) . It provides custom obstacles as point , line or polygons.
            2. published topics:~<name>/teb_feedback(teb_local_planner/FeedbackMsg) including velocity profile and temporal information as well as the obstacle list. Parameter~<name>/publish_feedback must be enabled firstly.  We can use this topic to get obstacles information from teb_local_planner.
      • Occupancy value of grid (two methods)
        • From gmapping( could judge the unknown area with value of -1)
          • static : topic of '/map'(nav_msgs/OccupancyGrid)
          • dynamic:  topic of '/dynamic_map'(nav_msgs/GetMap)
        • From move_base ( could judge the inflation area with value of 99)
          • static: topic of '/move_base/global_costmap/costmap'(nav_msgs/OccupancyGrid).
          • dynamic: topic of '/move_base/global_costmap/costmap_update'(nav_msgs/OccupancyGridUpdate) where the members of message includes x,y as original position not robot base position which is (x + width/2,y+height/2)
      • DWA_local_planner
        • Two parameters that can influence the distance to the global plan path.
          1. path_distance_bias : The bigger this value  the closer to the global path.
          2. goal_distance_bias: The bigger this value the shorter the path to the goal.
        • These two parameter adjustant could cause robot constant rotation in the recovery mode.
      • Simulator for navigation
        • heatmap (download with git clone https://github.com/eybee/heatmap)
          The heatmap package allows you to create a WIFI-heatmap.The heatmap package allows the user to select a previously recorded map (e.g. by gmapping) and define a measurement area on it. Then acoverage path gets planned and the robot takes Wifi signal strength samples while covering the area. These samples are visualized in RViz. On its way the robot uses the navigation_stack to avoid ostacles in the measuring area. When robot is done the package offers the possibility to do an interpolation to get a continuous heatmap from the scattered samples.
          This package is to measure the WIFI signal quality while covering the area. But we here can get a fast way to get the coverage algorithm to produce a nav_msgs/path message for a specific area. And the relative files are list below.
          • path_planner.py
            This file is to run alone to produce the waypoints on the path which can cover the area by accepting the area points by topic with polygonstamped message and publish the nav_msgs/path message when the polygon of area is created.
          • coverage.py
            This file provides the functionality of obtaining the waypoints by passing the polygon of area.
          • maptools.py
            This file provides functions for map and shape transformation.
          •  Usage
            1. roslaunch heatmap heatmap_sim.launch
            2. Click on the map to set point to create a polygon using the 'Publish point' button.
            3. Make sure the points you clicks can make a close loop, then the waypoints path will be produced. Robot will track the path with base_local_planner .
          • Notes
            1.  The polygon message is published in the heatmap_client.cpp when user clicks the last point which can connect the first point to create a close loop.
            2.  In the path_planner.py , the main loop runs in the '_init_' function and waits for the polygon message once the message is subscribed successfully with filed_callback function then start to plan the path using 'plan_path' function while the path will be also published as each of the waypoints on the path is reached after sending goal of position to move_base server in the function of 'step_path_following'.
            3. This package can be used to simulate the algorithm for navigation , but it seems not to provide the method to escape from obstacle so it always block when encountering the obstacle.

        • sim_platform
          This package use gazebo as a simulator to setup an environment where the robot can detect the obstacles by laser scan that emits in the range of 270 degree and meanwhile publishing the map message for rviz. There are three launch files to run the whole simulator , listed below
          • environment.launch
            run gazebo_ros/launch/empty_world.launch with a world file.
          • run_simulator.launch
            run the gazebo_ros node and other nodelets for kobuki and run node for turtlebot_teleop_key operation
          • gmapping_sim.launch
            run the gmapping.launch for publishing map message
          • Notes
            1. The /Odom topic that gazebo published is generated along with random noise to simulate the real environment. When using this simulation to observe the path produced by teb_local_planner especially in straight line , if robot moves along to this path that is straight in the beginning of planning from teb_local_planner, robot will not always accurately follow along the initial straight path because the /Odom is not accurate so that the position of robot can not be addressed rigidly on the initial straight path.
      • Geometry and motion planning library
      • SLAM

        • SceneLib

          SceneLib is an open-source C++ library for SLAM designed and implemented byAndrew Davison with additional work byPaul Smith. Importantly, SceneLib depends in part on the open source VW34 library for computer vision developed at Oxford'sActive Vision Lab under the leadership ofDavid Murray andIan Reid, who have also had a large role in guiding the development of SceneLib.
          SceneLib is a generic SLAM library in principle, with a modular approach to specification of the details of robot and sensor types. However it also has specialised components to permit real-time vision-based SLAM witha single camera (MonoSLAM) and the design is optimised towards this type of application.
        • MRPT
          Mobile Robot Programming Toolkit provides developers with portable and well-tested applications andlibraries covering data structures and algorithms employed in common robotics research areas. It isopen source, released under theBSD license.

          The Mobile Robot Programming Toolkit (MRPT) is an extensive, cross-platform, and open source C++ library aimed to help robotics researchers to design and implement algorithms (mainly) in the fields of Simultaneous Localization and Mapping (SLAM),computer vision, and motion planning (obstacle avoidance). The priorities are efficiency and reusability of code.

          The libraries include classes for easily managing 3D(6D) geometry, probability density functions (pdfs) over many predefined variables (points and poses, landmarks, maps), Bayesian inference (Kalman filters, particle filters), image processing, path planning and obstacle avoidance, 3D visualization of all kind of maps (points, occupancy grids, landmarks,...), etc.
      • asr_ftc_local_planner
        • This package provides an implementation of the "Follow the Carrot" algorithm to local robot navigation on a flat plane. Given a global plan to follow and a costmap, the local planner produces velocity commands to send to a mobile base. The parameters for this planner are also dynamically reconfigurable. This package implements the asr_nav_core interface for a local planner. For this to work the standardmove_base andnav_core must be adapted (look at kapitel 3.1 Needed Packages).

        • The planner is divided into three phases after setting a new goal:

          1. Rotate on the spot to the global plan orientation.

          2. Drive to the goal.

          3. Rotate on the spot to the goal orientation.
          ftc_local_planner.jpg

          The planner goes along the global plan until the maximal distance or angle is reached. With this maximal distance or angle calculates the planner the velocity of the robot to follow the global plan. For example in the first image row you see a calculation of the velocity if the sim_time = 1s and max_x_vel = 2 m/s. So de robot could drive 2 meters in the given sim_time. Thereforce we go 2 meters along the global plan. Now we check the angle of the reached point and the robot orientation. If it is lower than simt_time * max_rotation_vel we calculate the two velocity how you seen under the pictures row.

          In the second picture row, we also go 2 meters along the global plan. Now we check the angle. It is higher than sim_time * max_rotation_vel. So we go backward the global plan until the max reachable angle is reached.

        • Benefits

          • fast driving: The robot tries to reach the maximal velocity as fast as possible.
          • few parameters: Only 11 parameters must be set.
          • few jerks: the robot tries to drive constantly with the maximal velocity.
          • for small rooms: the robot drives closely to the global plan, no loops (like the dwa_local_planner). So it can drive without collision in small rooms.
          • Drive around obstacles with the global plan: guaranteed to find a way around the obstacle.
          • short processing time.

        • Limitations

          • Only for circular robots.
          • Only differential driven robots.
          • Drives only forward (not backwards).

        • Tutorials

          http://wiki.ros.org/asr_ftc_local_planner/SetupNavigationForFTCPlanner

          http://wiki.ros.org/asr_ftc_local_planner/FindRightParameters
        • Notes
          • This ftc local planner is designed to drive the robot along the global plan , two main functions are components of completing this task.  The first function of two is
            rotateToOrientation
            ,which rotates robot to the same orientation as the global plan .
            The second function of two is 
                  driveToward
                                  which produces cmd_vel to drive robot along the global plan to the goal
          •  This planner could provide a solution along the global plan by calculating the target position on the global path according to max velocity and sim time ,but there is one problem that could not make robot move consecutively that means the robot always stop to rotate when off-tracking the global plan because of its first function to ensure robot orientation same as target position while the cmd_vel.linear value is zero when rotating. We can adjust the parameters to solve this problem as possible as we can . But for our application of aiming to move the robot along a straight line , we could modify the source code to reduce the rotation count and better rotate on the meanwhile moving forward.
                        
        • QA
          • pure virtual method called
            terminate called without an active exception
          • in move_base.cpp file , line with 572
                   bool  gotPlan = n. ok () &&  makePlan (temp_goal, *planner_plan_);
            //line 474 in the function of Move_base::makePlan
            if (!planner_-> makePlan (start, goal, plan) || plan. empty ()){
                   ROS_DEBUG_NAMED ( "move_base" , "Failed to find a  plan to point (%.2f, %.2f)" , goal. pose . position . x , goal. pose . position . y );
                   return   false ;
                }
      • dynamic_reconfigure
      • Nodelet
        Nodelets are designed to provide a way to run multiple algorithms on a single machine, in a single process, without incurring copy costs when passing messages intraprocess. roscpp has optimizations to do zero copy pointer passing between publish and subscribe calls within the same node. To do this nodelets allow dynamic loading of classes into the same node, however they provide simple separate namespaces such that the nodelet acts like a seperate node, despite being in the same process. This has been extended further in that it is dynamically loadable at runtime using        pluginlib.

        • ROS中nodelet的使用

        • Notes
          1.  Create a .cpp file where a class inherited from nodelet::Nodelet should be declared and onInit() function is called when loading this nodelet , so your codes should at first be added in this function and must not be blocked . Dont forget add a line to register this nodelet as a plugin ,like this :
            PLUGINLIB_DECLARE_CLASS ( pkg, class_name, class_type, base_class_type )

            PLUGINLIB_DECLARE_CLASS(nodelet_ns, Plus, nodelet_ns::Plus, nodelet::Nodelet);
          2.  In Cmakelist.txt , using add_library and target_link_libraies is to generate a libxxx.so file in the directory catkin_libraries represents,like this:
            1. add_library(nodelet_test_lib plus.cpp)  
            2. target_link_libraries(nodelet_test_lib ${catkin_LIBRARIES}) 

          3. Create a xml file to describe the library  this plugin is in  and specify the class name (better with pkg/type format as the nodelet name when loading ) and class type information which is used for package.xml to export.
            1. <library path="lib/libnodelet_test_lib">  
            2.   <class name="nodelet_ns/Plus" type="nodelet_ns::Plus" base_class_type="nodelet::Nodelet">  
            3.     <description>   
            4.       A node to add a value and republish.  
            5.     </description>  
            6.   </class>  
            7. </library> 

          4. In Package.xml , export this plugin ,like this:
            1. <export>  
            2.     <nodelet plugin="${prefix}/nodelet_test_plugin.xml"/>  
            3.   </export> 
          5.  Lastly , loading this nodelet in a launch file , we can load the nodelet in a nodelet manager or in a standalone way like this :
             
            nodelet load pkg/Type manager - Launch a nodelet of type pkg/Type on manager manager //向manager中loader nodelet
nodelet standalone pkg/Type   - Launch a nodelet of type pkg/Type in a standalone node //程序复用，相当启动一个普通node
nodelet unload name manager   - Unload a nodelet a nodelet by name from manager //从manager移除nodelet
nodelet manager               - Launch a nodelet manager node  //创建mananger

            #standalone
            <node pkg="nodelet" type="nodelet" name="test6" args="standalone nodelet_ns/Plus " output="screen"/> 
            # in the manager
            1. <node pkg="nodelet" type="nodelet" name="manager_1"  args="manager" output="screen"/>  
            2.   
            3.   <node pkg="nodelet" type="nodelet" name="test1" args="load nodelet_ns/Plus manager_1" output="screen"/>

      • Service

        Python on ROS - (1) - Service & Client

        ROS的学习（十六）用C++写一个简单的服务器(service)和客户端(client)

        • Notes(C ++)
          1. Client.
             Use NodeHandle::serviceClient to create a ServiceClient object and use call function to get response by passing the service structure contains the request.
             
            1. ros::NodeHandle n;  
            2.   ros::ServiceClient client = n.serviceClient<beginner_tutorials::AddTwoInts>("add_two_ints");  
            3.   beginner_tutorials::AddTwoInts srv;  
            4.   srv.request.a = atoll(argv[1]);  
            5.   srv.request.b = atoll(argv[2]);  
            6.   if (client.call(srv))  
            7.   {  
            8.     ROS_INFO("Sum: %ld", (long int)srv.response.sum);  
            9.   }  
            10.   else  
            11.   {  
            12.     ROS_ERROR("Failed to call service add_two_ints");  
            13.     return 1;  
            14.   } 
          2. Server.
            Use NodeHandle::advertiseService to create a ServiceServerobject with a callback function to process the request and return response
            ros::ServiceServer service = n.advertiseService("add_two_ints", add);
            1. bool add(beginner_tutorials::AddTwoInts::Request  &req,  
            2.          beginner_tutorials::AddTwoInts::Response &res)  
            3. {  
            4.   res.sum = req.a + req.b;  
            5.   ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);  
            6.   ROS_INFO("sending back response: [%ld]", (long int)res.sum);  
            7.   return true;  
            8. } 
      • Pluginlib

        Writing A Global Path Planner As Plugin in ROS


        • Create a custom global planner (linear_global_planner)
          • Create a cpp file and a .h file to create a class inherited from nav_core:BaseGlobalPlanner, and register it as a base global planner through the macro PLUGINLIB_EXPORT_CLASS
            // in .h file
            class               LinearGlobalPlanner  :  public   nav_core ::BaseGlobalPlanner{
            //..... two virtual functions should be overrided
                     virtual   bool   makePlan ( const  geometry_msgs::PoseStamped& start, 
                         const  geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);

                     /**
                    * @brief  Initialization function for the LinearGlobalPlanner
                    * @param  name The name of this planner
                    * @param  costmap_ros A pointer to the ROS wrapper of the costmap to use for planning
                    */
                     virtual   void   initialize (std::string name, costmap_2d::Costmap2DROS* costmap_ros);
            }
            //in .cpp file
            PLUGINLIB_EXPORT_CLASS (linear_global_planner::LinearGlobalPlanner,nav_core::BaseGlobalPlanner)

          • Configure the CMakelist.txt file , the following lines are added at first.
            add_library (linear_global_planner src/linearGlobalPlanner.cpp)
            target_link_libraries (linear_global_planner
                 ${catkin_LIBRARIES}
                )
            If you use any other library or header file included in your files , then note that add corresponding package name to thefind_package andcatkin_package tags. In this example , I included tf, base_local_planner and costmap_2d , so I have to add package name related . I think the pluginlib , nav_core and roscpp should be added for a base global planner plugin application. Partial codes like this
            find_package (catkin REQUIRED
                    COMPONENTS
                        roscpp
                        tf
                        nav_core
                        costmap_2d
                        base_local_planner
                        pluginlib
                    )
            catkin_package(
                INCLUDE_DIRS include
                LIBRARIES carrot_planner
                CATKIN_DEPENDS
                    roscpp
                    pluginlib
                    costmap_2d
                    base_local_planner
                    nav_core
            )

          • After completing above steps, we already registered a plugin as a base global planner ,but it can not be accessed by other programs so we need export the library so that it can be found by any program . Create a plugin.xml file to configure this class to the library we export , and specify this plugin.xml under the tag <export> in thepackage.xmlfile to export this library so that other programs could find and use this library as long as using the name of library and also the class in this library could be accessed.
            # in plugin.xml
            <             library   path = "lib/liblinear_global_planner" >
          •    < class   name = "linear_global_planner/LinearGlobalPlanner"   type = "linear_global_planner::LinearGlobalPlanner"   base_class_type = "nav_core::BaseGlobalPlanner" >
                 < description >
                  A simple planner that makes the linear path from start to goal 
                 </ description >
               </ class >
            </ library >
            # in package.xml
                          < export >
                            <!-- Other tools can request additional information be placed here -->         < nav_core   plugin = "${prefix}/plugins/linear_global_planner_plugin.xml"   />
            </             export >
          • If catkin_make no error, Now we can configure linear_global_planner as a base_global_planner instead of other global planners like carrot_planner


        • Q&A
          • Q:When catkin_make , we got this error:
                            from /home/evan/workspace/roboware/cleaner_catkin_ws/src/linear_global_planner/src/linearGlobalPlanner.cpp:2:
          • /opt/ros/indigo/include/costmap_2d/observation.h:36:29: fatal error: pcl/point_types.h: No such file or directory
             #include <pcl/point_types.h>
            Answer:
            Add the lines marked in red color . It is because the pcl is located in the system directory (usr/include/pcl-1.7) , and the package that used this pcl did not be added in the tag catkin_package and find_package.
            catkin_package(
               INCLUDE_DIRS include
              LIBRARIES linear_global_planner
              CATKIN_DEPENDS
              roscpp
              pluginlib
              costmap_2d
              base_local_planner
              nav_core
            #  CATKIN_DEPENDS other_catkin_pkg
            #  DEPENDS system_lib
            )
            include_directories (
              include
               ${catkin_INCLUDE_DIRS}
            )

          • Q:
            CMake Error at /opt/ros/indigo/share/catkin/cmake/catkin_package.cmake:191 (message):
              catkin_package() the catkin package 'roscpp' has been find_package()-ed but
              is not listed as a build dependency in the package.xml
          • Answer 
      • Coordinate Frames for Mobile Platforms

        odom

        The coordinate frame called odom is a world-fixed frame. The poseof a mobile platform in theodom frame can drift over time,without any bounds. This drift makes theodom frame useless as along-term global reference. However, the pose of a robot in theodom frame is guaranteed to be continuous, meaning that the poseof a mobile platform in theodom frame always evolves in a smoothway, without discrete jumps.

        In a typical setup the odom frame is computed based on an odometrysource, such as wheel odometry, visual odometry or an inertialmeasurement unit.

        The odom frame is useful as an accurate, short-term localreference, but drift makes it a poor frame for long-term reference.

        map

        The coordinate frame called map is a world fixed frame, with itsZ-axis pointing upwards. The pose of a mobile platform, relative tothemap frame, should not significantly drift over time. Themap frame is not continuous, meaning the pose of a mobile platformin themap frame can change in discrete jumps at any time.

        In a typical setup, a localization component constantly re-computesthe robot pose in themap frame based on sensor observations,therefore eliminating drift, but causing discrete jumps when newsensor information arrives.

        The map frame is useful as a long-term global reference, butdiscrete jumps in position estimators make it a poor reference frame for local sensing andacting.

        Relationship between Frames

        The map frame is the parent of odom, and odom is theparent of base_link. Although intuition would say that bothmap andodom should be attached to base_link, this is notallowed because each frame can only have one parent.

lightrain0
关注
  • 0
    点赞
  • 踩
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值