├── README.md ├── mrobot_bringup ├── CMakeLists.txt ├── config │ └── fake_mrobot_arbotix.yaml ├── include │ └── mrobot_bringup │ │ └── mrobot.h ├── launch │ ├── fake_laser_freenect.launch │ ├── fake_mrobot_with_kinect.launch │ ├── fake_mrobot_with_laser.launch │ ├── freenect.launch │ ├── mrobot.launch │ ├── mrobot_with_camera.launch │ ├── mrobot_with_kinect.launch │ ├── mrobot_with_laser.launch │ ├── rplidar.launch │ └── usb_cam.launch ├── package.xml ├── scripts │ ├── calibrate_angular.py │ ├── calibrate_linear.py │ ├── move_base_square.py │ ├── nav_square.py │ ├── odom_ekf.py │ ├── odom_out_and_back.py │ └── timed_out_and_back.py └── src │ ├── mrobot.cpp │ └── mrobot_bringup.cpp ├── mrobot_description ├── CMakeLists.txt ├── config │ ├── fake_mrobot_arbotix.yaml │ ├── mrobot.rviz │ ├── mrobot_arbotix.rviz │ └── mrobot_urdf.rviz ├── launch │ ├── arbotix_mrobot_with_camera.launch │ ├── arbotix_mrobot_with_kinect.launch │ ├── arbotix_mrobot_with_laser.launch │ ├── display_mrobot.launch │ ├── display_mrobot_chassis_urdf.launch │ ├── display_mrobot_with_camera.launch │ ├── display_mrobot_with_kinect.launch │ └── display_mrobot_with_laser.launch ├── meshes │ ├── kinect.dae │ ├── kinect.jpg │ └── kinect.tga ├── package.xml └── urdf │ ├── camera.xacro │ ├── kinect.xacro │ ├── mrobot.urdf │ ├── mrobot.urdf.xacro │ ├── mrobot_body.urdf.xacro │ ├── mrobot_chassis.gv │ ├── mrobot_chassis.pdf │ ├── mrobot_chassis.urdf │ ├── mrobot_with_camera.urdf.xacro │ ├── mrobot_with_kinect.urdf.xacro │ ├── mrobot_with_rplidar.urdf.xacro │ └── rplidar.xacro ├── mrobot_gazebo ├── CMakeLists.txt ├── launch │ ├── mrobot_kinect_nav_gazebo.launch │ ├── mrobot_laser_nav_gazebo.launch │ ├── view_mrobot_gazebo.launch │ ├── view_mrobot_with_camera_gazebo.launch │ ├── view_mrobot_with_kinect_gazebo.launch │ └── view_mrobot_with_laser_gazebo.launch ├── package.xml ├── urdf │ ├── camera.xacro │ ├── kinect.xacro │ ├── mrobot.urdf.xacro │ ├── mrobot_body.urdf.xacro │ ├── mrobot_with_camera.urdf.xacro │ ├── mrobot_with_kinect.urdf.xacro │ ├── mrobot_with_rplidar.urdf.xacro │ └── rplidar.xacro └── worlds │ ├── cloister.world │ ├── playground.world │ ├── playpen.world │ └── room.world ├── mrobot_navigation ├── CMakeLists.txt ├── config │ ├── fake │ │ ├── base_local_planner_params.yaml │ │ ├── costmap_common_params.yaml │ │ ├── global_costmap_params.yaml │ │ └── local_costmap_params.yaml │ ├── mrobot │ │ ├── base_local_planner_params.yaml │ │ ├── costmap_common_params.yaml │ │ ├── global_costmap_params.yaml │ │ └── local_costmap_params.yaml │ └── rplidar.lua ├── launch │ ├── amcl.launch │ ├── cartographer_demo_rplidar.launch │ ├── exploring_slam_demo.launch │ ├── fake_move_base.launch │ ├── fake_nav_cloister_demo.launch │ ├── fake_nav_demo.launch │ ├── gmapping.launch │ ├── gmapping_demo.launch │ ├── hector.launch │ ├── hector_demo.launch │ ├── move_base.launch │ └── nav_demo.launch ├── maps │ ├── blank_map.pgm │ ├── blank_map.yaml │ ├── blank_map_with_obstacle.pgm │ ├── blank_map_with_obstacle.yaml │ ├── cloister_gmapping.pgm │ ├── cloister_gmapping.yaml │ ├── cloister_hector.pgm │ ├── cloister_hector.yaml │ ├── gmapping_map.pgm │ ├── gmapping_map.yaml │ ├── hector_map.pgm │ ├── hector_map.yaml │ ├── room.pgm │ ├── room.yaml │ ├── test_map.pgm │ └── test_map.yaml ├── package.xml ├── rviz │ ├── amcl.rviz │ ├── demo_2d.rviz │ ├── gmapping.rviz │ └── nav.rviz └── scripts │ ├── exploring_slam.py │ └── random_navigation.py └── mrobot_teleop ├── CMakeLists.txt ├── launch ├── logitech.launch └── mrobot_teleop.launch ├── package.xml └── scripts └── mrobot_teleop.py /README.md: -------------------------------------------------------------------------------- 1 | # robot_mrobot 2 | 3 | This is a ROS package, which is used to learn about some operations of move robot. 4 | 5 | - Learn to create and optimize a car model. 6 | - Learn to use rviz+ArbotiX to build a motion simulator. 7 | - Master the basic usage of ros_control (https://github.com/1027243334/ros_control). 8 | - Learn to use gazebo for simulation. 9 | 10 | The tutorial with more details is located in xxx. -------------------------------------------------------------------------------- /mrobot_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mrobot_bringup) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | sensor_msgs 14 | geometry_msgs 15 | tf 16 | ) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | # find_package(Boost REQUIRED COMPONENTS system) 20 | 21 | 22 | ## Uncomment this if the package has a setup.py. This macro ensures 23 | ## modules and global scripts declared therein get installed 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 25 | # catkin_python_setup() 26 | 27 | ################################################ 28 | ## Declare ROS messages, services and actions ## 29 | ################################################ 30 | 31 | ## To declare and build messages, services or actions from within this 32 | ## package, follow these steps: 33 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 34 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 35 | ## * In the file package.xml: 36 | ## * add a build_depend tag for "message_generation" 37 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 38 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 39 | ## but can be declared for certainty nonetheless: 40 | ## * add a run_depend tag for "message_runtime" 41 | ## * In this file (CMakeLists.txt): 42 | ## * add "message_generation" and every package in MSG_DEP_SET to 43 | ## find_package(catkin REQUIRED COMPONENTS ...) 44 | ## * add "message_runtime" and every package in MSG_DEP_SET to 45 | ## catkin_package(CATKIN_DEPENDS ...) 46 | ## * uncomment the add_*_files sections below as needed 47 | ## and list every .msg/.srv/.action file to be processed 48 | ## * uncomment the generate_messages entry below 49 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 50 | 51 | ## Generate messages in the 'msg' folder 52 | # add_message_files( 53 | # FILES 54 | # Message1.msg 55 | # Message2.msg 56 | # ) 57 | 58 | ## Generate services in the 'srv' folder 59 | # add_service_files( 60 | # FILES 61 | # Service1.srv 62 | # Service2.srv 63 | # ) 64 | 65 | ## Generate actions in the 'action' folder 66 | # add_action_files( 67 | # FILES 68 | # Action1.action 69 | # Action2.action 70 | # ) 71 | 72 | ## Generate added messages and services with any dependencies listed here 73 | # generate_messages( 74 | # DEPENDENCIES 75 | # sensor_msgs 76 | # ) 77 | 78 | ################################################ 79 | ## Declare ROS dynamic reconfigure parameters ## 80 | ################################################ 81 | 82 | ## To declare and build dynamic reconfigure parameters within this 83 | ## package, follow these steps: 84 | ## * In the file package.xml: 85 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 86 | ## * In this file (CMakeLists.txt): 87 | ## * add "dynamic_reconfigure" to 88 | ## find_package(catkin REQUIRED COMPONENTS ...) 89 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 90 | ## and list every .cfg file to be processed 91 | 92 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 93 | # generate_dynamic_reconfigure_options( 94 | # cfg/DynReconf1.cfg 95 | # cfg/DynReconf2.cfg 96 | # ) 97 | 98 | ################################### 99 | ## catkin specific configuration ## 100 | ################################### 101 | ## The catkin_package macro generates cmake config files for your package 102 | ## Declare things to be passed to dependent projects 103 | ## INCLUDE_DIRS: uncomment this if you package contains header files 104 | ## LIBRARIES: libraries you create in this project that dependent projects also need 105 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 106 | ## DEPENDS: system dependencies of this project that dependent projects also need 107 | catkin_package( 108 | INCLUDE_DIRS include 109 | # LIBRARIES mrobot_bringup 110 | # CATKIN_DEPENDS roscpp rospy sensor_msgs 111 | # DEPENDS system_lib 112 | ) 113 | 114 | ########### 115 | ## Build ## 116 | ########### 117 | 118 | ## Specify additional locations of header files 119 | ## Your package locations should be listed before other locations 120 | include_directories( 121 | include 122 | ${catkin_INCLUDE_DIRS} 123 | ) 124 | 125 | ## Declare a C++ library 126 | # add_library(${PROJECT_NAME} 127 | # src/${PROJECT_NAME}/mrobot_bringup.cpp 128 | # ) 129 | 130 | ## Add cmake target dependencies of the library 131 | ## as an example, code may need to be generated before libraries 132 | ## either from message generation or dynamic reconfigure 133 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 134 | 135 | ## Declare a C++ executable 136 | ## With catkin_make all packages are built within a single CMake context 137 | ## The recommended prefix ensures that target names across packages don't collide 138 | # add_executable(${PROJECT_NAME}_node src/mrobot_bringup_node.cpp) 139 | 140 | ## Rename C++ executable without prefix 141 | ## The above recommended prefix causes long target names, the following renames the 142 | ## target back to the shorter version for ease of user use 143 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 144 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 145 | 146 | ## Add cmake target dependencies of the executable 147 | ## same as for the library above 148 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 149 | 150 | ## Specify libraries to link a library or executable target against 151 | # target_link_libraries(${PROJECT_NAME}_node 152 | # ${catkin_LIBRARIES} 153 | # ) 154 | 155 | add_executable(mrobot_bringup 156 | src/mrobot_bringup.cpp 157 | src/mrobot.cpp) 158 | target_link_libraries(mrobot_bringup ${catkin_LIBRARIES}) 159 | 160 | ############# 161 | ## Install ## 162 | ############# 163 | 164 | # all install targets should use catkin DESTINATION variables 165 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 166 | 167 | ## Mark executable scripts (Python etc.) for installation 168 | ## in contrast to setup.py, you can choose the destination 169 | # install(PROGRAMS 170 | # scripts/my_python_script 171 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark executables and/or libraries for installation 175 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 176 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark cpp header files for installation 182 | # install(DIRECTORY include/${PROJECT_NAME}/ 183 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 184 | # FILES_MATCHING PATTERN "*.h" 185 | # PATTERN ".svn" EXCLUDE 186 | # ) 187 | 188 | ## Mark other files for installation (e.g. launch and bag files, etc.) 189 | # install(FILES 190 | # # myfile1 191 | # # myfile2 192 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 193 | # ) 194 | 195 | ############# 196 | ## Testing ## 197 | ############# 198 | 199 | ## Add gtest based cpp test target and link libraries 200 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_mrobot_bringup.cpp) 201 | # if(TARGET ${PROJECT_NAME}-test) 202 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 203 | # endif() 204 | 205 | ## Add folders to be run by python nosetests 206 | # catkin_add_nosetests(test) 207 | -------------------------------------------------------------------------------- /mrobot_bringup/config/fake_mrobot_arbotix.yaml: -------------------------------------------------------------------------------- 1 | port: /dev/ttyUSB0 2 | baud: 115200 3 | rate: 20 4 | sync_write: True 5 | sync_read: True 6 | read_rate: 20 7 | write_rate: 20 8 | 9 | controllers: { 10 | # Pololu motors: 1856 cpr = 0.3888105m travel = 4773 ticks per meter (empirical: 4100) 11 | base_controller: {type: diff_controller, base_frame_id: base_footprint, base_width: 0.26, ticks_meter: 4100, Kp: 12, Kd: 12, Ki: 0, Ko: 50, accel_limit: 1.0 } 12 | } 13 | -------------------------------------------------------------------------------- /mrobot_bringup/include/mrobot_bringup/mrobot.h: -------------------------------------------------------------------------------- 1 | #ifndef MROBOT_H 2 | #define MROBOT_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace mrobot 13 | { 14 | 15 | class MRobot 16 | { 17 | public: 18 | MRobot(); 19 | ~MRobot(); 20 | bool init(); 21 | bool spinOnce(double RobotV, double YawRate); 22 | 23 | private: 24 | bool readSpeed(); 25 | void writeSpeed(double RobotV, double YawRate); 26 | unsigned char getCrc8(unsigned char *ptr, unsigned short len); 27 | 28 | private: 29 | ros::Time current_time_, last_time_; 30 | 31 | double x_; 32 | double y_; 33 | double th_; 34 | 35 | double vx_; 36 | double vy_; 37 | double vth_; 38 | 39 | ros::NodeHandle nh; 40 | ros::Publisher pub_; 41 | tf::TransformBroadcaster odom_broadcaster_; 42 | }; 43 | 44 | } 45 | 46 | #endif /* MROBOT_H */ 47 | -------------------------------------------------------------------------------- /mrobot_bringup/launch/fake_laser_freenect.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /mrobot_bringup/launch/fake_mrobot_with_kinect.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /mrobot_bringup/launch/fake_mrobot_with_laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /mrobot_bringup/launch/freenect.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /mrobot_bringup/launch/mrobot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /mrobot_bringup/launch/mrobot_with_camera.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /mrobot_bringup/launch/mrobot_with_kinect.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /mrobot_bringup/launch/mrobot_with_laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /mrobot_bringup/launch/rplidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /mrobot_bringup/launch/usb_cam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /mrobot_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrobot_bringup 4 | 0.0.0 5 | The mrobot_bringup package 6 | 7 | 8 | 9 | 10 | hcx 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | rospy 45 | sensor_msgs 46 | roscpp 47 | rospy 48 | sensor_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /mrobot_bringup/scripts/calibrate_angular.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ calibrate_angular.py - Version 1.1 2013-12-20 4 | 5 | Rotate the robot 360 degrees to check the odometry parameters of the base controller. 6 | 7 | Created for the Pi Robot Project: http://www.pirobot.org 8 | Copyright (c) 2012 Patrick Goebel. All rights reserved. 9 | 10 | This program is free software; you can redistribute it and/or modify 11 | it under the terms of the GNU General Public License as published by 12 | the Free Software Foundation; either version 2 of the License, or 13 | (at your option) any later version.5 14 | 15 | This program is distributed in the hope that it will be useful, 16 | but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | GNU General Public License for more details at: 19 | 20 | http://www.gnu.org/licenses/gpl.html 21 | 22 | """ 23 | 24 | import rospy 25 | from geometry_msgs.msg import Twist, Quaternion 26 | from nav_msgs.msg import Odometry 27 | from dynamic_reconfigure.server import Server 28 | import dynamic_reconfigure.client 29 | from mrobot_nav.cfg import CalibrateAngularConfig 30 | import tf 31 | from math import radians, copysign 32 | from rbx1_nav.transform_utils import quat_to_angle, normalize_angle 33 | 34 | class CalibrateAngular(): 35 | def __init__(self): 36 | # Give the node a name 37 | rospy.init_node('calibrate_angular', anonymous=False) 38 | 39 | # Set rospy to execute a shutdown function when terminating the script 40 | rospy.on_shutdown(self.shutdown) 41 | 42 | # How fast will we check the odometry values? 43 | self.rate = rospy.get_param('~rate', 20) 44 | r = rospy.Rate(self.rate) 45 | 46 | # The test angle is 360 degrees 47 | self.test_angle = radians(rospy.get_param('~test_angle', 360.0)) 48 | 49 | self.speed = rospy.get_param('~speed', 0.5) # radians per second 50 | self.tolerance = radians(rospy.get_param('tolerance', 1)) # degrees converted to radians 51 | self.odom_angular_scale_correction = rospy.get_param('~odom_angular_scale_correction', 1.0) 52 | self.start_test = rospy.get_param('~start_test', True) 53 | 54 | # Publisher to control the robot's speed 55 | self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) 56 | 57 | # Fire up the dynamic_reconfigure server 58 | dyn_server = Server(CalibrateAngularConfig, self.dynamic_reconfigure_callback) 59 | 60 | # Connect to the dynamic_reconfigure server 61 | dyn_client = dynamic_reconfigure.client.Client("calibrate_angular", timeout=60) 62 | 63 | # The base frame is usually base_link or base_footprint 64 | self.base_frame = rospy.get_param('~base_frame', '/base_link') 65 | 66 | # The odom frame is usually just /odom 67 | self.odom_frame = rospy.get_param('~odom_frame', '/odom') 68 | 69 | # Initialize the tf listener 70 | self.tf_listener = tf.TransformListener() 71 | 72 | # Give tf some time to fill its buffer 73 | rospy.sleep(2) 74 | 75 | # Make sure we see the odom and base frames 76 | # self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) 77 | 78 | try: 79 | self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) 80 | # self.base_frame = '/base_footprint' 81 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 82 | try: 83 | self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(60.0)) 84 | self.base_frame = '/base_link' 85 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 86 | rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint") 87 | rospy.signal_shutdown("tf Exception") 88 | 89 | rospy.loginfo("Bring up rqt_reconfigure to control the test.") 90 | 91 | reverse = 1 92 | 93 | while not rospy.is_shutdown(): 94 | if self.start_test: 95 | # Get the current rotation angle from tf 96 | self.odom_angle = self.get_odom_angle() 97 | 98 | last_angle = self.odom_angle 99 | turn_angle = 0 100 | self.test_angle *= reverse 101 | error = self.test_angle - turn_angle 102 | 103 | # Alternate directions between tests 104 | reverse = -reverse 105 | 106 | while abs(error) > self.tolerance and self.start_test: 107 | if rospy.is_shutdown(): 108 | return 109 | 110 | # Rotate the robot to reduce the error 111 | move_cmd = Twist() 112 | move_cmd.angular.z = copysign(self.speed, error) 113 | self.cmd_vel.publish(move_cmd) 114 | r.sleep() 115 | 116 | # Get the current rotation angle from tf 117 | self.odom_angle = self.get_odom_angle() 118 | 119 | # Compute how far we have gone since the last measurement 120 | delta_angle = self.odom_angular_scale_correction * normalize_angle(self.odom_angle - last_angle) 121 | 122 | # Add to our total angle so far 123 | turn_angle += delta_angle 124 | 125 | # Compute the new error 126 | error = self.test_angle - turn_angle 127 | 128 | # Store the current angle for the next comparison 129 | last_angle = self.odom_angle 130 | 131 | # Stop the robot 132 | self.cmd_vel.publish(Twist()) 133 | 134 | # Update the status flag 135 | self.start_test = False 136 | params = {'start_test': False} 137 | dyn_client.update_configuration(params) 138 | 139 | rospy.sleep(0.5) 140 | 141 | # Stop the robot 142 | self.cmd_vel.publish(Twist()) 143 | 144 | def get_odom_angle(self): 145 | # Get the current transform between the odom and base frames 146 | try: 147 | (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0)) 148 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 149 | rospy.loginfo("TF Exception") 150 | return 151 | 152 | # Convert the rotation from a quaternion to an Euler angle 153 | return quat_to_angle(Quaternion(*rot)) 154 | 155 | def dynamic_reconfigure_callback(self, config, level): 156 | self.test_angle = radians(config['test_angle']) 157 | self.speed = config['speed'] 158 | self.tolerance = radians(config['tolerance']) 159 | self.odom_angular_scale_correction = config['odom_angular_scale_correction'] 160 | self.start_test = config['start_test'] 161 | 162 | return config 163 | 164 | def shutdown(self): 165 | # Always stop the robot when shutting down the node 166 | rospy.loginfo("Stopping the robot...") 167 | self.cmd_vel.publish(Twist()) 168 | rospy.sleep(1) 169 | 170 | if __name__ == '__main__': 171 | try: 172 | CalibrateAngular() 173 | except: 174 | rospy.loginfo("Calibration terminated.") 175 | -------------------------------------------------------------------------------- /mrobot_bringup/scripts/calibrate_linear.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ calibrate_linear.py - Version 1.1 2013-12-20 4 | 5 | Move the robot 1.0 meter to check on the PID parameters of the base controller. 6 | 7 | Created for the Pi Robot Project: http://www.pirobot.org 8 | Copyright (c) 2012 Patrick Goebel. All rights reserved. 9 | 10 | This program is free software; you can redistribute it and/or modify 11 | it under the terms of the GNU General Public License as published by 12 | the Free Software Foundation; either version 2 of the License, or 13 | (at your option) any later version.5 14 | 15 | This program is distributed in the hope that it will be useful, 16 | but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | GNU General Public License for more details at: 19 | 20 | http://www.gnu.org/licenses/gpl.html 21 | 22 | """ 23 | 24 | import rospy 25 | from geometry_msgs.msg import Twist, Point 26 | from math import copysign, sqrt, pow 27 | from dynamic_reconfigure.server import Server 28 | import dynamic_reconfigure.client 29 | from mrobot_nav.cfg import CalibrateLinearConfig 30 | import tf 31 | 32 | class CalibrateLinear(): 33 | def __init__(self): 34 | # Give the node a name 35 | rospy.init_node('calibrate_linear', anonymous=False) 36 | 37 | # Set rospy to execute a shutdown function when terminating the script 38 | rospy.on_shutdown(self.shutdown) 39 | 40 | # How fast will we check the odometry values? 41 | self.rate = rospy.get_param('~rate', 20) 42 | r = rospy.Rate(self.rate) 43 | 44 | # Set the distance to travel 45 | self.test_distance = rospy.get_param('~test_distance', 1.0) # meters 46 | self.speed = rospy.get_param('~speed', 0.15) # meters per second 47 | self.tolerance = rospy.get_param('~tolerance', 0.01) # meters 48 | self.odom_linear_scale_correction = rospy.get_param('~odom_linear_scale_correction', 1.0) 49 | self.start_test = rospy.get_param('~start_test', True) 50 | 51 | # Publisher to control the robot's speed 52 | self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) 53 | 54 | # Fire up the dynamic_reconfigure server 55 | dyn_server = Server(CalibrateLinearConfig, self.dynamic_reconfigure_callback) 56 | 57 | # Connect to the dynamic_reconfigure server 58 | dyn_client = dynamic_reconfigure.client.Client("calibrate_linear", timeout=60) 59 | 60 | # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot 61 | self.base_frame = rospy.get_param('~base_frame', '/base_link') 62 | 63 | # The odom frame is usually just /odom 64 | self.odom_frame = rospy.get_param('~odom_frame', '/odom') 65 | 66 | # Initialize the tf listener 67 | self.tf_listener = tf.TransformListener() 68 | 69 | # Give tf some time to fill its buffer 70 | rospy.sleep(2) 71 | 72 | # Make sure we see the odom and base frames 73 | # self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) 74 | 75 | # rospy.loginfo("Bring up rqt_reconfigure to control the test.") 76 | 77 | # self.odom_frame = '/odom' 78 | 79 | # Find out if the robot uses /base_link or /base_footprint 80 | try: 81 | self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) 82 | # self.base_frame = '/base_footprint' 83 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 84 | try: 85 | self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(60.0)) 86 | self.base_frame = '/base_link' 87 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 88 | rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint") 89 | rospy.signal_shutdown("tf Exception") 90 | 91 | rospy.loginfo("Bring up rqt_reconfigure to control the test.") 92 | 93 | self.position = Point() 94 | 95 | # Get the starting position from the tf transform between the odom and base frames 96 | self.position = self.get_position() 97 | 98 | x_start = self.position.x 99 | y_start = self.position.y 100 | 101 | move_cmd = Twist() 102 | 103 | while not rospy.is_shutdown(): 104 | # Stop the robot by default 105 | move_cmd = Twist() 106 | 107 | if self.start_test: 108 | # Get the current position from the tf transform between the odom and base frames 109 | self.position = self.get_position() 110 | 111 | # Compute the Euclidean distance from the target point 112 | distance = sqrt(pow((self.position.x - x_start), 2) + 113 | pow((self.position.y - y_start), 2)) 114 | 115 | # Correct the estimated distance by the correction factor 116 | distance *= self.odom_linear_scale_correction 117 | 118 | # How close are we? 119 | error = distance - self.test_distance 120 | 121 | # Are we close enough? 122 | if not self.start_test or abs(error) < self.tolerance: 123 | self.start_test = False 124 | params = {'start_test': False} 125 | rospy.loginfo(params) 126 | dyn_client.update_configuration(params) 127 | else: 128 | # If not, move in the appropriate direction 129 | move_cmd.linear.x = copysign(self.speed, -1 * error) 130 | else: 131 | self.position = self.get_position() 132 | x_start = self.position.x 133 | y_start = self.position.y 134 | 135 | self.cmd_vel.publish(move_cmd) 136 | r.sleep() 137 | 138 | # Stop the robot 139 | self.cmd_vel.publish(Twist()) 140 | 141 | def dynamic_reconfigure_callback(self, config, level): 142 | self.test_distance = config['test_distance'] 143 | self.speed = config['speed'] 144 | self.tolerance = config['tolerance'] 145 | self.odom_linear_scale_correction = config['odom_linear_scale_correction'] 146 | self.start_test = config['start_test'] 147 | 148 | return config 149 | 150 | def get_position(self): 151 | # Get the current transform between the odom and base frames 152 | try: 153 | (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0)) 154 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 155 | rospy.loginfo("TF Exception") 156 | return 157 | 158 | return Point(*trans) 159 | 160 | def shutdown(self): 161 | # Always stop the robot when shutting down the node 162 | rospy.loginfo("Stopping the robot...") 163 | self.cmd_vel.publish(Twist()) 164 | rospy.sleep(1) 165 | 166 | if __name__ == '__main__': 167 | try: 168 | CalibrateLinear() 169 | rospy.spin() 170 | except: 171 | rospy.loginfo("Calibration terminated.") 172 | -------------------------------------------------------------------------------- /mrobot_bringup/scripts/move_base_square.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ move_base_square.py - Version 1.1 2013-12-20 4 | 5 | Command a robot to move in a square using move_base actions.. 6 | 7 | Created for the Pi Robot Project: http://www.pirobot.org 8 | Copyright (c) 2012 Patrick Goebel. All rights reserved. 9 | 10 | This program is free software; you can redistribute it and/or modify 11 | it under the terms of the GNU General Public License as published by 12 | the Free Software Foundation; either version 2 of the License, or 13 | (at your option) any later version.5 14 | 15 | This program is distributed in the hope that it will be useful, 16 | but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | GNU General Public License for more details at: 19 | 20 | http://www.gnu.org/licenses/gpl.htmlPoint 21 | 22 | """ 23 | 24 | import rospy 25 | import actionlib 26 | from actionlib_msgs.msg import * 27 | from geometry_msgs.msg import Pose, Point, Quaternion, Twist 28 | from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal 29 | from tf.transformations import quaternion_from_euler 30 | from visualization_msgs.msg import Marker 31 | from math import radians, pi 32 | 33 | class MoveBaseSquare(): 34 | def __init__(self): 35 | rospy.init_node('nav_test', anonymous=False) 36 | 37 | rospy.on_shutdown(self.shutdown) 38 | 39 | # How big is the square we want the robot to navigate? 40 | square_size = rospy.get_param("~square_size", 1.0) # meters 41 | 42 | # Create a list to hold the target quaternions (orientations) 43 | quaternions = list() 44 | 45 | # First define the corner orientations as Euler angles 46 | euler_angles = (pi/2, pi, 3*pi/2, 0) 47 | 48 | # Then convert the angles to quaternions 49 | for angle in euler_angles: 50 | q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz') 51 | q = Quaternion(*q_angle) 52 | quaternions.append(q) 53 | 54 | # Create a list to hold the waypoint poses 55 | waypoints = list() 56 | 57 | # Append each of the four waypoints to the list. Each waypoint 58 | # is a pose consisting of a position and orientation in the map frame. 59 | waypoints.append(Pose(Point(square_size, 0.0, 0.0), quaternions[0])) 60 | waypoints.append(Pose(Point(square_size, square_size, 0.0), quaternions[1])) 61 | waypoints.append(Pose(Point(0.0, square_size, 0.0), quaternions[2])) 62 | waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3])) 63 | 64 | # Initialize the visualization markers for RViz 65 | self.init_markers() 66 | 67 | # Set a visualization marker at each waypoint 68 | for waypoint in waypoints: 69 | p = Point() 70 | p = waypoint.position 71 | self.markers.points.append(p) 72 | 73 | # Publisher to manually control the robot (e.g. to stop it, queue_size=5) 74 | self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) 75 | 76 | # Subscribe to the move_base action server 77 | self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) 78 | 79 | rospy.loginfo("Waiting for move_base action server...") 80 | 81 | # Wait 60 seconds for the action server to become available 82 | self.move_base.wait_for_server(rospy.Duration(60)) 83 | 84 | rospy.loginfo("Connected to move base server") 85 | rospy.loginfo("Starting navigation test") 86 | 87 | # Initialize a counter to track waypoints 88 | i = 0 89 | 90 | # Cycle through the four waypoints 91 | while i < 4 and not rospy.is_shutdown(): 92 | # Update the marker display 93 | self.marker_pub.publish(self.markers) 94 | 95 | # Intialize the waypoint goal 96 | goal = MoveBaseGoal() 97 | 98 | # Use the map frame to define goal poses 99 | goal.target_pose.header.frame_id = 'map' 100 | 101 | # Set the time stamp to "now" 102 | goal.target_pose.header.stamp = rospy.Time.now() 103 | 104 | # Set the goal pose to the i-th waypoint 105 | goal.target_pose.pose = waypoints[i] 106 | 107 | # Start the robot moving toward the goal 108 | self.move(goal) 109 | 110 | i += 1 111 | 112 | def move(self, goal): 113 | # Send the goal pose to the MoveBaseAction server 114 | self.move_base.send_goal(goal) 115 | 116 | # Allow 1 minute to get there 117 | finished_within_time = self.move_base.wait_for_result(rospy.Duration(60)) 118 | 119 | # If we don't get there in time, abort the goal 120 | if not finished_within_time: 121 | self.move_base.cancel_goal() 122 | rospy.loginfo("Timed out achieving goal") 123 | else: 124 | # We made it! 125 | state = self.move_base.get_state() 126 | if state == GoalStatus.SUCCEEDED: 127 | rospy.loginfo("Goal succeeded!") 128 | 129 | def init_markers(self): 130 | # Set up our waypoint markers 131 | marker_scale = 0.2 132 | marker_lifetime = 0 # 0 is forever 133 | marker_ns = 'waypoints' 134 | marker_id = 0 135 | marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0} 136 | 137 | # Define a marker publisher. 138 | self.marker_pub = rospy.Publisher('waypoint_markers', Marker, queue_size=5) 139 | 140 | # Initialize the marker points list. 141 | self.markers = Marker() 142 | self.markers.ns = marker_ns 143 | self.markers.id = marker_id 144 | self.markers.type = Marker.CUBE_LIST 145 | self.markers.action = Marker.ADD 146 | self.markers.lifetime = rospy.Duration(marker_lifetime) 147 | self.markers.scale.x = marker_scale 148 | self.markers.scale.y = marker_scale 149 | self.markers.color.r = marker_color['r'] 150 | self.markers.color.g = marker_color['g'] 151 | self.markers.color.b = marker_color['b'] 152 | self.markers.color.a = marker_color['a'] 153 | 154 | self.markers.header.frame_id = 'odom' 155 | self.markers.header.stamp = rospy.Time.now() 156 | self.markers.points = list() 157 | 158 | def shutdown(self): 159 | rospy.loginfo("Stopping the robot...") 160 | # Cancel any active goals 161 | self.move_base.cancel_goal() 162 | rospy.sleep(2) 163 | # Stop the robot 164 | self.cmd_vel_pub.publish(Twist()) 165 | rospy.sleep(1) 166 | 167 | if __name__ == '__main__': 168 | try: 169 | MoveBaseSquare() 170 | except rospy.ROSInterruptException: 171 | rospy.loginfo("Navigation test finished.") 172 | -------------------------------------------------------------------------------- /mrobot_bringup/scripts/nav_square.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ nav_square.py - Version 1.1 2013-12-20 4 | 5 | A basic demo of the using odometry data to move the robot 6 | along a square trajectory. 7 | 8 | Created for the Pi Robot Project: http://www.pirobot.org 9 | Copyright (c) 2012 Patrick Goebel. All rights reserved. 10 | 11 | This program is free software; you can redistribute it and/or modify 12 | it under the terms of the GNU General Public License as published by 13 | the Free Software Foundation; either version 2 of the License, or 14 | (at your option) any later version.5 15 | 16 | This program is distributed in the hope that it will be useful, 17 | but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | GNU General Public License for more details at: 20 | 21 | http://www.gnu.org/licenses/gpl.html 22 | 23 | """ 24 | 25 | import rospy 26 | from geometry_msgs.msg import Twist, Point, Quaternion 27 | import tf,PyKDL 28 | from math import radians, copysign, sqrt, pow, pi 29 | 30 | def quat_to_angle(quat): 31 | rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w) 32 | return rot.GetRPY()[2] 33 | 34 | def normalize_angle(angle): 35 | res = angle 36 | while res > pi: 37 | res -= 2.0 * pi 38 | while res < -pi: 39 | res += 2.0 * pi 40 | return res 41 | 42 | class NavSquare(): 43 | def __init__(self): 44 | # Give the node a name 45 | rospy.init_node('nav_square', anonymous=False) 46 | 47 | # Set rospy to execute a shutdown function when terminating the script 48 | rospy.on_shutdown(self.shutdown) 49 | 50 | # How fast will we check the odometry values? 51 | rate = 20 52 | 53 | # Set the equivalent ROS rate variable 54 | r = rospy.Rate(rate) 55 | 56 | # Set the parameters for the target square 57 | goal_distance = rospy.get_param("~goal_distance", 1.0) # meters 58 | goal_angle = radians(rospy.get_param("~goal_angle", 90)) # degrees converted to radians 59 | linear_speed = rospy.get_param("~linear_speed", 0.2) # meters per second 60 | angular_speed = rospy.get_param("~angular_speed", 0.7) # radians per second 61 | angular_tolerance = radians(rospy.get_param("~angular_tolerance", 2)) # degrees to radians 62 | 63 | # Publisher to control the robot's speed 64 | self.cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5) 65 | 66 | # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot 67 | self.base_frame = rospy.get_param('~base_frame', '/base_link') 68 | 69 | # The odom frame is usually just /odom 70 | self.odom_frame = rospy.get_param('~odom_frame', '/odom') 71 | 72 | # Initialize the tf listener 73 | self.tf_listener = tf.TransformListener() 74 | 75 | # Give tf some time to fill its buffer 76 | rospy.sleep(2) 77 | 78 | # Set the odom frame 79 | self.odom_frame = '/odom' 80 | 81 | # Find out if the robot uses /base_link or /base_footprint 82 | try: 83 | self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) 84 | self.base_frame = '/base_footprint' 85 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 86 | try: 87 | self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) 88 | self.base_frame = '/base_link' 89 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 90 | rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint") 91 | rospy.signal_shutdown("tf Exception") 92 | 93 | # Initialize the position variable as a Point type 94 | position = Point() 95 | 96 | # Cycle through the four sides of the square 97 | for i in range(4): 98 | # Initialize the movement command 99 | move_cmd = Twist() 100 | 101 | # Set the movement command to forward motion 102 | move_cmd.linear.x = linear_speed 103 | 104 | # Get the starting position values 105 | (position, rotation) = self.get_odom() 106 | 107 | x_start = position.x 108 | y_start = position.y 109 | 110 | # Keep track of the distance traveled 111 | distance = 0 112 | 113 | # Enter the loop to move along a side 114 | while distance < goal_distance and not rospy.is_shutdown(): 115 | # Publish the Twist message and sleep 1 cycle 116 | self.cmd_vel.publish(move_cmd) 117 | 118 | r.sleep() 119 | 120 | # Get the current position 121 | (position, rotation) = self.get_odom() 122 | 123 | # Compute the Euclidean distance from the start 124 | distance = sqrt(pow((position.x - x_start), 2) + 125 | pow((position.y - y_start), 2)) 126 | 127 | # Stop the robot before rotating 128 | move_cmd = Twist() 129 | self.cmd_vel.publish(move_cmd) 130 | rospy.sleep(1.0) 131 | 132 | # Set the movement command to a rotation 133 | move_cmd.angular.z = angular_speed 134 | 135 | # Track the last angle measured 136 | last_angle = rotation 137 | 138 | # Track how far we have turned 139 | turn_angle = 0 140 | 141 | # Begin the rotation 142 | while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown(): 143 | # Publish the Twist message and sleep 1 cycle 144 | self.cmd_vel.publish(move_cmd) 145 | 146 | r.sleep() 147 | 148 | # Get the current rotation 149 | (position, rotation) = self.get_odom() 150 | 151 | # Compute the amount of rotation since the last lopp 152 | delta_angle = normalize_angle(rotation - last_angle) 153 | 154 | turn_angle += delta_angle 155 | last_angle = rotation 156 | 157 | move_cmd = Twist() 158 | self.cmd_vel.publish(move_cmd) 159 | rospy.sleep(1.0) 160 | 161 | # Stop the robot when we are done 162 | self.cmd_vel.publish(Twist()) 163 | 164 | def get_odom(self): 165 | # Get the current transform between the odom and base frames 166 | try: 167 | (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0)) 168 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 169 | rospy.loginfo("TF Exception") 170 | return 171 | 172 | return (Point(*trans), quat_to_angle(Quaternion(*rot))) 173 | 174 | def shutdown(self): 175 | # Always stop the robot when shutting down the node 176 | rospy.loginfo("Stopping the robot...") 177 | self.cmd_vel.publish(Twist()) 178 | rospy.sleep(1) 179 | 180 | if __name__ == '__main__': 181 | try: 182 | NavSquare() 183 | except rospy.ROSInterruptException: 184 | rospy.loginfo("Navigation terminated.") 185 | 186 | -------------------------------------------------------------------------------- /mrobot_bringup/scripts/odom_ekf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ odom_ekf.py - Version 1.1 2013-12-20 4 | 5 | Republish the /robot_pose_ekf/odom_combined topic which is of type 6 | geometry_msgs/PoseWithCovarianceStamped as an equivalent message of 7 | type nav_msgs/Odometry so we can view it in RViz. 8 | 9 | Created for the Pi Robot Project: http://www.pirobot.org 10 | Copyright (c) 2012 Patrick Goebel. All rights reserved. 11 | 12 | This program is free software; you can redistribute it and/or modify 13 | it under the terms of the GNU General Public License as published by 14 | the Free Software Foundation; either version 2 of the License, or 15 | (at your option) any later version.5 16 | 17 | This program is distributed in the hope that it will be useful, 18 | but WITHOUT ANY WARRANTY; without even the implied warranty of 19 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 20 | GNU General Public License for more details at: 21 | 22 | http://www.gnu.org/licenses/gpl.html 23 | 24 | """ 25 | 26 | import rospy 27 | from geometry_msgs.msg import PoseWithCovarianceStamped 28 | from nav_msgs.msg import Odometry 29 | 30 | class OdomEKF(): 31 | def __init__(self): 32 | # Give the node a name 33 | rospy.init_node('odom_ekf', anonymous=False) 34 | 35 | # Publisher of type nav_msgs/Odometry 36 | self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=5) 37 | 38 | # Wait for the /odom_combined topic to become available 39 | rospy.wait_for_message('input', PoseWithCovarianceStamped) 40 | 41 | # Subscribe to the /odom_combined topic 42 | rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom) 43 | 44 | rospy.loginfo("Publishing combined odometry on /odom_ekf") 45 | 46 | def pub_ekf_odom(self, msg): 47 | odom = Odometry() 48 | odom.header = msg.header 49 | odom.header.frame_id = '/odom' 50 | odom.child_frame_id = 'base_link' 51 | odom.pose = msg.pose 52 | 53 | self.ekf_pub.publish(odom) 54 | 55 | if __name__ == '__main__': 56 | try: 57 | OdomEKF() 58 | rospy.spin() 59 | except: 60 | pass 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /mrobot_bringup/scripts/odom_out_and_back.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ odom_out_and_back.py - Version 1.1 2013-12-20 4 | 5 | A basic demo of using the /odom topic to move a robot a given distance 6 | or rotate through a given angle. 7 | 8 | Created for the Pi Robot Project: http://www.pirobot.org 9 | Copyright (c) 2012 Patrick Goebel. All rights reserved. 10 | 11 | This program is free software; you can redistribute it and/or modify 12 | it under the terms of the GNU General Public License as published by 13 | the Free Software Foundation; either version 2 of the License, or 14 | (at your option) any later version.5 15 | 16 | This program is distributed in the hope that it will be useful, 17 | but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | GNU General Public License for more details at: 20 | 21 | http://www.gnu.org/licenses/gpl.html 22 | 23 | """ 24 | 25 | import rospy 26 | from geometry_msgs.msg import Twist, Point, Quaternion 27 | import tf,PyKDL 28 | from math import radians, copysign, sqrt, pow, pi 29 | 30 | def quat_to_angle(quat): 31 | rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w) 32 | return rot.GetRPY()[2] 33 | 34 | def normalize_angle(angle): 35 | res = angle 36 | while res > pi: 37 | res -= 2.0 * pi 38 | while res < -pi: 39 | res += 2.0 * pi 40 | return res 41 | 42 | class OutAndBack(): 43 | def __init__(self): 44 | # Give the node a name 45 | rospy.init_node('out_and_back', anonymous=False) 46 | 47 | # Set rospy to execute a shutdown function when exiting 48 | rospy.on_shutdown(self.shutdown) 49 | 50 | # Publisher to control the robot's speed 51 | self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) 52 | 53 | # How fast will we update the robot's movement? 54 | rate = 20 55 | 56 | # Set the equivalent ROS rate variable 57 | r = rospy.Rate(rate) 58 | 59 | # Set the forward linear speed to 0.15 meters per second 60 | linear_speed = 0.15 61 | 62 | # Set the travel distance in meters 63 | goal_distance = 1.0 64 | 65 | # Set the rotation speed in radians per second 66 | angular_speed = 0.5 67 | 68 | # Set the angular tolerance in degrees converted to radians 69 | angular_tolerance = radians(1.0) 70 | 71 | # Set the rotation angle to Pi radians (180 degrees) 72 | goal_angle = pi 73 | 74 | # Initialize the tf listener 75 | self.tf_listener = tf.TransformListener() 76 | 77 | # Give tf some time to fill its buffer 78 | rospy.sleep(2) 79 | 80 | # Set the odom frame 81 | self.odom_frame = '/odom' 82 | 83 | # Find out if the robot uses /base_link or /base_footprint 84 | try: 85 | self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) 86 | self.base_frame = '/base_footprint' 87 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 88 | try: 89 | self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) 90 | self.base_frame = '/base_link' 91 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 92 | rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint") 93 | rospy.signal_shutdown("tf Exception") 94 | 95 | # Initialize the position variable as a Point type 96 | position = Point() 97 | 98 | # Loop once for each leg of the trip 99 | for i in range(2): 100 | # Initialize the movement command 101 | move_cmd = Twist() 102 | 103 | # Set the movement command to forward motion 104 | move_cmd.linear.x = linear_speed 105 | 106 | # Get the starting position values 107 | (position, rotation) = self.get_odom() 108 | 109 | x_start = position.x 110 | y_start = position.y 111 | 112 | # Keep track of the distance traveled 113 | distance = 0 114 | 115 | # Enter the loop to move along a side 116 | while distance < goal_distance and not rospy.is_shutdown(): 117 | # Publish the Twist message and sleep 1 cycle 118 | self.cmd_vel.publish(move_cmd) 119 | 120 | r.sleep() 121 | 122 | # Get the current position 123 | (position, rotation) = self.get_odom() 124 | 125 | # Compute the Euclidean distance from the start 126 | distance = sqrt(pow((position.x - x_start), 2) + 127 | pow((position.y - y_start), 2)) 128 | 129 | # Stop the robot before the rotation 130 | move_cmd = Twist() 131 | self.cmd_vel.publish(move_cmd) 132 | rospy.sleep(1) 133 | 134 | # Set the movement command to a rotation 135 | move_cmd.angular.z = angular_speed 136 | 137 | # Track the last angle measured 138 | last_angle = rotation 139 | 140 | # Track how far we have turned 141 | turn_angle = 0 142 | 143 | while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown(): 144 | # Publish the Twist message and sleep 1 cycle 145 | self.cmd_vel.publish(move_cmd) 146 | r.sleep() 147 | 148 | # Get the current rotation 149 | (position, rotation) = self.get_odom() 150 | 151 | # Compute the amount of rotation since the last loop 152 | delta_angle = normalize_angle(rotation - last_angle) 153 | 154 | # Add to the running total 155 | turn_angle += delta_angle 156 | last_angle = rotation 157 | 158 | # Stop the robot before the next leg 159 | move_cmd = Twist() 160 | self.cmd_vel.publish(move_cmd) 161 | rospy.sleep(1) 162 | 163 | # Stop the robot for good 164 | self.cmd_vel.publish(Twist()) 165 | 166 | def get_odom(self): 167 | # Get the current transform between the odom and base frames 168 | try: 169 | (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0)) 170 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 171 | rospy.loginfo("TF Exception") 172 | return 173 | 174 | return (Point(*trans), quat_to_angle(Quaternion(*rot))) 175 | 176 | def shutdown(self): 177 | # Always stop the robot when shutting down the node. 178 | rospy.loginfo("Stopping the robot...") 179 | self.cmd_vel.publish(Twist()) 180 | rospy.sleep(1) 181 | 182 | if __name__ == '__main__': 183 | try: 184 | OutAndBack() 185 | except: 186 | rospy.loginfo("Out-and-Back node terminated.") 187 | 188 | -------------------------------------------------------------------------------- /mrobot_bringup/scripts/timed_out_and_back.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ timed_out_and_back.py - Version 1.2 2014-12-14 4 | 5 | A basic demo of the using odometry data to move the robot along 6 | and out-and-back trajectory. 7 | 8 | Created for the Pi Robot Project: http://www.pirobot.org 9 | Copyright (c) 2012 Patrick Goebel. All rights reserved. 10 | 11 | This program is free software; you can redistribute it and/or modify 12 | it under the terms of the GNU General Public License as published by 13 | the Free Software Foundation; either version 2 of the License, or 14 | (at your option) any later version.5 15 | 16 | This program is distributed in the hope that it will be useful, 17 | but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | GNU General Public License for more details at: 20 | 21 | http://www.gnu.org/licenses/gpl.html 22 | 23 | """ 24 | 25 | import rospy 26 | from geometry_msgs.msg import Twist 27 | from math import pi 28 | 29 | class OutAndBack(): 30 | def __init__(self): 31 | # Give the node a name 32 | rospy.init_node('out_and_back', anonymous=False) 33 | 34 | # Set rospy to execute a shutdown function when exiting 35 | rospy.on_shutdown(self.shutdown) 36 | 37 | # Publisher to control the robot's speed 38 | self.cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=1) 39 | 40 | # How fast will we update the robot's movement? 41 | rate = 100 42 | 43 | # Set the equivalent ROS rate variable 44 | r = rospy.Rate(rate) 45 | 46 | # Set the forward linear speed to 0.2 meters per second 47 | linear_speed = 0.125 48 | 49 | # Set the travel distance to 1.0 meters 50 | goal_distance = 1.0 51 | 52 | # How long should it take us to get there? 53 | linear_duration = goal_distance / linear_speed 54 | 55 | # Set the rotation speed to 1.0 radians per second 56 | angular_speed = 1.0 57 | 58 | # Set the rotation angle to Pi radians (180 degrees) 59 | goal_angle = pi 60 | 61 | # How long should it take to rotate? 62 | angular_duration = goal_angle / angular_speed 63 | 64 | # Loop through the two legs of the trip 65 | for i in range(2): 66 | # Initialize the movement command 67 | move_cmd = Twist() 68 | 69 | # Set the forward speed 70 | move_cmd.linear.x = linear_speed 71 | 72 | # Move forward for a time to go the desired distance 73 | ticks = int(linear_duration * rate) 74 | 75 | for t in range(ticks): 76 | self.cmd_vel.publish(move_cmd) 77 | r.sleep() 78 | 79 | # Stop the robot before the rotation 80 | move_cmd = Twist() 81 | self.cmd_vel.publish(move_cmd) 82 | rospy.sleep(1) 83 | 84 | # Now rotate left roughly 180 degrees 85 | 86 | # Set the angular speed 87 | move_cmd.angular.z = angular_speed 88 | 89 | # Rotate for a time to go 180 degrees 90 | ticks = int(angular_duration * rate) 91 | 92 | for t in range(ticks): 93 | self.cmd_vel.publish(move_cmd) 94 | r.sleep() 95 | 96 | # Stop the robot before the next leg 97 | move_cmd = Twist() 98 | self.cmd_vel.publish(move_cmd) 99 | rospy.sleep(1) 100 | 101 | # Stop the robot 102 | self.cmd_vel.publish(Twist()) 103 | 104 | def shutdown(self): 105 | # Always stop the robot when shutting down the node. 106 | rospy.loginfo("Stopping the robot...") 107 | self.cmd_vel.publish(Twist()) 108 | rospy.sleep(1) 109 | 110 | if __name__ == '__main__': 111 | try: 112 | OutAndBack() 113 | except: 114 | rospy.loginfo("Out-and-Back node terminated.") 115 | 116 | -------------------------------------------------------------------------------- /mrobot_bringup/src/mrobot.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "mrobot_bringup/mrobot.h" 3 | 4 | namespace mrobot 5 | { 6 | 7 | const unsigned char ender[2] = {0x0d, 0x0a}; 8 | const unsigned char header[2] = {0x55, 0xaa}; 9 | const int SPEED_INFO = 0xa55a; 10 | const int GET_SPEED = 0xaaaa; 11 | const double ROBOT_RADIUS = 105.00; 12 | const double ROBOT_LENGTH = 210.50; 13 | 14 | boost::asio::io_service iosev; 15 | boost::asio::serial_port sp(iosev, "/dev/ttyUSB0"); 16 | 17 | boost::array odom_pose_covariance = { 18 | {1e-9, 0, 0, 0, 0, 0, 19 | 0, 1e-3, 1e-9, 0, 0, 0, 20 | 0, 0, 1e6, 0, 0, 0, 21 | 0, 0, 0, 1e6, 0, 0, 22 | 0, 0, 0, 0, 1e6, 0, 23 | 0, 0, 0, 0, 0, 1e-9}}; 24 | boost::array odom_twist_covariance = { 25 | {1e-9, 0, 0, 0, 0, 0, 26 | 0, 1e-3, 1e-9, 0, 0, 0, 27 | 0, 0, 1e6, 0, 0, 0, 28 | 0, 0, 0, 1e6, 0, 0, 29 | 0, 0, 0, 0, 1e6, 0, 30 | 0, 0, 0, 0, 0, 1e-9}}; 31 | 32 | union sendData 33 | { 34 | int d; 35 | unsigned char data[4]; 36 | }leftdata, rightdata; 37 | 38 | union checkSum 39 | { 40 | short d; 41 | unsigned char data[1]; 42 | }SendCheckSum, ReceiveCheckSum; 43 | 44 | union receiveHeader 45 | { 46 | int d; 47 | unsigned char data[2]; 48 | }receive_command, receive_header; 49 | 50 | union sendCommand 51 | { 52 | int d; 53 | unsigned char data[2]; 54 | }send_command; 55 | 56 | union odometry 57 | { 58 | float odoemtry_float; 59 | unsigned char odometry_char[4]; 60 | }vel_left, vel_right; 61 | 62 | MRobot::MRobot(): 63 | x_(0.0), y_(0.0), th_(0.0), 64 | vx_(0.0), vy_(0.0), vth_(0.0) 65 | { 66 | } 67 | 68 | MRobot::~MRobot() 69 | { 70 | } 71 | 72 | bool MRobot::init() 73 | { 74 | // 串口连接 75 | sp.set_option(boost::asio::serial_port::baud_rate(115200)); 76 | sp.set_option(boost::asio::serial_port::flow_control(boost::asio::serial_port::flow_control::none)); 77 | sp.set_option(boost::asio::serial_port::parity(boost::asio::serial_port::parity::none)); 78 | sp.set_option(boost::asio::serial_port::stop_bits(boost::asio::serial_port::stop_bits::one)); 79 | sp.set_option(boost::asio::serial_port::character_size(8)); 80 | 81 | ros::Time::init(); 82 | current_time_ = ros::Time::now(); 83 | last_time_ = ros::Time::now(); 84 | 85 | //定义发布消息的名称 86 | pub_ = nh.advertise("odom", 50); 87 | 88 | return true; 89 | } 90 | 91 | bool MRobot::readSpeed() 92 | { 93 | int i, length = 0; 94 | unsigned char checkSum; 95 | unsigned char buf[200]; 96 | 97 | // 读取串口数据 98 | boost::asio::read(sp, boost::asio::buffer(buf)); 99 | ros::Time curr_time; 100 | for (i = 0; i < 2; i++) 101 | receive_header.data[i] = buf[i]; 102 | 103 | // 检查信息头 104 | if (receive_header.data[0] != header[0] || receive_header.data[1] != header[1]) 105 | { 106 | ROS_ERROR("Received message header error!"); 107 | return false; 108 | } 109 | 110 | for (i = 0; i < 2; i++) 111 | receive_command.data[i] = buf[i + 2]; 112 | 113 | length = buf[4]; 114 | checkSum = getCrc8(buf, 5 + length); 115 | 116 | // 检查信息类型 117 | if(receive_command.d != SPEED_INFO) 118 | { 119 | ROS_ERROR("Received command error!"); 120 | return false; 121 | } 122 | 123 | // 检查信息尾 124 | if (buf[6 + length] != ender[0] || buf[6 + length + 1] != ender[1]) 125 | { 126 | ROS_ERROR("Received message header error!"); 127 | return false; 128 | } 129 | 130 | // 检查信息校验值 131 | ReceiveCheckSum.data[0] = buf[5 + length]; 132 | if (checkSum != ReceiveCheckSum.d) 133 | { 134 | ROS_ERROR("Received data check sum error!"); 135 | return false; 136 | } 137 | 138 | // 读取速度值 139 | for(i = 0; i < 4; i++) 140 | { 141 | vel_left.odometry_char[i] = buf[i + 5]; 142 | vel_right.odometry_char[i] = buf[i + 9]; 143 | } 144 | 145 | // 积分计算里程计信息 146 | vx_ = (vel_right.odoemtry_float + vel_left.odoemtry_float) / 2 / 1000; 147 | vth_ = (vel_right.odoemtry_float - vel_left.odoemtry_float) / ROBOT_LENGTH; 148 | 149 | curr_time = ros::Time::now(); 150 | 151 | double dt = (curr_time - last_time_).toSec(); 152 | double delta_x = (vx_ * cos(th_) - vy_ * sin(th_)) * dt; 153 | double delta_y = (vx_ * sin(th_) + vy_ * cos(th_)) * dt; 154 | double delta_th = vth_ * dt; 155 | 156 | x_ += delta_x; 157 | y_ += delta_y; 158 | th_ += delta_th; 159 | last_time_ = curr_time; 160 | 161 | return true; 162 | } 163 | 164 | void MRobot::writeSpeed(double RobotV, double YawRate) 165 | { 166 | unsigned char buf[16] = {0}; 167 | int i, length = 0; 168 | double r = RobotV / YawRate; 169 | 170 | // 计算左右轮期望速度 171 | if(RobotV == 0) 172 | { 173 | leftdata.d = -YawRate * ROBOT_RADIUS; 174 | rightdata.d = YawRate * ROBOT_RADIUS; 175 | } 176 | else if(YawRate == 0) 177 | { 178 | leftdata.d = RobotV; 179 | rightdata.d = RobotV; 180 | } 181 | else 182 | { 183 | leftdata.d = YawRate * (r - ROBOT_RADIUS); 184 | rightdata.d = YawRate * (r + ROBOT_RADIUS); 185 | } 186 | 187 | // 设置消息头 188 | for(i = 0; i < 2; i++) 189 | buf[i] = header[i]; 190 | 191 | // 设置消息类型 192 | send_command.d = GET_SPEED; 193 | for(i = 0; i < 2; i++) 194 | buf[i + 2] = send_command.data[i]; 195 | 196 | // 设置机器人左右轮速度 197 | length = 8; 198 | buf[4] = length; 199 | for(i = 0; i < 4; i++) 200 | { 201 | buf[i + 5] = rightdata.data[i]; 202 | buf[i + 9] = leftdata.data[i]; 203 | } 204 | 205 | // 设置校验值、消息尾 206 | buf[5 + length] = getCrc8(buf, 5 + length); 207 | buf[6 + length] = ender[0]; 208 | buf[6 + length + 1] = ender[1]; 209 | 210 | // 通过串口下发数据 211 | boost::asio::write(sp, boost::asio::buffer(buf)); 212 | } 213 | 214 | bool MRobot::spinOnce(double RobotV, double YawRate) 215 | { 216 | // 下发机器人期望速度 217 | writeSpeed(RobotV, YawRate); 218 | 219 | // 读取机器人实际速度 220 | readSpeed(); 221 | 222 | current_time_ = ros::Time::now(); 223 | // 发布TF 224 | geometry_msgs::TransformStamped odom_trans; 225 | odom_trans.header.stamp = current_time_; 226 | odom_trans.header.frame_id = "odom"; 227 | odom_trans.child_frame_id = "base_footprint"; 228 | 229 | geometry_msgs::Quaternion odom_quat; 230 | odom_quat = tf::createQuaternionMsgFromYaw(th_); 231 | odom_trans.transform.translation.x = x_; 232 | odom_trans.transform.translation.y = y_; 233 | odom_trans.transform.translation.z = 0.0; 234 | odom_trans.transform.rotation = odom_quat; 235 | 236 | odom_broadcaster_.sendTransform(odom_trans); 237 | 238 | // 发布里程计消息 239 | nav_msgs::Odometry msgl; 240 | msgl.header.stamp = current_time_; 241 | msgl.header.frame_id = "odom"; 242 | 243 | msgl.pose.pose.position.x = x_; 244 | msgl.pose.pose.position.y = y_; 245 | msgl.pose.pose.position.z = 0.0; 246 | msgl.pose.pose.orientation = odom_quat; 247 | msgl.pose.covariance = odom_pose_covariance; 248 | 249 | msgl.child_frame_id = "base_footprint"; 250 | msgl.twist.twist.linear.x = vx_; 251 | msgl.twist.twist.linear.y = vy_; 252 | msgl.twist.twist.angular.z = vth_; 253 | msgl.twist.covariance = odom_twist_covariance; 254 | 255 | pub_.publish(msgl); 256 | } 257 | 258 | unsigned char MRobot::getCrc8(unsigned char *ptr, unsigned short len) 259 | { 260 | unsigned char crc; 261 | unsigned char i; 262 | crc = 0; 263 | while(len--) 264 | { 265 | crc ^= *ptr++; 266 | for(i = 0; i < 8; i++) 267 | { 268 | if(crc&0x01) 269 | crc=(crc>>1)^0x8C; 270 | else 271 | crc >>= 1; 272 | } 273 | } 274 | return crc; 275 | } 276 | 277 | } 278 | -------------------------------------------------------------------------------- /mrobot_bringup/src/mrobot_bringup.cpp: -------------------------------------------------------------------------------- 1 | #include "mrobot_bringup/mrobot.h" 2 | 3 | double RobotV_ = 0; 4 | double YawRate_ = 0; 5 | 6 | // 速度控制消息的回调函数 7 | void cmdCallback(const geometry_msgs::Twist& msg) 8 | { 9 | RobotV_ = msg.linear.x * 1000; 10 | YawRate_ = msg.angular.z; 11 | } 12 | 13 | int main(int argc, char** argv) 14 | { 15 | //初始化ROS节点 16 | ros::init(argc, argv, "mrobot_bringup"); 17 | ros::NodeHandle nh; 18 | 19 | //初始化MRobot 20 | mrobot::MRobot robot; 21 | if(!robot.init()) 22 | ROS_ERROR("MRobot initialized failed."); 23 | ROS_INFO("MRobot initialized successful."); 24 | 25 | ros::Subscriber sub = nh.subscribe("cmd_vel", 50, cmdCallback); 26 | 27 | //循环运行 28 | ros::Rate loop_rate(50); 29 | while (ros::ok()) 30 | { 31 | ros::spinOnce(); 32 | 33 | // 机器人控制 34 | robot.spinOnce(RobotV_, YawRate_); 35 | 36 | loop_rate.sleep(); 37 | } 38 | 39 | return 0; 40 | } 41 | 42 | -------------------------------------------------------------------------------- /mrobot_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mrobot_description) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | urdf 9 | xacro 10 | ) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a run_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if you package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES mrobot_description 104 | # CATKIN_DEPENDS urdf xacro 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | # include_directories(include) 115 | include_directories( 116 | ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(mrobot_description 121 | # src/${PROJECT_NAME}/mrobot_description.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(mrobot_description ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | # add_executable(mrobot_description_node src/mrobot_description_node.cpp) 131 | 132 | ## Add cmake target dependencies of the executable 133 | ## same as for the library above 134 | # add_dependencies(mrobot_description_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 135 | 136 | ## Specify libraries to link a library or executable target against 137 | # target_link_libraries(mrobot_description_node 138 | # ${catkin_LIBRARIES} 139 | # ) 140 | 141 | ############# 142 | ## Install ## 143 | ############# 144 | 145 | # all install targets should use catkin DESTINATION variables 146 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 147 | 148 | ## Mark executable scripts (Python etc.) for installation 149 | ## in contrast to setup.py, you can choose the destination 150 | # install(PROGRAMS 151 | # scripts/my_python_script 152 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 153 | # ) 154 | 155 | ## Mark executables and/or libraries for installation 156 | # install(TARGETS mrobot_description mrobot_description_node 157 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 158 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 159 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 160 | # ) 161 | 162 | ## Mark cpp header files for installation 163 | # install(DIRECTORY include/${PROJECT_NAME}/ 164 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 165 | # FILES_MATCHING PATTERN "*.h" 166 | # PATTERN ".svn" EXCLUDE 167 | # ) 168 | 169 | ## Mark other files for installation (e.g. launch and bag files, etc.) 170 | # install(FILES 171 | # # myfile1 172 | # # myfile2 173 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 174 | # ) 175 | 176 | ############# 177 | ## Testing ## 178 | ############# 179 | 180 | ## Add gtest based cpp test target and link libraries 181 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_mrobot_description.cpp) 182 | # if(TARGET ${PROJECT_NAME}-test) 183 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 184 | # endif() 185 | 186 | ## Add folders to be run by python nosetests 187 | # catkin_add_nosetests(test) 188 | -------------------------------------------------------------------------------- /mrobot_description/config/fake_mrobot_arbotix.yaml: -------------------------------------------------------------------------------- 1 | controllers: { 2 | base_controller: {type: diff_controller, base_frame_id: base_footprint, base_width: 0.26, ticks_meter: 4100, Kp: 12, Kd: 12, Ki: 0, Ko: 50, accel_limit: 1.0 } 3 | } 4 | -------------------------------------------------------------------------------- /mrobot_description/config/mrobot.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1 10 | Splitter Ratio: 0.5 11 | Tree Height: 473 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.588679016 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.0299999993 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: true 52 | - Alpha: 1 53 | Class: rviz/RobotModel 54 | Collision Enabled: false 55 | Enabled: true 56 | Links: 57 | All Links Enabled: true 58 | Expand Joint Details: false 59 | Expand Link Details: false 60 | Expand Tree: false 61 | Link Tree Style: Links in Alphabetic Order 62 | base_link: 63 | Alpha: 1 64 | Show Axes: false 65 | Show Trail: false 66 | Value: true 67 | front_castor_link: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | Value: true 72 | left_motor: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | Value: true 77 | left_wheel_link: 78 | Alpha: 1 79 | Show Axes: false 80 | Show Trail: false 81 | Value: true 82 | rear_castor_link: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | Value: true 87 | right_motor: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | Value: true 92 | right_wheel_link: 93 | Alpha: 1 94 | Show Axes: false 95 | Show Trail: false 96 | Value: true 97 | Name: RobotModel 98 | Robot Description: robot_description 99 | TF Prefix: "" 100 | Update Interval: 0 101 | Value: true 102 | Visual Enabled: true 103 | - Class: rviz/TF 104 | Enabled: true 105 | Frame Timeout: 15 106 | Frames: 107 | All Enabled: true 108 | base_link: 109 | Value: true 110 | front_castor_link: 111 | Value: true 112 | left_motor: 113 | Value: true 114 | left_wheel_link: 115 | Value: true 116 | rear_castor_link: 117 | Value: true 118 | right_motor: 119 | Value: true 120 | right_wheel_link: 121 | Value: true 122 | Marker Scale: 0.100000001 123 | Name: TF 124 | Show Arrows: true 125 | Show Axes: true 126 | Show Names: true 127 | Tree: 128 | base_link: 129 | front_castor_link: 130 | {} 131 | left_motor: 132 | left_wheel_link: 133 | {} 134 | rear_castor_link: 135 | {} 136 | right_motor: 137 | right_wheel_link: 138 | {} 139 | Update Interval: 0 140 | Value: true 141 | Enabled: true 142 | Global Options: 143 | Background Color: 48; 48; 48 144 | Fixed Frame: base_link 145 | Frame Rate: 30 146 | Name: root 147 | Tools: 148 | - Class: rviz/Interact 149 | Hide Inactive Objects: true 150 | - Class: rviz/MoveCamera 151 | - Class: rviz/Select 152 | - Class: rviz/FocusCamera 153 | - Class: rviz/Measure 154 | - Class: rviz/SetInitialPose 155 | Topic: /initialpose 156 | - Class: rviz/SetGoal 157 | Topic: /move_base_simple/goal 158 | - Class: rviz/PublishPoint 159 | Single click: true 160 | Topic: /clicked_point 161 | Value: true 162 | Views: 163 | Current: 164 | Class: rviz/Orbit 165 | Distance: 0.571963191 166 | Enable Stereo Rendering: 167 | Stereo Eye Separation: 0.0599999987 168 | Stereo Focal Distance: 1 169 | Swap Stereo Eyes: false 170 | Value: false 171 | Focal Point: 172 | X: -0.00707128178 173 | Y: 0.000214380445 174 | Z: 0.0199916083 175 | Focal Shape Fixed Size: true 176 | Focal Shape Size: 0.0500000007 177 | Name: Current View 178 | Near Clip Distance: 0.00999999978 179 | Pitch: 0.390397906 180 | Target Frame: 181 | Value: Orbit (rviz) 182 | Yaw: 0.455398023 183 | Saved: ~ 184 | Window Geometry: 185 | Displays: 186 | collapsed: false 187 | Height: 754 188 | Hide Left Dock: false 189 | Hide Right Dock: false 190 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000268fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000268000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a70000003efc0100000002fb0000000800540069006d00650100000000000004a70000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003370000026800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 191 | Selection: 192 | collapsed: false 193 | Time: 194 | collapsed: false 195 | Tool Properties: 196 | collapsed: false 197 | Views: 198 | collapsed: false 199 | Width: 1191 200 | X: 276 201 | Y: 84 202 | -------------------------------------------------------------------------------- /mrobot_description/config/mrobot_urdf.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1 10 | Splitter Ratio: 0.5 11 | Tree Height: 473 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.588679016 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.0299999993 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: true 52 | - Alpha: 1 53 | Class: rviz/RobotModel 54 | Collision Enabled: false 55 | Enabled: true 56 | Links: 57 | All Links Enabled: true 58 | Expand Joint Details: false 59 | Expand Link Details: false 60 | Expand Tree: false 61 | Link Tree Style: Links in Alphabetic Order 62 | base_link: 63 | Alpha: 1 64 | Show Axes: false 65 | Show Trail: false 66 | Value: true 67 | front_castor_link: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | Value: true 72 | left_motor: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | Value: true 77 | left_wheel_link: 78 | Alpha: 1 79 | Show Axes: false 80 | Show Trail: false 81 | Value: true 82 | rear_castor_link: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | Value: true 87 | right_motor: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | Value: true 92 | right_wheel_link: 93 | Alpha: 1 94 | Show Axes: false 95 | Show Trail: false 96 | Value: true 97 | Name: RobotModel 98 | Robot Description: robot_description 99 | TF Prefix: "" 100 | Update Interval: 0 101 | Value: true 102 | Visual Enabled: true 103 | - Class: rviz/TF 104 | Enabled: true 105 | Frame Timeout: 15 106 | Frames: 107 | All Enabled: true 108 | base_link: 109 | Value: true 110 | front_castor_link: 111 | Value: true 112 | left_motor: 113 | Value: true 114 | left_wheel_link: 115 | Value: true 116 | rear_castor_link: 117 | Value: true 118 | right_motor: 119 | Value: true 120 | right_wheel_link: 121 | Value: true 122 | Marker Scale: 0.100000001 123 | Name: TF 124 | Show Arrows: true 125 | Show Axes: true 126 | Show Names: true 127 | Tree: 128 | base_link: 129 | front_castor_link: 130 | {} 131 | left_motor: 132 | left_wheel_link: 133 | {} 134 | rear_castor_link: 135 | {} 136 | right_motor: 137 | right_wheel_link: 138 | {} 139 | Update Interval: 0 140 | Value: true 141 | Enabled: true 142 | Global Options: 143 | Background Color: 48; 48; 48 144 | Fixed Frame: base_link 145 | Frame Rate: 30 146 | Name: root 147 | Tools: 148 | - Class: rviz/Interact 149 | Hide Inactive Objects: true 150 | - Class: rviz/MoveCamera 151 | - Class: rviz/Select 152 | - Class: rviz/FocusCamera 153 | - Class: rviz/Measure 154 | - Class: rviz/SetInitialPose 155 | Topic: /initialpose 156 | - Class: rviz/SetGoal 157 | Topic: /move_base_simple/goal 158 | - Class: rviz/PublishPoint 159 | Single click: true 160 | Topic: /clicked_point 161 | Value: true 162 | Views: 163 | Current: 164 | Class: rviz/Orbit 165 | Distance: 0.571963191 166 | Enable Stereo Rendering: 167 | Stereo Eye Separation: 0.0599999987 168 | Stereo Focal Distance: 1 169 | Swap Stereo Eyes: false 170 | Value: false 171 | Focal Point: 172 | X: -0.00707128178 173 | Y: 0.000214380445 174 | Z: 0.0199916083 175 | Focal Shape Fixed Size: true 176 | Focal Shape Size: 0.0500000007 177 | Name: Current View 178 | Near Clip Distance: 0.00999999978 179 | Pitch: 0.390397906 180 | Target Frame: 181 | Value: Orbit (rviz) 182 | Yaw: 0.455398023 183 | Saved: ~ 184 | Window Geometry: 185 | Displays: 186 | collapsed: false 187 | Height: 754 188 | Hide Left Dock: false 189 | Hide Right Dock: false 190 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000268fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000268000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a70000003efc0100000002fb0000000800540069006d00650100000000000004a70000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003370000026800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 191 | Selection: 192 | collapsed: false 193 | Time: 194 | collapsed: false 195 | Tool Properties: 196 | collapsed: false 197 | Views: 198 | collapsed: false 199 | Width: 1191 200 | X: 276 201 | Y: 84 202 | -------------------------------------------------------------------------------- /mrobot_description/launch/arbotix_mrobot_with_camera.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /mrobot_description/launch/arbotix_mrobot_with_kinect.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /mrobot_description/launch/arbotix_mrobot_with_laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /mrobot_description/launch/display_mrobot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /mrobot_description/launch/display_mrobot_chassis_urdf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /mrobot_description/launch/display_mrobot_with_camera.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /mrobot_description/launch/display_mrobot_with_kinect.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /mrobot_description/launch/display_mrobot_with_laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /mrobot_description/meshes/kinect.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/1027243334/robot_mrobot/f275f19ffb932d76f7d481043768b2ce689125f2/mrobot_description/meshes/kinect.jpg -------------------------------------------------------------------------------- /mrobot_description/meshes/kinect.tga: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/1027243334/robot_mrobot/f275f19ffb932d76f7d481043768b2ce689125f2/mrobot_description/meshes/kinect.tga -------------------------------------------------------------------------------- /mrobot_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrobot_description 4 | 0.0.0 5 | The mrobot_description package 6 | 7 | 8 | 9 | 10 | hcx 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | urdf 44 | xacro 45 | urdf 46 | xacro 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /mrobot_description/urdf/camera.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /mrobot_description/urdf/kinect.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /mrobot_description/urdf/mrobot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /mrobot_description/urdf/mrobot_chassis.gv: -------------------------------------------------------------------------------- 1 | digraph G { 2 | node [shape=box]; 3 | "base_link" [label="base_link"]; 4 | "left_motor" [label="left_motor"]; 5 | "left_wheel_link" [label="left_wheel_link"]; 6 | "right_motor" [label="right_motor"]; 7 | "right_wheel_link" [label="right_wheel_link"]; 8 | "front_caster_link" [label="front_caster_link"]; 9 | node [shape=ellipse, color=blue, fontcolor=blue]; 10 | "base_link" -> "base_left_motor_joint" [label="xyz: -0.055 0.075 0 \nrpy: 0 -0 0"] 11 | "base_left_motor_joint" -> "left_motor" 12 | "left_motor" -> "left_wheel_joint" [label="xyz: 0 0.0485 0 \nrpy: 0 -0 0"] 13 | "left_wheel_joint" -> "left_wheel_link" 14 | "base_link" -> "base_right_motor_joint" [label="xyz: -0.055 -0.075 0 \nrpy: 0 -0 0"] 15 | "base_right_motor_joint" -> "right_motor" 16 | "right_motor" -> "right_wheel_joint" [label="xyz: 0 -0.0485 0 \nrpy: 0 -0 0"] 17 | "right_wheel_joint" -> "right_wheel_link" 18 | "base_link" -> "front_caster_joint" [label="xyz: 0.1135 0 -0.0165 \nrpy: 0 -0 0"] 19 | "front_caster_joint" -> "front_caster_link" 20 | } 21 | -------------------------------------------------------------------------------- /mrobot_description/urdf/mrobot_chassis.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/1027243334/robot_mrobot/f275f19ffb932d76f7d481043768b2ce689125f2/mrobot_description/urdf/mrobot_chassis.pdf -------------------------------------------------------------------------------- /mrobot_description/urdf/mrobot_chassis.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | -------------------------------------------------------------------------------- /mrobot_description/urdf/mrobot_with_camera.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /mrobot_description/urdf/mrobot_with_kinect.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /mrobot_description/urdf/mrobot_with_rplidar.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /mrobot_description/urdf/rplidar.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /mrobot_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mrobot_gazebo) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | gazebo_plugins 9 | gazebo_ros 10 | gazebo_ros_control 11 | roscpp 12 | rospy 13 | ) 14 | 15 | ## System dependencies are found with CMake's conventions 16 | # find_package(Boost REQUIRED COMPONENTS system) 17 | 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | # catkin_python_setup() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | 28 | ## To declare and build messages, services or actions from within this 29 | ## package, follow these steps: 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 31 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 32 | ## * In the file package.xml: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 36 | ## but can be declared for certainty nonetheless: 37 | ## * add a run_depend tag for "message_runtime" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "message_generation" and every package in MSG_DEP_SET to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * add "message_runtime" and every package in MSG_DEP_SET to 42 | ## catkin_package(CATKIN_DEPENDS ...) 43 | ## * uncomment the add_*_files sections below as needed 44 | ## and list every .msg/.srv/.action file to be processed 45 | ## * uncomment the generate_messages entry below 46 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 47 | 48 | ## Generate messages in the 'msg' folder 49 | # add_message_files( 50 | # FILES 51 | # Message1.msg 52 | # Message2.msg 53 | # ) 54 | 55 | ## Generate services in the 'srv' folder 56 | # add_service_files( 57 | # FILES 58 | # Service1.srv 59 | # Service2.srv 60 | # ) 61 | 62 | ## Generate actions in the 'action' folder 63 | # add_action_files( 64 | # FILES 65 | # Action1.action 66 | # Action2.action 67 | # ) 68 | 69 | ## Generate added messages and services with any dependencies listed here 70 | # generate_messages( 71 | # DEPENDENCIES 72 | # std_msgs # Or other packages containing msgs 73 | # ) 74 | 75 | ################################################ 76 | ## Declare ROS dynamic reconfigure parameters ## 77 | ################################################ 78 | 79 | ## To declare and build dynamic reconfigure parameters within this 80 | ## package, follow these steps: 81 | ## * In the file package.xml: 82 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 83 | ## * In this file (CMakeLists.txt): 84 | ## * add "dynamic_reconfigure" to 85 | ## find_package(catkin REQUIRED COMPONENTS ...) 86 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 87 | ## and list every .cfg file to be processed 88 | 89 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 90 | # generate_dynamic_reconfigure_options( 91 | # cfg/DynReconf1.cfg 92 | # cfg/DynReconf2.cfg 93 | # ) 94 | 95 | ################################### 96 | ## catkin specific configuration ## 97 | ################################### 98 | ## The catkin_package macro generates cmake config files for your package 99 | ## Declare things to be passed to dependent projects 100 | ## INCLUDE_DIRS: uncomment this if you package contains header files 101 | ## LIBRARIES: libraries you create in this project that dependent projects also need 102 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 103 | ## DEPENDS: system dependencies of this project that dependent projects also need 104 | catkin_package( 105 | # INCLUDE_DIRS include 106 | # LIBRARIES mrobot_gazebo 107 | # CATKIN_DEPENDS gazebo_plugins gazebo_ros gazebo_ros_control roscpp rospy 108 | # DEPENDS system_lib 109 | ) 110 | 111 | ########### 112 | ## Build ## 113 | ########### 114 | 115 | ## Specify additional locations of header files 116 | ## Your package locations should be listed before other locations 117 | # include_directories(include) 118 | include_directories( 119 | ${catkin_INCLUDE_DIRS} 120 | ) 121 | 122 | ## Declare a C++ library 123 | # add_library(mrobot_gazebo 124 | # src/${PROJECT_NAME}/mrobot_gazebo.cpp 125 | # ) 126 | 127 | ## Add cmake target dependencies of the library 128 | ## as an example, code may need to be generated before libraries 129 | ## either from message generation or dynamic reconfigure 130 | # add_dependencies(mrobot_gazebo ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 131 | 132 | ## Declare a C++ executable 133 | # add_executable(mrobot_gazebo_node src/mrobot_gazebo_node.cpp) 134 | 135 | ## Add cmake target dependencies of the executable 136 | ## same as for the library above 137 | # add_dependencies(mrobot_gazebo_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 138 | 139 | ## Specify libraries to link a library or executable target against 140 | # target_link_libraries(mrobot_gazebo_node 141 | # ${catkin_LIBRARIES} 142 | # ) 143 | 144 | ############# 145 | ## Install ## 146 | ############# 147 | 148 | # all install targets should use catkin DESTINATION variables 149 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 150 | 151 | ## Mark executable scripts (Python etc.) for installation 152 | ## in contrast to setup.py, you can choose the destination 153 | # install(PROGRAMS 154 | # scripts/my_python_script 155 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 156 | # ) 157 | 158 | ## Mark executables and/or libraries for installation 159 | # install(TARGETS mrobot_gazebo mrobot_gazebo_node 160 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 161 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 162 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 163 | # ) 164 | 165 | ## Mark cpp header files for installation 166 | # install(DIRECTORY include/${PROJECT_NAME}/ 167 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 168 | # FILES_MATCHING PATTERN "*.h" 169 | # PATTERN ".svn" EXCLUDE 170 | # ) 171 | 172 | ## Mark other files for installation (e.g. launch and bag files, etc.) 173 | # install(FILES 174 | # # myfile1 175 | # # myfile2 176 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 177 | # ) 178 | 179 | ############# 180 | ## Testing ## 181 | ############# 182 | 183 | ## Add gtest based cpp test target and link libraries 184 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_mrobot_gazebo.cpp) 185 | # if(TARGET ${PROJECT_NAME}-test) 186 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 187 | # endif() 188 | 189 | ## Add folders to be run by python nosetests 190 | # catkin_add_nosetests(test) 191 | -------------------------------------------------------------------------------- /mrobot_gazebo/launch/mrobot_kinect_nav_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /mrobot_gazebo/launch/mrobot_laser_nav_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /mrobot_gazebo/launch/view_mrobot_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /mrobot_gazebo/launch/view_mrobot_with_camera_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /mrobot_gazebo/launch/view_mrobot_with_kinect_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /mrobot_gazebo/launch/view_mrobot_with_laser_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /mrobot_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrobot_gazebo 4 | 0.0.0 5 | The mrobot_gazebo package 6 | 7 | 8 | 9 | 10 | hcx 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | gazebo_plugins 44 | gazebo_ros 45 | gazebo_ros_control 46 | roscpp 47 | rospy 48 | gazebo_plugins 49 | gazebo_ros 50 | gazebo_ros_control 51 | roscpp 52 | rospy 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /mrobot_gazebo/urdf/camera.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | Gazebo/Black 32 | 33 | 34 | 35 | 36 | 30.0 37 | 38 | 1.3962634 39 | 40 | 1280 41 | 720 42 | R8G8B8 43 | 44 | 45 | 0.02 46 | 300 47 | 48 | 49 | gaussian 50 | 0.0 51 | 0.007 52 | 53 | 54 | 55 | true 56 | 0.0 57 | /camera 58 | image_raw 59 | camera_info 60 | camera_link 61 | 0.07 62 | 0.0 63 | 0.0 64 | 0.0 65 | 0.0 66 | 0.0 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /mrobot_gazebo/urdf/kinect.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | true 33 | 20.0 34 | 35 | ${60.0*M_PI/180.0} 36 | 37 | R8G8B8 38 | 640 39 | 480 40 | 41 | 42 | 0.05 43 | 8.0 44 | 45 | 46 | 47 | ${prefix} 48 | true 49 | 10 50 | rgb/image_raw 51 | depth/image_raw 52 | depth/points 53 | rgb/camera_info 54 | depth/camera_info 55 | ${prefix}_frame_optical 56 | 0.1 57 | 0.0 58 | 0.0 59 | 0.0 60 | 0.0 61 | 0.0 62 | 0.4 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /mrobot_gazebo/urdf/mrobot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /mrobot_gazebo/urdf/mrobot_with_camera.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /mrobot_gazebo/urdf/mrobot_with_kinect.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /mrobot_gazebo/urdf/mrobot_with_rplidar.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /mrobot_gazebo/urdf/rplidar.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | Gazebo/Black 32 | 33 | 34 | 35 | 36 | 0 0 0 0 0 0 37 | false 38 | 5.5 39 | 40 | 41 | 42 | 360 43 | 1 44 | -3 45 | 3 46 | 47 | 48 | 49 | 0.10 50 | 6.0 51 | 0.01 52 | 53 | 54 | gaussian 55 | 0.0 56 | 0.01 57 | 58 | 59 | 60 | /scan 61 | laser_link 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /mrobot_navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mrobot_navigation) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | geometry_msgs 9 | move_base_msgs 10 | roscpp 11 | rospy 12 | tf 13 | visualization_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a run_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # geometry_msgs# move_base_msgs# visualization_msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if you package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES mrobot_navigation 108 | # CATKIN_DEPENDS geometry_msgs move_base_msgs roscpp rospy tf visualization_msgs 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | # include_directories(include) 119 | include_directories( 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(mrobot_navigation 125 | # src/${PROJECT_NAME}/mrobot_navigation.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(mrobot_navigation ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | # add_executable(mrobot_navigation_node src/mrobot_navigation_node.cpp) 135 | 136 | ## Add cmake target dependencies of the executable 137 | ## same as for the library above 138 | # add_dependencies(mrobot_navigation_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 139 | 140 | ## Specify libraries to link a library or executable target against 141 | # target_link_libraries(mrobot_navigation_node 142 | # ${catkin_LIBRARIES} 143 | # ) 144 | 145 | ############# 146 | ## Install ## 147 | ############# 148 | 149 | # all install targets should use catkin DESTINATION variables 150 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 151 | 152 | ## Mark executable scripts (Python etc.) for installation 153 | ## in contrast to setup.py, you can choose the destination 154 | # install(PROGRAMS 155 | # scripts/my_python_script 156 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 157 | # ) 158 | 159 | ## Mark executables and/or libraries for installation 160 | # install(TARGETS mrobot_navigation mrobot_navigation_node 161 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 162 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 163 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 164 | # ) 165 | 166 | ## Mark cpp header files for installation 167 | # install(DIRECTORY include/${PROJECT_NAME}/ 168 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 169 | # FILES_MATCHING PATTERN "*.h" 170 | # PATTERN ".svn" EXCLUDE 171 | # ) 172 | 173 | ## Mark other files for installation (e.g. launch and bag files, etc.) 174 | # install(FILES 175 | # # myfile1 176 | # # myfile2 177 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 178 | # ) 179 | 180 | ############# 181 | ## Testing ## 182 | ############# 183 | 184 | ## Add gtest based cpp test target and link libraries 185 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_mrobot_navigation.cpp) 186 | # if(TARGET ${PROJECT_NAME}-test) 187 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 188 | # endif() 189 | 190 | ## Add folders to be run by python nosetests 191 | # catkin_add_nosetests(test) 192 | -------------------------------------------------------------------------------- /mrobot_navigation/config/fake/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | controller_frequency: 3.0 2 | recovery_behavior_enabled: false 3 | clearing_rotation_allowed: false 4 | 5 | TrajectoryPlannerROS: 6 | max_vel_x: 0.5 7 | min_vel_x: 0.1 8 | max_vel_y: 0.0 # zero for a differential drive robot 9 | min_vel_y: 0.0 10 | max_vel_theta: 1.0 11 | min_vel_theta: -1.0 12 | min_in_place_vel_theta: 0.4 13 | escape_vel: -0.1 14 | acc_lim_x: 1.5 15 | acc_lim_y: 0.0 # zero for a differential drive robot 16 | acc_lim_theta: 1.2 17 | 18 | holonomic_robot: false 19 | yaw_goal_tolerance: 0.1 # about 6 degrees 20 | xy_goal_tolerance: 0.05 # 5 cm 21 | latch_xy_goal_tolerance: false 22 | pdist_scale: 0.4 23 | gdist_scale: 0.8 24 | meter_scoring: true 25 | 26 | heading_lookahead: 0.325 27 | heading_scoring: false 28 | heading_scoring_timestep: 0.8 29 | occdist_scale: 0.05 30 | oscillation_reset_dist: 0.05 31 | publish_cost_grid_pc: false 32 | prune_plan: true 33 | 34 | sim_time: 1.0 35 | sim_granularity: 0.05 36 | angular_sim_granularity: 0.1 37 | vx_samples: 8 38 | vy_samples: 0 # zero for a differential drive robot 39 | vtheta_samples: 20 40 | dwa: true 41 | simple_attractor: false 42 | -------------------------------------------------------------------------------- /mrobot_navigation/config/fake/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 2.5 2 | raytrace_range: 3.0 3 | #footprint: [[0.175, 0.175], [0.175, -0.175], [-0.175, -0.175], [-0.175, 0.175]] 4 | #footprint_inflation: 0.01 5 | robot_radius: 0.175 6 | inflation_radius: 0.1 7 | max_obstacle_height: 0.6 8 | min_obstacle_height: 0.0 9 | observation_sources: scan 10 | scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0} 11 | -------------------------------------------------------------------------------- /mrobot_navigation/config/fake/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | robot_base_frame: base_footprint 4 | update_frequency: 1.0 5 | publish_frequency: 1.0 6 | static_map: true 7 | rolling_window: false 8 | resolution: 0.01 9 | transform_tolerance: 1.0 10 | map_type: costmap 11 | -------------------------------------------------------------------------------- /mrobot_navigation/config/fake/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: map 3 | robot_base_frame: base_footprint 4 | update_frequency: 3.0 5 | publish_frequency: 1.0 6 | static_map: true 7 | rolling_window: false 8 | width: 6.0 9 | height: 6.0 10 | resolution: 0.01 11 | transform_tolerance: 1.0 12 | -------------------------------------------------------------------------------- /mrobot_navigation/config/mrobot/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | controller_frequency: 3.0 2 | recovery_behavior_enabled: false 3 | clearing_rotation_allowed: false 4 | 5 | TrajectoryPlannerROS: 6 | max_vel_x: 0.3 7 | min_vel_x: 0.05 8 | max_vel_y: 0.0 # zero for a differential drive robot 9 | min_vel_y: 0.0 10 | min_in_place_vel_theta: 0.5 11 | escape_vel: -0.1 12 | acc_lim_x: 2.5 13 | acc_lim_y: 0.0 # zero for a differential drive robot 14 | acc_lim_theta: 3.2 15 | 16 | holonomic_robot: false 17 | yaw_goal_tolerance: 0.1 # about 6 degrees 18 | xy_goal_tolerance: 0.1 # 10 cm 19 | latch_xy_goal_tolerance: false 20 | pdist_scale: 0.9 21 | gdist_scale: 0.6 22 | meter_scoring: true 23 | 24 | heading_lookahead: 0.325 25 | heading_scoring: false 26 | heading_scoring_timestep: 0.8 27 | occdist_scale: 0.1 28 | oscillation_reset_dist: 0.05 29 | publish_cost_grid_pc: false 30 | prune_plan: true 31 | 32 | sim_time: 1.0 33 | sim_granularity: 0.025 34 | angular_sim_granularity: 0.025 35 | vx_samples: 8 36 | vy_samples: 0 # zero for a differential drive robot 37 | vtheta_samples: 20 38 | dwa: true 39 | simple_attractor: false 40 | -------------------------------------------------------------------------------- /mrobot_navigation/config/mrobot/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 2.5 2 | raytrace_range: 3.0 3 | robot_radius: 0.165 4 | inflation_radius: 0.1 5 | max_obstacle_height: 0.6 6 | min_obstacle_height: 0.0 7 | observation_sources: scan 8 | scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0} 9 | -------------------------------------------------------------------------------- /mrobot_navigation/config/mrobot/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_footprint 4 | update_frequency: 1.0 5 | publish_frequency: 0 6 | static_map: true 7 | rolling_window: false 8 | resolution: 0.01 9 | transform_tolerance: 1.0 10 | map_type: costmap 11 | -------------------------------------------------------------------------------- /mrobot_navigation/config/mrobot/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_footprint 4 | update_frequency: 3.0 5 | publish_frequency: 1.0 6 | static_map: false 7 | rolling_window: true 8 | width: 6.0 9 | height: 6.0 10 | resolution: 0.01 11 | transform_tolerance: 1.0 12 | map_type: costmap 13 | -------------------------------------------------------------------------------- /mrobot_navigation/config/rplidar.lua: -------------------------------------------------------------------------------- 1 | -- Copyright 2016 The Cartographer Authors 2 | -- 3 | -- Licensed under the Apache License, Version 2.0 (the "License"); 4 | -- you may not use this file except in compliance with the License. 5 | -- You may obtain a copy of the License at 6 | -- 7 | -- http://www.apache.org/licenses/LICENSE-2.0 8 | -- 9 | -- Unless required by applicable law or agreed to in writing, software 10 | -- distributed under the License is distributed on an "AS IS" BASIS, 11 | -- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | -- See the License for the specific language governing permissions and 13 | -- limitations under the License. 14 | 15 | -- 请复制该文件到cartographer_ros/cartographer_ros/configuration_files中编译 16 | include "map_builder.lua" 17 | include "trajectory_builder.lua" 18 | 19 | options = { 20 | map_builder = MAP_BUILDER, 21 | trajectory_builder = TRAJECTORY_BUILDER, 22 | map_frame = "map", 23 | tracking_frame = "laser_link", 24 | published_frame = "laser_link", 25 | odom_frame = "odom", 26 | provide_odom_frame = true, 27 | use_odometry = false, 28 | num_laser_scans = 1, 29 | num_multi_echo_laser_scans = 0, 30 | num_subdivisions_per_laser_scan = 1, 31 | num_point_clouds = 0, 32 | lookup_transform_timeout_sec = 0.2, 33 | submap_publish_period_sec = 0.3, 34 | pose_publish_period_sec = 5e-3, 35 | trajectory_publish_period_sec = 30e-3, 36 | rangefinder_sampling_ratio = 1., 37 | odometry_sampling_ratio = 1., 38 | imu_sampling_ratio = 1., 39 | } 40 | 41 | MAP_BUILDER.use_trajectory_builder_2d = true 42 | 43 | TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35 44 | TRAJECTORY_BUILDER_2D.min_range = 0.3 45 | TRAJECTORY_BUILDER_2D.max_range = 8. 46 | TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1. 47 | TRAJECTORY_BUILDER_2D.use_imu_data = false 48 | TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 49 | TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1 50 | TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10. 51 | TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1 52 | 53 | SPARSE_POSE_GRAPH.optimization_problem.huber_scale = 1e2 54 | SPARSE_POSE_GRAPH.optimize_every_n_scans = 35 55 | SPARSE_POSE_GRAPH.constraint_builder.min_score = 0.65 56 | 57 | return options 58 | -------------------------------------------------------------------------------- /mrobot_navigation/launch/amcl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /mrobot_navigation/launch/cartographer_demo_rplidar.launch: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 27 | 28 | 29 | 30 | 32 | 33 | -------------------------------------------------------------------------------- /mrobot_navigation/launch/exploring_slam_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /mrobot_navigation/launch/fake_move_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /mrobot_navigation/launch/fake_nav_cloister_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /mrobot_navigation/launch/fake_nav_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /mrobot_navigation/launch/gmapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /mrobot_navigation/launch/gmapping_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /mrobot_navigation/launch/hector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /mrobot_navigation/launch/hector_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /mrobot_navigation/launch/move_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /mrobot_navigation/launch/nav_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /mrobot_navigation/maps/blank_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/1027243334/robot_mrobot/f275f19ffb932d76f7d481043768b2ce689125f2/mrobot_navigation/maps/blank_map.pgm -------------------------------------------------------------------------------- /mrobot_navigation/maps/blank_map.yaml: -------------------------------------------------------------------------------- 1 | image: blank_map.pgm 2 | resolution: 0.01 3 | origin: [-2.5, -2.5, 0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.196 # Taken from the Willow Garage map in the turtlebot_navigation package 6 | negate: 0 7 | -------------------------------------------------------------------------------- /mrobot_navigation/maps/blank_map_with_obstacle.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/1027243334/robot_mrobot/f275f19ffb932d76f7d481043768b2ce689125f2/mrobot_navigation/maps/blank_map_with_obstacle.pgm -------------------------------------------------------------------------------- /mrobot_navigation/maps/blank_map_with_obstacle.yaml: -------------------------------------------------------------------------------- 1 | image: blank_map_with_obstacle.pgm 2 | resolution: 0.01 3 | origin: [-2.5, -2.5, 0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.195 # Taken from the Willow Garage map in the turtlebot_navigation package 6 | negate: 0 7 | -------------------------------------------------------------------------------- /mrobot_navigation/maps/cloister_gmapping.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/1027243334/robot_mrobot/f275f19ffb932d76f7d481043768b2ce689125f2/mrobot_navigation/maps/cloister_gmapping.pgm -------------------------------------------------------------------------------- /mrobot_navigation/maps/cloister_gmapping.yaml: -------------------------------------------------------------------------------- 1 | image: cloister_gmapping.pgm 2 | resolution: 0.050000 3 | origin: [-15.400000, -12.200000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /mrobot_navigation/maps/cloister_hector.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/1027243334/robot_mrobot/f275f19ffb932d76f7d481043768b2ce689125f2/mrobot_navigation/maps/cloister_hector.pgm -------------------------------------------------------------------------------- /mrobot_navigation/maps/cloister_hector.yaml: -------------------------------------------------------------------------------- 1 | image: cloister_hector.pgm 2 | resolution: 0.050000 3 | origin: [-51.224998, -51.224998, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /mrobot_navigation/maps/gmapping_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/1027243334/robot_mrobot/f275f19ffb932d76f7d481043768b2ce689125f2/mrobot_navigation/maps/gmapping_map.pgm -------------------------------------------------------------------------------- /mrobot_navigation/maps/gmapping_map.yaml: -------------------------------------------------------------------------------- 1 | image: gmapping_map.pgm 2 | resolution: 0.050000 3 | origin: [-12.200000, -13.500000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /mrobot_navigation/maps/hector_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/1027243334/robot_mrobot/f275f19ffb932d76f7d481043768b2ce689125f2/mrobot_navigation/maps/hector_map.pgm -------------------------------------------------------------------------------- /mrobot_navigation/maps/hector_map.yaml: -------------------------------------------------------------------------------- 1 | image: hector_map.pgm 2 | resolution: 0.050000 3 | origin: [-51.224998, -51.224998, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /mrobot_navigation/maps/room.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/1027243334/robot_mrobot/f275f19ffb932d76f7d481043768b2ce689125f2/mrobot_navigation/maps/room.pgm -------------------------------------------------------------------------------- /mrobot_navigation/maps/room.yaml: -------------------------------------------------------------------------------- 1 | image: room.pgm 2 | resolution: 0.010000 3 | origin: [-11.560000, -12.520000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /mrobot_navigation/maps/test_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/1027243334/robot_mrobot/f275f19ffb932d76f7d481043768b2ce689125f2/mrobot_navigation/maps/test_map.pgm -------------------------------------------------------------------------------- /mrobot_navigation/maps/test_map.yaml: -------------------------------------------------------------------------------- 1 | image: test_map.pgm 2 | resolution: 0.050000 3 | origin: [-13.800000, -12.200000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.9 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /mrobot_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrobot_navigation 4 | 0.0.0 5 | The mrobot_navigation package 6 | 7 | 8 | 9 | 10 | hcx 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | geometry_msgs 44 | move_base_msgs 45 | roscpp 46 | rospy 47 | tf 48 | visualization_msgs 49 | geometry_msgs 50 | move_base_msgs 51 | roscpp 52 | rospy 53 | tf 54 | visualization_msgs 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /mrobot_navigation/rviz/demo_2d.rviz: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | Panels: 16 | - Class: rviz/Displays 17 | Help Height: 78 18 | Name: Displays 19 | Property Tree Widget: 20 | Expanded: 21 | - /Submaps1 22 | - /PointCloud21 23 | Splitter Ratio: 0.600671 24 | Tree Height: 821 25 | - Class: rviz/Selection 26 | Name: Selection 27 | - Class: rviz/Tool Properties 28 | Expanded: 29 | - /2D Pose Estimate1 30 | - /2D Nav Goal1 31 | - /Publish Point1 32 | Name: Tool Properties 33 | Splitter Ratio: 0.588679 34 | - Class: rviz/Views 35 | Expanded: 36 | - /Current View1 37 | Name: Views 38 | Splitter Ratio: 0.5 39 | - Class: rviz/Time 40 | Experimental: false 41 | Name: Time 42 | SyncMode: 0 43 | SyncSource: PointCloud2 44 | Visualization Manager: 45 | Class: "" 46 | Displays: 47 | - Alpha: 0.5 48 | Cell Size: 1 49 | Class: rviz/Grid 50 | Color: 160; 160; 164 51 | Enabled: true 52 | Line Style: 53 | Line Width: 0.03 54 | Value: Lines 55 | Name: Grid 56 | Normal Cell Count: 0 57 | Offset: 58 | X: 0 59 | Y: 0 60 | Z: 0 61 | Plane: XY 62 | Plane Cell Count: 100 63 | Reference Frame: 64 | Value: true 65 | - Class: rviz/TF 66 | Enabled: true 67 | Frame Timeout: 15 68 | Frames: 69 | All Enabled: true 70 | base_link: 71 | Value: true 72 | horizontal_laser_link: 73 | Value: true 74 | imu_link: 75 | Value: true 76 | map: 77 | Value: true 78 | odom: 79 | Value: true 80 | vertical_laser_link: 81 | Value: true 82 | Marker Scale: 1 83 | Name: TF 84 | Show Arrows: true 85 | Show Axes: true 86 | Show Names: true 87 | Tree: 88 | map: 89 | odom: 90 | base_link: 91 | horizontal_laser_link: 92 | {} 93 | imu_link: 94 | {} 95 | vertical_laser_link: 96 | {} 97 | Update Interval: 0 98 | Value: true 99 | - Alpha: 1 100 | Class: rviz/RobotModel 101 | Collision Enabled: false 102 | Enabled: true 103 | Links: 104 | All Links Enabled: true 105 | Expand Joint Details: false 106 | Expand Link Details: false 107 | Expand Tree: false 108 | Link Tree Style: Links in Alphabetic Order 109 | base_link: 110 | Alpha: 1 111 | Show Axes: false 112 | Show Trail: false 113 | horizontal_laser_link: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | Value: true 118 | imu_link: 119 | Alpha: 1 120 | Show Axes: false 121 | Show Trail: false 122 | Value: true 123 | vertical_laser_link: 124 | Alpha: 1 125 | Show Axes: false 126 | Show Trail: false 127 | Value: true 128 | Name: RobotModel 129 | Robot Description: robot_description 130 | TF Prefix: "" 131 | Update Interval: 0 132 | Value: true 133 | Visual Enabled: true 134 | - Class: Submaps 135 | Enabled: true 136 | Name: Submaps 137 | Submap query service: /submap_query 138 | Topic: /submap_list 139 | Tracking frame: base_link 140 | Unreliable: false 141 | Value: true 142 | - Alpha: 1 143 | Autocompute Intensity Bounds: true 144 | Autocompute Value Bounds: 145 | Max Value: 10 146 | Min Value: -10 147 | Value: true 148 | Axis: Z 149 | Channel Name: intensity 150 | Class: rviz/PointCloud2 151 | Color: 0; 255; 0 152 | Color Transformer: FlatColor 153 | Decay Time: 0 154 | Enabled: true 155 | Invert Rainbow: false 156 | Max Color: 255; 255; 255 157 | Max Intensity: 4096 158 | Min Color: 0; 0; 0 159 | Min Intensity: 0 160 | Name: PointCloud2 161 | Position Transformer: XYZ 162 | Queue Size: 10 163 | Selectable: true 164 | Size (Pixels): 3 165 | Size (m): 0.05 166 | Style: Flat Squares 167 | Topic: /scan_matched_points2 168 | Unreliable: false 169 | Use Fixed Frame: true 170 | Use rainbow: true 171 | Value: true 172 | - Class: rviz/MarkerArray 173 | Enabled: true 174 | Marker Topic: /trajectory_node_list 175 | Name: MarkerArray 176 | Namespaces: 177 | "": true 178 | Queue Size: 100 179 | Value: true 180 | Enabled: true 181 | Global Options: 182 | Background Color: 100; 100; 100 183 | Fixed Frame: map 184 | Frame Rate: 30 185 | Name: root 186 | Tools: 187 | - Class: rviz/Interact 188 | Hide Inactive Objects: true 189 | - Class: rviz/MoveCamera 190 | - Class: rviz/Select 191 | - Class: rviz/FocusCamera 192 | - Class: rviz/Measure 193 | - Class: rviz/SetInitialPose 194 | Topic: /initialpose 195 | - Class: rviz/SetGoal 196 | Topic: /move_base_simple/goal 197 | - Class: rviz/PublishPoint 198 | Single click: true 199 | Topic: /clicked_point 200 | Value: true 201 | Views: 202 | Current: 203 | Angle: 0 204 | Class: rviz/TopDownOrtho 205 | Enable Stereo Rendering: 206 | Stereo Eye Separation: 0.06 207 | Stereo Focal Distance: 1 208 | Swap Stereo Eyes: false 209 | Value: false 210 | Name: Current View 211 | Near Clip Distance: 0.01 212 | Scale: 10 213 | Target Frame: 214 | Value: TopDownOrtho (rviz) 215 | X: 0 216 | Y: 0 217 | Saved: ~ 218 | Window Geometry: 219 | Displays: 220 | collapsed: false 221 | Height: 1123 222 | Hide Left Dock: false 223 | Hide Right Dock: false 224 | QMainWindow State: 000000ff00000000fd0000000400000000000001c5000003c0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000003c0000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003c0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000041000003c0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006501000000000000077e000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000049e000003c000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 225 | Selection: 226 | collapsed: false 227 | Time: 228 | collapsed: false 229 | Tool Properties: 230 | collapsed: false 231 | Views: 232 | collapsed: false 233 | Width: 1918 234 | X: 0 235 | Y: 24 236 | -------------------------------------------------------------------------------- /mrobot_navigation/scripts/exploring_slam.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import roslib; 5 | import rospy 6 | import actionlib 7 | from actionlib_msgs.msg import * 8 | from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist 9 | from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal 10 | from random import sample 11 | from math import pow, sqrt 12 | 13 | class NavTest(): 14 | def __init__(self): 15 | rospy.init_node('exploring_slam', anonymous=True) 16 | rospy.on_shutdown(self.shutdown) 17 | 18 | # 在每个目标位置暂停的时间 (单位：s) 19 | self.rest_time = rospy.get_param("~rest_time", 2) 20 | 21 | # 是否仿真？ 22 | self.fake_test = rospy.get_param("~fake_test", True) 23 | 24 | # 到达目标的状态 25 | goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED', 26 | 'SUCCEEDED', 'ABORTED', 'REJECTED', 27 | 'PREEMPTING', 'RECALLING', 'RECALLED', 28 | 'LOST'] 29 | 30 | # 设置目标点的位置 31 | # 在rviz中点击 2D Nav Goal 按键，然后单击地图中一点 32 | # 在终端中就会看到该点的坐标信息 33 | locations = dict() 34 | 35 | locations['1'] = Pose(Point(4.589, -0.376, 0.000), Quaternion(0.000, 0.000, -0.447, 0.894)) 36 | locations['2'] = Pose(Point(4.231, -6.050, 0.000), Quaternion(0.000, 0.000, -0.847, 0.532)) 37 | locations['3'] = Pose(Point(-0.674, -5.244, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000)) 38 | locations['4'] = Pose(Point(-5.543, -4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764)) 39 | locations['5'] = Pose(Point(-4.701, -0.590, 0.000), Quaternion(0.000, 0.000, 0.340, 0.940)) 40 | locations['6'] = Pose(Point(2.924, 0.018, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000)) 41 | 42 | # 发布控制机器人的消息 43 | self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) 44 | 45 | # 订阅move_base服务器的消息 46 | self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) 47 | 48 | rospy.loginfo("Waiting for move_base action server...") 49 | 50 | # 60s等待时间限制 51 | self.move_base.wait_for_server(rospy.Duration(60)) 52 | rospy.loginfo("Connected to move base server") 53 | 54 | # 保存机器人的在rviz中的初始位置 55 | initial_pose = PoseWithCovarianceStamped() 56 | 57 | # 保存成功率、运行时间、和距离的变量 58 | n_locations = len(locations) 59 | n_goals = 0 60 | n_successes = 0 61 | i = n_locations 62 | distance_traveled = 0 63 | start_time = rospy.Time.now() 64 | running_time = 0 65 | location = "" 66 | last_location = "" 67 | 68 | # 确保有初始位置 69 | while initial_pose.header.stamp == "": 70 | rospy.sleep(1) 71 | 72 | rospy.loginfo("Starting navigation test") 73 | 74 | # 开始主循环，随机导航 75 | while not rospy.is_shutdown(): 76 | # 如果已经走完了所有点，再重新开始排序 77 | if i == n_locations: 78 | i = 0 79 | sequence = sample(locations, n_locations) 80 | 81 | # 如果最后一个点和第一个点相同，则跳过 82 | if sequence[0] == last_location: 83 | i = 1 84 | 85 | # 在当前的排序中获取下一个目标点 86 | location = sequence[i] 87 | 88 | # 跟踪行驶距离 89 | # 使用更新的初始位置 90 | if initial_pose.header.stamp == "": 91 | distance = sqrt(pow(locations[location].position.x - 92 | locations[last_location].position.x, 2) + 93 | pow(locations[location].position.y - 94 | locations[last_location].position.y, 2)) 95 | else: 96 | rospy.loginfo("Updating current pose.") 97 | distance = sqrt(pow(locations[location].position.x - 98 | initial_pose.pose.pose.position.x, 2) + 99 | pow(locations[location].position.y - 100 | initial_pose.pose.pose.position.y, 2)) 101 | initial_pose.header.stamp = "" 102 | 103 | # 存储上一次的位置，计算距离 104 | last_location = location 105 | 106 | # 计数器加1 107 | i += 1 108 | n_goals += 1 109 | 110 | # 设定下一个目标点 111 | self.goal = MoveBaseGoal() 112 | self.goal.target_pose.pose = locations[location] 113 | self.goal.target_pose.header.frame_id = 'map' 114 | self.goal.target_pose.header.stamp = rospy.Time.now() 115 | 116 | # 让用户知道下一个位置 117 | rospy.loginfo("Going to: " + str(location)) 118 | 119 | # 向下一个位置进发 120 | self.move_base.send_goal(self.goal) 121 | 122 | # 五分钟时间限制 123 | finished_within_time = self.move_base.wait_for_result(rospy.Duration(300)) 124 | 125 | # 查看是否成功到达 126 | if not finished_within_time: 127 | self.move_base.cancel_goal() 128 | rospy.loginfo("Timed out achieving goal") 129 | else: 130 | state = self.move_base.get_state() 131 | if state == GoalStatus.SUCCEEDED: 132 | rospy.loginfo("Goal succeeded!") 133 | n_successes += 1 134 | distance_traveled += distance 135 | rospy.loginfo("State:" + str(state)) 136 | else: 137 | rospy.loginfo("Goal failed with error code: " + str(goal_states[state])) 138 | 139 | # 运行所用时间 140 | running_time = rospy.Time.now() - start_time 141 | running_time = running_time.secs / 60.0 142 | 143 | # 输出本次导航的所有信息 144 | rospy.loginfo("Success so far: " + str(n_successes) + "/" + 145 | str(n_goals) + " = " + 146 | str(100 * n_successes/n_goals) + "%") 147 | 148 | rospy.loginfo("Running time: " + str(trunc(running_time, 1)) + 149 | " min Distance: " + str(trunc(distance_traveled, 1)) + " m") 150 | 151 | rospy.sleep(self.rest_time) 152 | 153 | def update_initial_pose(self, initial_pose): 154 | self.initial_pose = initial_pose 155 | 156 | def shutdown(self): 157 | rospy.loginfo("Stopping the robot...") 158 | self.move_base.cancel_goal() 159 | rospy.sleep(2) 160 | self.cmd_vel_pub.publish(Twist()) 161 | rospy.sleep(1) 162 | 163 | def trunc(f, n): 164 | slen = len('%.*f' % (n, f)) 165 | 166 | return float(str(f)[:slen]) 167 | 168 | if __name__ == '__main__': 169 | try: 170 | NavTest() 171 | rospy.spin() 172 | 173 | except rospy.ROSInterruptException: 174 | rospy.loginfo("Exploring SLAM finished.") 175 | -------------------------------------------------------------------------------- /mrobot_navigation/scripts/random_navigation.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import roslib; 5 | import rospy 6 | import actionlib 7 | from actionlib_msgs.msg import * 8 | from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist 9 | from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal 10 | from random import sample 11 | from math import pow, sqrt 12 | 13 | class NavTest(): 14 | def __init__(self): 15 | rospy.init_node('random_navigation', anonymous=True) 16 | rospy.on_shutdown(self.shutdown) 17 | 18 | # 在每个目标位置暂停的时间 19 | self.rest_time = rospy.get_param("~rest_time", 2) 20 | 21 | # 到达目标的状态 22 | goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED', 23 | 'SUCCEEDED', 'ABORTED', 'REJECTED', 24 | 'PREEMPTING', 'RECALLING', 'RECALLED', 25 | 'LOST'] 26 | 27 | # 设置目标点的位置 28 | # 如果想要获得某一点的坐标，在rviz中点击 2D Nav Goal 按键，然后单机地图中一点 29 | # 在终端中就会看到坐标信息 30 | locations = dict() 31 | 32 | locations['p1'] = Pose(Point(1.150, 5.461, 0.000), Quaternion(0.000, 0.000, -0.013, 1.000)) 33 | locations['p2'] = Pose(Point(6.388, 2.66, 0.000), Quaternion(0.000, 0.000, 0.063, 0.998)) 34 | locations['p3'] = Pose(Point(8.089, -1.657, 0.000), Quaternion(0.000, 0.000, 0.946, -0.324)) 35 | locations['p4'] = Pose(Point(9.767, 5.171, 0.000), Quaternion(0.000, 0.000, 0.139, 0.990)) 36 | locations['p5'] = Pose(Point(0.502, 1.270, 0.000), Quaternion(0.000, 0.000, 0.919, -0.392)) 37 | locations['p6'] = Pose(Point(4.557, 1.234, 0.000), Quaternion(0.000, 0.000, 0.627, 0.779)) 38 | 39 | # 发布控制机器人的消息 40 | self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) 41 | 42 | # 订阅move_base服务器的消息 43 | self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) 44 | 45 | rospy.loginfo("Waiting for move_base action server...") 46 | 47 | # 60s等待时间限制 48 | self.move_base.wait_for_server(rospy.Duration(60)) 49 | rospy.loginfo("Connected to move base server") 50 | 51 | # 保存机器人的在rviz中的初始位置 52 | initial_pose = PoseWithCovarianceStamped() 53 | 54 | # 保存成功率、运行时间、和距离的变量 55 | n_locations = len(locations) 56 | n_goals = 0 57 | n_successes = 0 58 | i = n_locations 59 | distance_traveled = 0 60 | start_time = rospy.Time.now() 61 | running_time = 0 62 | location = "" 63 | last_location = "" 64 | 65 | # 确保有初始位置 66 | while initial_pose.header.stamp == "": 67 | rospy.sleep(1) 68 | 69 | rospy.loginfo("Starting navigation test") 70 | 71 | # 开始主循环，随机导航 72 | while not rospy.is_shutdown(): 73 | # 如果已经走完了所有点，再重新开始排序 74 | if i == n_locations: 75 | i = 0 76 | sequence = sample(locations, n_locations) 77 | 78 | # 如果最后一个点和第一个点相同，则跳过 79 | if sequence[0] == last_location: 80 | i = 1 81 | 82 | # 在当前的排序中获取下一个目标点 83 | location = sequence[i] 84 | 85 | # 跟踪行驶距离 86 | # 使用更新的初始位置 87 | if initial_pose.header.stamp == "": 88 | distance = sqrt(pow(locations[location].position.x - 89 | locations[last_location].position.x, 2) + 90 | pow(locations[location].position.y - 91 | locations[last_location].position.y, 2)) 92 | else: 93 | rospy.loginfo("Updating current pose.") 94 | distance = sqrt(pow(locations[location].position.x - 95 | initial_pose.pose.pose.position.x, 2) + 96 | pow(locations[location].position.y - 97 | initial_pose.pose.pose.position.y, 2)) 98 | initial_pose.header.stamp = "" 99 | 100 | # 存储上一次的位置，计算距离 101 | last_location = location 102 | 103 | # 计数器加1 104 | i += 1 105 | n_goals += 1 106 | 107 | # 设定下一个目标点 108 | self.goal = MoveBaseGoal() 109 | self.goal.target_pose.pose = locations[location] 110 | self.goal.target_pose.header.frame_id = 'map' 111 | self.goal.target_pose.header.stamp = rospy.Time.now() 112 | 113 | # 让用户知道下一个位置 114 | rospy.loginfo("Going to: " + str(location)) 115 | 116 | # 向下一个位置进发 117 | self.move_base.send_goal(self.goal) 118 | 119 | # 五分钟时间限制 120 | finished_within_time = self.move_base.wait_for_result(rospy.Duration(300)) 121 | 122 | # 查看是否成功到达 123 | if not finished_within_time: 124 | self.move_base.cancel_goal() 125 | rospy.loginfo("Timed out achieving goal") 126 | else: 127 | state = self.move_base.get_state() 128 | if state == GoalStatus.SUCCEEDED: 129 | rospy.loginfo("Goal succeeded!") 130 | n_successes += 1 131 | distance_traveled += distance 132 | rospy.loginfo("State:" + str(state)) 133 | else: 134 | rospy.loginfo("Goal failed with error code: " + str(goal_states[state])) 135 | 136 | # 运行所用时间 137 | running_time = rospy.Time.now() - start_time 138 | running_time = running_time.secs / 60.0 139 | 140 | # 输出本次导航的所有信息 141 | rospy.loginfo("Success so far: " + str(n_successes) + "/" + 142 | str(n_goals) + " = " + 143 | str(100 * n_successes/n_goals) + "%") 144 | 145 | rospy.loginfo("Running time: " + str(trunc(running_time, 1)) + 146 | " min Distance: " + str(trunc(distance_traveled, 1)) + " m") 147 | 148 | rospy.sleep(self.rest_time) 149 | 150 | def update_initial_pose(self, initial_pose): 151 | self.initial_pose = initial_pose 152 | 153 | def shutdown(self): 154 | rospy.loginfo("Stopping the robot...") 155 | self.move_base.cancel_goal() 156 | rospy.sleep(2) 157 | self.cmd_vel_pub.publish(Twist()) 158 | rospy.sleep(1) 159 | 160 | def trunc(f, n): 161 | slen = len('%.*f' % (n, f)) 162 | 163 | return float(str(f)[:slen]) 164 | 165 | if __name__ == '__main__': 166 | try: 167 | NavTest() 168 | rospy.spin() 169 | 170 | except rospy.ROSInterruptException: 171 | rospy.loginfo("Random navigation finished.") 172 | -------------------------------------------------------------------------------- /mrobot_teleop/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mrobot_teleop) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | geometry_msgs 9 | roscpp 10 | rospy 11 | ) 12 | 13 | ## System dependencies are found with CMake's conventions 14 | # find_package(Boost REQUIRED COMPONENTS system) 15 | 16 | 17 | ## Uncomment this if the package has a setup.py. This macro ensures 18 | ## modules and global scripts declared therein get installed 19 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 20 | # catkin_python_setup() 21 | 22 | ################################################ 23 | ## Declare ROS messages, services and actions ## 24 | ################################################ 25 | 26 | ## To declare and build messages, services or actions from within this 27 | ## package, follow these steps: 28 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 29 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 30 | ## * In the file package.xml: 31 | ## * add a build_depend tag for "message_generation" 32 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 33 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 34 | ## but can be declared for certainty nonetheless: 35 | ## * add a run_depend tag for "message_runtime" 36 | ## * In this file (CMakeLists.txt): 37 | ## * add "message_generation" and every package in MSG_DEP_SET to 38 | ## find_package(catkin REQUIRED COMPONENTS ...) 39 | ## * add "message_runtime" and every package in MSG_DEP_SET to 40 | ## catkin_package(CATKIN_DEPENDS ...) 41 | ## * uncomment the add_*_files sections below as needed 42 | ## and list every .msg/.srv/.action file to be processed 43 | ## * uncomment the generate_messages entry below 44 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 45 | 46 | ## Generate messages in the 'msg' folder 47 | # add_message_files( 48 | # FILES 49 | # Message1.msg 50 | # Message2.msg 51 | # ) 52 | 53 | ## Generate services in the 'srv' folder 54 | # add_service_files( 55 | # FILES 56 | # Service1.srv 57 | # Service2.srv 58 | # ) 59 | 60 | ## Generate actions in the 'action' folder 61 | # add_action_files( 62 | # FILES 63 | # Action1.action 64 | # Action2.action 65 | # ) 66 | 67 | ## Generate added messages and services with any dependencies listed here 68 | # generate_messages( 69 | # DEPENDENCIES 70 | # geometry_msgs 71 | # ) 72 | 73 | ################################################ 74 | ## Declare ROS dynamic reconfigure parameters ## 75 | ################################################ 76 | 77 | ## To declare and build dynamic reconfigure parameters within this 78 | ## package, follow these steps: 79 | ## * In the file package.xml: 80 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 81 | ## * In this file (CMakeLists.txt): 82 | ## * add "dynamic_reconfigure" to 83 | ## find_package(catkin REQUIRED COMPONENTS ...) 84 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 85 | ## and list every .cfg file to be processed 86 | 87 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 88 | # generate_dynamic_reconfigure_options( 89 | # cfg/DynReconf1.cfg 90 | # cfg/DynReconf2.cfg 91 | # ) 92 | 93 | ################################### 94 | ## catkin specific configuration ## 95 | ################################### 96 | ## The catkin_package macro generates cmake config files for your package 97 | ## Declare things to be passed to dependent projects 98 | ## INCLUDE_DIRS: uncomment this if you package contains header files 99 | ## LIBRARIES: libraries you create in this project that dependent projects also need 100 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 101 | ## DEPENDS: system dependencies of this project that dependent projects also need 102 | catkin_package( 103 | # INCLUDE_DIRS include 104 | # LIBRARIES mrobot_teleop 105 | # CATKIN_DEPENDS geometry_msgs roscpp rospy 106 | # DEPENDS system_lib 107 | ) 108 | 109 | ########### 110 | ## Build ## 111 | ########### 112 | 113 | ## Specify additional locations of header files 114 | ## Your package locations should be listed before other locations 115 | # include_directories(include) 116 | include_directories( 117 | ${catkin_INCLUDE_DIRS} 118 | ) 119 | 120 | ## Declare a C++ library 121 | # add_library(mrobot_teleop 122 | # src/${PROJECT_NAME}/mrobot_teleop.cpp 123 | # ) 124 | 125 | ## Add cmake target dependencies of the library 126 | ## as an example, code may need to be generated before libraries 127 | ## either from message generation or dynamic reconfigure 128 | # add_dependencies(mrobot_teleop ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Declare a C++ executable 131 | # add_executable(mrobot_teleop_node src/mrobot_teleop_node.cpp) 132 | 133 | ## Add cmake target dependencies of the executable 134 | ## same as for the library above 135 | # add_dependencies(mrobot_teleop_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Specify libraries to link a library or executable target against 138 | # target_link_libraries(mrobot_teleop_node 139 | # ${catkin_LIBRARIES} 140 | # ) 141 | 142 | ############# 143 | ## Install ## 144 | ############# 145 | 146 | # all install targets should use catkin DESTINATION variables 147 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 148 | 149 | ## Mark executable scripts (Python etc.) for installation 150 | ## in contrast to setup.py, you can choose the destination 151 | # install(PROGRAMS 152 | # scripts/my_python_script 153 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark executables and/or libraries for installation 157 | # install(TARGETS mrobot_teleop mrobot_teleop_node 158 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 159 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 160 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark cpp header files for installation 164 | # install(DIRECTORY include/${PROJECT_NAME}/ 165 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 166 | # FILES_MATCHING PATTERN "*.h" 167 | # PATTERN ".svn" EXCLUDE 168 | # ) 169 | 170 | ## Mark other files for installation (e.g. launch and bag files, etc.) 171 | # install(FILES 172 | # # myfile1 173 | # # myfile2 174 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 175 | # ) 176 | 177 | ############# 178 | ## Testing ## 179 | ############# 180 | 181 | ## Add gtest based cpp test target and link libraries 182 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_mrobot_teleop.cpp) 183 | # if(TARGET ${PROJECT_NAME}-test) 184 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 185 | # endif() 186 | 187 | ## Add folders to be run by python nosetests 188 | # catkin_add_nosetests(test) 189 | -------------------------------------------------------------------------------- /mrobot_teleop/launch/logitech.launch: -------------------------------------------------------------------------------- 1 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /mrobot_teleop/launch/mrobot_teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /mrobot_teleop/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrobot_teleop 4 | 0.0.0 5 | The mrobot_teleop package 6 | 7 | 8 | 9 | 10 | hcx 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | geometry_msgs 44 | roscpp 45 | rospy 46 | geometry_msgs 47 | roscpp 48 | rospy 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /mrobot_teleop/scripts/mrobot_teleop.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | import rospy 4 | from geometry_msgs.msg import Twist 5 | import sys, select, termios, tty 6 | 7 | msg = """ 8 | Control mrobot! 9 | --------------------------- 10 | Moving around: 11 | u i o 12 | j k l 13 | m , . 14 | 15 | q/z : increase/decrease max speeds by 10% 16 | w/x : increase/decrease only linear speed by 10% 17 | e/c : increase/decrease only angular speed by 10% 18 | space key, k : force stop 19 | anything else : stop smoothly 20 | 21 | CTRL-C to quit 22 | """ 23 | 24 | moveBindings = { 25 | 'i':(1,0), 26 | 'o':(1,-1), 27 | 'j':(0,1), 28 | 'l':(0,-1), 29 | 'u':(1,1), 30 | ',':(-1,0), 31 | '.':(-1,1), 32 | 'm':(-1,-1), 33 | } 34 | 35 | speedBindings={ 36 | 'q':(1.1,1.1), 37 | 'z':(.9,.9), 38 | 'w':(1.1,1), 39 | 'x':(.9,1), 40 | 'e':(1,1.1), 41 | 'c':(1,.9), 42 | } 43 | 44 | def getKey(): 45 | tty.setraw(sys.stdin.fileno()) 46 | rlist, _, _ = select.select([sys.stdin], [], [], 0.1) 47 | if rlist: 48 | key = sys.stdin.read(1) 49 | else: 50 | key = '' 51 | 52 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 53 | return key 54 | 55 | speed = .2 56 | turn = 1 57 | 58 | def vels(speed,turn): 59 | return "currently:\tspeed %s\tturn %s " % (speed,turn) 60 | 61 | if __name__=="__main__": 62 | settings = termios.tcgetattr(sys.stdin) 63 | 64 | rospy.init_node('mrobot_teleop') 65 | pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5) 66 | 67 | x = 0 68 | th = 0 69 | status = 0 70 | count = 0 71 | acc = 0.1 72 | target_speed = 0 73 | target_turn = 0 74 | control_speed = 0 75 | control_turn = 0 76 | try: 77 | print msg 78 | print vels(speed,turn) 79 | while(1): 80 | key = getKey() 81 | # 运动控制方向键（1：正方向，-1负方向） 82 | if key in moveBindings.keys(): 83 | x = moveBindings[key][0] 84 | th = moveBindings[key][1] 85 | count = 0 86 | # 速度修改键 87 | elif key in speedBindings.keys(): 88 | speed = speed * speedBindings[key][0] # 线速度增加0.1倍 89 | turn = turn * speedBindings[key][1] # 角速度增加0.1倍 90 | count = 0 91 | 92 | print vels(speed,turn) 93 | if (status == 14): 94 | print msg 95 | status = (status + 1) % 15 96 | # 停止键 97 | elif key == ' ' or key == 'k' : 98 | x = 0 99 | th = 0 100 | control_speed = 0 101 | control_turn = 0 102 | else: 103 | count = count + 1 104 | if count > 4: 105 | x = 0 106 | th = 0 107 | if (key == '\x03'): 108 | break 109 | 110 | # 目标速度=速度值*方向值 111 | target_speed = speed * x 112 | target_turn = turn * th 113 | 114 | # 速度限位，防止速度增减过快 115 | if target_speed > control_speed: 116 | control_speed = min( target_speed, control_speed + 0.02 ) 117 | elif target_speed < control_speed: 118 | control_speed = max( target_speed, control_speed - 0.02 ) 119 | else: 120 | control_speed = target_speed 121 | 122 | if target_turn > control_turn: 123 | control_turn = min( target_turn, control_turn + 0.1 ) 124 | elif target_turn < control_turn: 125 | control_turn = max( target_turn, control_turn - 0.1 ) 126 | else: 127 | control_turn = target_turn 128 | 129 | # 创建并发布twist消息 130 | twist = Twist() 131 | twist.linear.x = control_speed; 132 | twist.linear.y = 0; 133 | twist.linear.z = 0 134 | twist.angular.x = 0; 135 | twist.angular.y = 0; 136 | twist.angular.z = control_turn 137 | pub.publish(twist) 138 | 139 | except: 140 | print e 141 | 142 | finally: 143 | twist = Twist() 144 | twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0 145 | twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0 146 | pub.publish(twist) 147 | 148 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 149 | --------------------------------------------------------------------------------