一、机器人模型和仿真环境的搭建
1、利用catkin_make命令创建工作空间，然后利用catkin__create_pkg命令创建功能包。然后，搭建仿真环境和机器人模型，并且配置gazebo中机器人所需的传感器。机器人模型使用Solidworks建模，并且生成urdf文件和模型文件。
修改urdf文件，加入gazebo的各种插件后的urdf文件代码如下：
1 <robot 2 name="slam_robot"> 3 4 5 <link 6 name="base_footprint"> 7 <visual> 8 <origin 9 xyz="0 0 0" 10 rpy="0 0 0" /> 11 <geometry> 12 <box size="0.001 0.001 0.001" /> 13 </geometry> 14 </visual> 15 </link> 16 <gazebo 17 reference="base_footprint"> 18 <material> Gazebo/Grey </material> 19 </gazebo> 20 21 22 23 <link 24 name="base_link"> 25 <inertial> 26 <origin 27 xyz="0.000123585099380281 -1.81158790515598E-07 0.121582066979231" 28 rpy="0 0 0" /> 29 <mass 30 value="20.8233150515763" /> 31 <inertia 32 ixx="0.520337446154128" 33 ixy="1.15771094237282E-07" 34 ixz="5.55921584004563E-05" 35 iyy="0.540021669673746" 36 iyz="3.94749060970847E-07" 37 izz="0.404229114852017" /> 38 </inertial> 39 <visual> 40 <origin 41 xyz="0 0 0" 42 rpy="0 0 0" /> 43 <geometry> 44 <mesh 45 filename="package://slam_robot/meshes/base_link.STL" /> 46 </geometry> 47 <material 48 name=""> 49 <color 50 rgba="1 1 1 1" /> 51 </material> 52 </visual> 53 <collision> 54 <origin 55 xyz="0 0 0" 56 rpy="0 0 0" /> 57 <geometry> 58 <mesh 59 filename="package://slam_robot/meshes/base_link.STL" /> 60 </geometry> 61 </collision> 62 </link> 63 64 <gazebo 65 reference="base_link" > 66 <material> Gazebo/Grey </material> 67 </gazebo> 68 69 <joint 70 name="base_joint" 71 type="fixed" > 72 <origin 73 xyz="0 0 0.025" 74 rpy="0 0 0" /> 75 <parent link="base_footprint" /> 76 <child link="base_link" /> 77 </joint> 78 79 80 81 <link 82 name="left_wheel_link"> 83 <inertial> 84 <origin 85 xyz="0 0 0.017469530472088" 86 rpy="0 0 0" /> 87 <mass 88 value="0.317051102258936" /> 89 <inertia 90 ixx="0.000261588067757881" 91 ixy="1.35525271560688E-20" 92 ixz="-1.79403851286325E-20" 93 iyy="0.000261588067757881" 94 iyz="-8.88780679344079E-20" 95 izz="0.000462659864810872" /> 96 </inertial> 97 <visual> 98 <origin 99 xyz="0 0 0" 100 rpy="0 0 0" /> 101 <geometry> 102 <mesh 103 filename="package://slam_robot/meshes/left_wheel_link.STL" /> 104 </geometry> 105 <material 106 name=""> 107 <color 108 rgba="1 1 1 1" /> 109 </material> 110 </visual> 111 <collision> 112 <origin 113 xyz="0 0 0" 114 rpy="0 0 0" /> 115 <geometry> 116 <mesh 117 filename="package://slam_robot/meshes/left_wheel_link.STL" /> 118 </geometry> 119 </collision> 120 </link> 121 122 <gazebo 123 reference="left_wheel_link" > 124 <material> Gazebo/Green </material> 125 </gazebo> 126 127 <joint 128 name="left_wheel_joint" 129 type="continuous"> 130 <origin 131 xyz="0 0.162 0.055" 132 rpy="-1.5708 -0.9209 0" /> 133 <parent 134 link="base_footprint" /> 135 <child 136 link="left_wheel_link" /> 137 <axis 138 xyz="0 0 1" /> 139 </joint> 140 141 <transmission 142 name="left_wheel_joint_trans" > 143 <type> transmission_interface/SimpleTransmission </type> 144 <joint 145 name="left_wheel_joint" /> 146 <actuator 147 name="left_wheel_joint_motor" > 148 <hardwareInterface> VelocityJointInterface </hardwareInterface> 149 <mechanicalReduction> 1 </mechanicalReduction> 150 </actuator> 151 </transmission> 152 153 154 155 <link 156 name="right_wheel_link"> 157 <inertial> 158 <origin 159 xyz="1.38777878078145E-17 0 0.017469530472088" 160 rpy="0 0 0" /> 161 <mass 162 value="0.317051102258936" /> 163 <inertia 164 ixx="0.000261588067757881" 165 ixy="6.7762635780344E-21" 166 ixz="-2.09479316049536E-20" 167 iyy="0.000261588067757881" 168 iyz="-9.03431261430328E-20" 169 izz="0.000462659864810872" /> 170 </inertial> 171 <visual> 172 <origin 173 xyz="0 0 0" 174 rpy="0 0 0" /> 175 <geometry> 176 <mesh 177 filename="package://slam_robot/meshes/right_wheel_link.STL" /> 178 </geometry> 179 <material 180 name=""> 181 <color 182 rgba="1 1 1 1" /> 183 </material> 184 </visual> 185 <collision> 186 <origin 187 xyz="0 0 0" 188 rpy="0 0 0" /> 189 <geometry> 190 <mesh 191 filename="package://slam_robot/meshes/right_wheel_link.STL" /> 192 </geometry> 193 </collision> 194 </link> 195 196 <gazebo 197 reference="right_wheel_link" > 198 <material> Gazebo/Green </material> 199 </gazebo> 200 201 <joint 202 name="right_wheel_joint" 203 type="continuous"> 204 <origin 205 xyz="0 -0.162 0.055" 206 rpy="1.5708 0.16843 0" /> 207 <parent 208 link="base_footprint" /> 209 <child 210 link="right_wheel_link" /> 211 <axis 212 xyz="0 0 -1" /> 213 </joint> 214 <transmission 215 name="right_wheel_joint_trans" > 216 <type> transmission_interface/SimpleTransmission </type> 217 <joint 218 name="right_wheel_joint" /> 219 <actuator 220 name="right_wheel_joint_motor" > 221 <hardwareInterface> VelocityJointInterface </hardwareInterface> 222 <mechanicalReduction> 1 </mechanicalReduction> 223 </actuator> 224 </transmission> 225 226 227 228 <link 229 name="prewheel_support_link"> 230 <inertial> 231 <origin 232 xyz="-0.00133539993117242 4.33680868994202E-18 0.0154040382343795" 233 rpy="0 0 0" /> 234 <mass 235 value="0.00409920756010184" /> 236 <inertia 237 ixx="1.15081132756128E-06" 238 ixy="1.98523347012727E-22" 239 ixz="9.25536176070155E-08" 240 iyy="7.42031839650237E-07" 241 iyz="-1.28212994945719E-22" 242 izz="7.013412543038E-07" /> 243 </inertial> 244 <visual> 245 <origin 246 xyz="0 0 0" 247 rpy="0 0 0" /> 248 <geometry> 249 <mesh 250 filename="package://slam_robot/meshes/prewheel_support_link.STL" /> 251 </geometry> 252 <material 253 name=""> 254 <color 255 rgba="1 1 1 1" /> 256 </material> 257 </visual> 258 <collision> 259 <origin 260 xyz="0 0 0" 261 rpy="0 0 0" /> 262 <geometry> 263 <mesh 264 filename="package://slam_robot/meshes/prewheel_support_link.STL" /> 265 </geometry> 266 </collision> 267 </link> 268 269 <gazebo 270 reference="prewheel_support_link" > 271 <material> Gazebo/Yellow </material> 272 </gazebo> 273 274 <joint 275 name="prewheel_support_joint" 276 type="continuous"> 277 <origin 278 xyz="0.15 0 0.03" 279 rpy="3.1416 0 0.04998" /> 280 <parent 281 link="base_link" /> 282 <child 283 link="prewheel_support_link" /> 284 <axis 285 xyz="0 0 -1" /> 286 </joint> 287 288 289 290 <link 291 name="prewheel_link"> 292 <inertial> 293 <origin 294 xyz="0 1.38777878078145E-17 0.01" 295 rpy="0 0 0" /> 296 <mass 297 value="0.035770858989988" /> 298 <inertia 299 ixx="6.32969092661898E-06" 300 ixy="-2.11758236813575E-22" 301 ixz="1.88629712726959E-22" 302 iyy="6.32969092661898E-06" 303 iyz="-1.63662662360686E-22" 304 izz="1.05325895982562E-05" /> 305 </inertial> 306 <visual> 307 <origin 308 xyz="0 0 0" 309 rpy="0 0 0" /> 310 <geometry> 311 <mesh 312 filename="package://slam_robot/meshes/prewheel_link.STL" /> 313 </geometry> 314 <material 315 name=""> 316 <color 317 rgba="1 1 1 1" /> 318 </material> 319 </visual> 320 <collision> 321 <origin 322 xyz="0 0 0" 323 rpy="0 0 0" /> 324 <geometry> 325 <mesh 326 filename="package://slam_robot/meshes/prewheel_link.STL" /> 327 </geometry> 328 </collision> 329 </link> 330 331 <gazebo 332 reference="prewheel_link" > 333 <material> Gazebo/Green </material> 334 </gazebo> 335 336 <joint 337 name="prewheel_joint" 338 type="continuous"> 339 <origin 340 xyz="-0.0056569 -0.01 0.029997" 341 rpy="1.5708 -1.0175 -3.1416" /> 342 <parent 343 link="prewheel_support_link" /> 344 <child 345 link="prewheel_link" /> 346 <axis 347 xyz="0 0 -1" /> 348 </joint> 349 350 351 352 <link 353 name="backwheel_support_link"> 354 <inertial> 355 <origin 356 xyz="-0.00133539993117243 1.73472347597681E-18 0.0154040382343795" 357 rpy="0 0 0" /> 358 <mass 359 value="0.00409920756010185" /> 360 <inertia 361 ixx="1.15081132756129E-06" 362 ixy="1.2573145310806E-22" 363 ixz="9.25536176070161E-08" 364 iyy="7.4203183965024E-07" 365 iyz="-2.20030042939105E-22" 366 izz="7.01341254303803E-07" /> 367 </inertial> 368 <visual> 369 <origin 370 xyz="0 0 0" 371 rpy="0 0 0" /> 372 <geometry> 373 <mesh 374 filename="package://slam_robot/meshes/backwheel_support_link.STL" /> 375 </geometry> 376 <material 377 name=""> 378 <color 379 rgba="1 1 1 1" /> 380 </material> 381 </visual> 382 <collision> 383 <origin 384 xyz="0 0 0" 385 rpy="0 0 0" /> 386 <geometry> 387 <mesh 388 filename="package://slam_robot/meshes/backwheel_support_link.STL" /> 389 </geometry> 390 </collision> 391 </link> 392 393 <gazebo 394 reference="backwheel_support_link" > 395 <material> Gazebo/Yellow </material> 396 </gazebo> 397 398 <joint 399 name="backwheel_support_joint" 400 type="continuous"> 401 <origin 402 xyz="-0.15 0 0.03" 403 rpy="3.1416 0 -0.08199" /> 404 <parent 405 link="base_link" /> 406 <child 407 link="backwheel_support_link" /> 408 <axis 409 xyz="0 0 -1" /> 410 </joint> 411 412 413 414 <link 415 name="backwheel_link"> 416 <inertial> 417 <origin 418 xyz="0 0 0.01" 419 rpy="0 0 0" /> 420 <mass 421 value="0.035770858989988" /> 422 <inertia 423 ixx="6.32969092661898E-06" 424 ixy="-1.27054942088145E-21" 425 ixz="-4.27778369597206E-22" 426 iyy="6.32969092661898E-06" 427 iyz="3.64057242950478E-22" 428 izz="1.05325895982562E-05" /> 429 </inertial> 430 <visual> 431 <origin 432 xyz="0 0 0" 433 rpy="0 0 0" /> 434 <geometry> 435 <mesh 436 filename="package://slam_robot/meshes/backwheel_link.STL" /> 437 </geometry> 438 <material 439 name=""> 440 <color 441 rgba="1 1 1 1" /> 442 </material> 443 </visual> 444 <collision> 445 <origin 446 xyz="0 0 0" 447 rpy="0 0 0" /> 448 <geometry> 449 <mesh 450 filename="package://slam_robot/meshes/backwheel_link.STL" /> 451 </geometry> 452 </collision> 453 </link> 454 455 <gazebo 456 reference="backwheel_link" > 457 <material> Gazebo/Green </material> 458 </gazebo> 459 460 <joint 461 name="backwheel_joint" 462 type="continuous"> 463 <origin 464 xyz="-0.0056569 -0.01 0.029997" 465 rpy="1.5708 -1.0175 3.1416" /> 466 <parent 467 link="backwheel_support_link" /> 468 <child 469 link="backwheel_link" /> 470 <axis 471 xyz="0 0 -1" /> 472 </joint> 473 474 475 476 <link 477 name="depth_camera_link"> 478 <inertial> 479 <origin 480 xyz="-6.95827433885958E-05 2.14214216520403E-05 0.0250711682856426" 481 rpy="0 0 0" /> 482 <mass 483 value="0.496610492071422" /> 484 <inertia 485 ixx="0.00269744563124009" 486 ixy="-2.02864180575388E-07" 487 ixz="-2.45925714877531E-09" 488 iyy="0.000169280642999443" 489 iyz="7.57095535031013E-10" 490 izz="0.00266010065057093" /> 491 </inertial> 492 <visual> 493 <origin 494 xyz="0 0 0" 495 rpy="0 0 0" /> 496 <geometry> 497 <mesh 498 filename="package://slam_robot/meshes/depth_camera_link.STL" /> 499 </geometry> 500 <material 501 name=""> 502 <color 503 rgba="1 1 1 1" /> 504 </material> 505 </visual> 506 <collision> 507 <origin 508 xyz="0 0 0" 509 rpy="0 0 0" /> 510 <geometry> 511 <mesh 512 filename="package://slam_robot/meshes/depth_camera_link.STL" /> 513 </geometry> 514 </collision> 515 </link> 516 517 <gazebo 518 reference="depth_camera_link" > 519 <material> Gazebo/Green </material> 520 </gazebo> 521 522 <joint 523 name="depth_camera_joint" 524 type="fixed"> 525 <origin 526 xyz="0 0 1.15" 527 rpy="0 0 0" /> 528 <parent 529 link="base_link" /> 530 <child 531 link="depth_camera_link" /> 532 <axis 533 xyz="0 0 0" /> 534 </joint> 535 536 <gazebo 537 reference="depth_camera_link"> 538 <sensor 539 type="depth" 540 name="depth_camera"> 541 <always_on> true </always_on> 542 <update_rate> 20.0 </update_rate> 543 <camera> 544 <horizontal_fov> 1.05 </horizontal_fov> 545 <image> 546 <format> R8G8B8 </format> 547 <width> 640 </width> 548 <height> 480 </height> 549 </image> 550 <clip> 551 <near> 0.05 </near> 552 <far> 8.0 </far> 553 </clip> 554 </camera> 555 <plugin 556 name="depth_camera_controller" 557 filename="libgazebo_ros_openni_kinect.so"> 558 <cameraName> depth_camera </cameraName> 559 <alwaysOn> true </alwaysOn> 560 <updateRate> 10 </updateRate> 561 <imageTopicName> rgb/image_raw </imageTopicName> 562 <depthImageTopicName> depth/image_raw </depthImageTopicName> 563 <pointCloudTopicName> depth/points </pointCloudTopicName> 564 <cameraInfoTopicName> rgb/camera_info </cameraInfoTopicName> 565 <depthImageCameraInfoTopicName> depth/camera_info </depthImageCameraInfoTopicName> 566 <frameName> depth_camera_link </frameName> 567 <baseline> 0.1 </baseline> 568 <distortion_k1> 0.0 </distortion_k1> 569 <distortion_k2> 0.0 </distortion_k2> 570 <distortion_k3> 0.0 </distortion_k3> 571 <distortion_t1> 0.0 </distortion_t1> 572 <distortion_t2> 0.0 </distortion_t2> 573 <pointCloudCutoff> 0.4 </pointCloudCutoff> 574 </plugin> 575 </sensor> 576 </gazebo> 577 578 579 580 <link 581 name="laser_radar_link"> 582 <inertial> 583 <origin 584 xyz="1.07210658509341E-05 1.73472347597681E-18 0.0300008263238345" 585 rpy="0 0 0" /> 586 <mass 587 value="0.452485404014218" /> 588 <inertia 589 ixx="0.000429778883865124" 590 ixy="2.52407257964088E-23" 591 ixz="-1.88779148450591E-08" 592 iyy="0.000430023306408609" 593 iyz="3.54515413261475E-25" 594 izz="0.000588351597907478" /> 595 </inertial> 596 <visual> 597 <origin 598 xyz="0 0 0" 599 rpy="0 0 0" /> 600 <geometry> 601 <mesh 602 filename="package://slam_robot/meshes/laser_radar_link.STL" /> 603 </geometry> 604 <material 605 name=""> 606 <color 607 rgba="1 1 1 1" /> 608 </material> 609 </visual> 610 <collision> 611 <origin 612 xyz="0 0 0" 613 rpy="0 0 0" /> 614 <geometry> 615 <mesh 616 filename="package://slam_robot/meshes/laser_radar_link.STL" /> 617 </geometry> 618 </collision> 619 </link> 620 621 <gazebo 622 reference="laser_radar_link" > 623 <material> Gazebo/Yellow </material> 624 </gazebo> 625 626 <joint 627 name="laser_radar_joint" 628 type="fixed"> 629 <origin 630 xyz="0 0 1.09" 631 rpy="0 0 0" /> 632 <parent 633 link="base_link" /> 634 <child 635 link="laser_radar_link" /> 636 <axis 637 xyz="0 0 0" /> 638 </joint> 639 640 <gazebo 641 reference="laser_radar_link"> 642 <sensor 643 type="ray" 644 name="rplidar"> 645 <pose>0 0 0 0 0 0</pose> 646 <visualize> false </visualize> 647 <update_rate> 5.5 </update_rate> 648 <ray> 649 <scan> 650 <horizontal> 651 <samples> 360 </samples> 652 <resolution> 1 </resolution> 653 <min_angle> -3 </min_angle> 654 <max_angle> 3 </max_angle> 655 </horizontal> 656 </scan> 657 <range> 658 <min> 0.10 </min> 659 <max> 6.0 </max> 660 <resolution> 0.01 </resolution> 661 </range> 662 <noise> 663 <type> gaussian </type> 664 <mean> 0.0 </mean> 665 <stddev> 0.01 </stddev> 666 </noise> 667 </ray> 668 <plugin 669 name="gazebo_rplidar" 670 filename="libgazebo_ros_laser.so"> 671 <topicName> /scan </topicName> 672 <frameName> laser_radar_link </frameName> 673 </plugin> 674 </sensor> 675 </gazebo> 676 677 678 679 <gazebo> 680 <plugin 681 name="differential_drive_controller" 682 filename="libgazebo_ros_diff_drive.so" > 683 <rosDebugLevel> Debug </rosDebugLevel> 684 <publishWheelTF> true </publishWheelTF> 685 <robotNamespace> / </robotNamespace> 686 <publishTf> 1 </publishTf> 687 <publishWheelJointState> true </publishWheelJointState> 688 <alwaysOn> true </alwaysOn> 689 <updateRate> 100.0 </updateRate> 690 <legacyMode> true </legacyMode> 691 <leftJoint> right_wheel_joint </leftJoint> 692 <rightJoint> left_wheel_joint </rightJoint> 693 <wheelSeparation> 0.2 </wheelSeparation> 694 <wheelDiameter> 0.1 </wheelDiameter> 695 <broadcastTF> 1 </broadcastTF> 696 <wheelTorque> 30 </wheelTorque> 697 <wheelAcceleration> 15 </wheelAcceleration> 698 <commandTopic> cmd_vel </commandTopic> 699 <odometryFrame> odom </odometryFrame> 700 <odometryTopic> odom </odometryTopic> 701 <publishOdomTF> true </publishOdomTF> 702 <odometrySource> 1 </odometrySource> 703 <robotBaseFrame> base_footprint </robotBaseFrame> 704 </plugin> 705 </gazebo> 706 707 </robot>
.STL模型文件需要放在合适的位置，且需要与urdf文件里面的路径和文件名完全对应：
2、编写launch文件slam_robot_gazebo.launch，加载模型以及发布模型之间的tf。
launch文件如下：
1 <launch> 2 3 <param 4 name="use_gui" 5 value="false" /> 6 <remap 7 from="/depth_camera/rgb/image_raw" 8 to="/camera/rgb/image_raw" /> 9 <remap 10 from="/depth_camera/depth/image_raw" 11 to="/camera/depth_registered/image_raw" /> 12 13 <!-- 运行gazebo仿真环境 --> 14 <include 15 file="$(find gazebo_ros)/launch/empty_world.launch"> 16 <arg 17 name="world_name" 18 value="$(find slam_robot)/worlds/slam_robot_world.world" /> 19 <arg 20 name="gui" 21 value="true" /> 22 <arg 23 name="debug" 24 value="false" /> 25 <arg 26 name="paused" 27 value="false" /> 28 <arg 29 name="use_sim_time" 30 value="true" /> 31 <arg 32 name="headless" 33 value="false" /> 34 </include> 35 36 <!-- 加载机器人模型描述参数 --> 37 <param 38 name="robot_description" 39 textfile="$(find slam_robot)/urdf/slam_robot_gazebo.urdf" /> 40 41 <!-- 运行joint_state_publisher节点，发布机器人的关节状态 --> 42 <node 43 name="joint_state_publisher" 44 pkg ="joint_state_publisher" 45 type="joint_state_publisher" /> 46 47 <!-- 运行robot_state_publisher节点，发布TF --> 48 <node 49 name="robot_state_publisher" 50 pkg ="robot_state_publisher" 51 type="robot_state_publisher" 52 output="screen" > 53 <param 54 name="publish_frequency" 55 type="double" 56 value="50.0" /> 57 </node> 58 59 <!-- 在gazebo中加载机器人模型 --> 60 <node 61 name="urdf_spawner" 62 pkg ="gazebo_ros" 63 type="spawn_model" 64 respawn="false" 65 output="screen" 66 args="-urdf -model slam_robot -param robot_description" /> 67 68 </launch>
其中需要加载.world文件，也就是机器人所在的仿真环境，注意.world文件的路径和文件名。本文所使用的slam_robot_world.world文件代码如下：
1 <?xml version="1.0" ?> 2 <sdf version="1.4"> 3 <world name="default"> 4 <scene> 5 <ambient>0.68 0.68 0.68 1.0</ambient> 6 <sky> 7 <sunrise/> 8 <clouds> 9 <speed>12</speed> 10 </clouds> 11 </sky> 12 </scene> 13 <physics type='ode'> 14 <max_step_size>0.001</max_step_size> 15 <real_time_factor>1</real_time_factor> 16 <real_time_update_rate>1000</real_time_update_rate> 17 <gravity>0 0 -9.8</gravity> 18 </physics> 19 20 <include> 21 <uri>model://ISCAS_groundplane</uri> 22 </include> 23 <include> 24 <uri>model://sun</uri> 25 </include> 26 <include> 27 <uri>model://ISCAS_Museum</uri> 28 <pose>0 0 0 0 0 -1.57</pose> 29 </include> 30 <!--<include> 31 <uri>model://ISCAS_post</uri> 32 <name>table1</name> 33 <pose>8 3.5 0 0 0 1</pose> 34 </include> 35 <include> 36 <uri>model://ISCAS_post</uri> 37 <name>table2</name> 38 <pose>6 5 0 0 0 1.57</pose> 39 </include>--> 40 </world> 41 </sdf>
注：world文件中需要用到模型文件，由于本文使用的是ROS-Academy-for-Beginners-master功能包集中的world地图，因此需要将此功能包集加入到工作空间中并进行编译。
3、创建手动发布运动指令的节点slam_robot_teleop.py（只需要用现成的功能包，修改一下文件名和发布的话题就可以），该节点的作用是通过键盘的按键发布运动控制的/cmd话题，以Twist的消息类型发布。
1 #!/usr/bin/env python 2 3 # Copyright (c) 2011, Willow Garage, Inc. 4 # All rights reserved. 5 # 6 # Redistribution and use in source and binary forms, with or without 7 # modification, are permitted provided that the following conditions are met: 8 # 9 # * Redistributions of source code must retain the above copyright 10 # notice, this list of conditions and the following disclaimer. 11 # * Redistributions in binary form must reproduce the above copyright 12 # notice, this list of conditions and the following disclaimer in the 13 # documentation and/or other materials provided with the distribution. 14 # * Neither the name of the Willow Garage, Inc. nor the names of its 15 # contributors may be used to endorse or promote products derived from 16 # this software without specific prior written permission. 17 # 18 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 # POSSIBILITY OF SUCH DAMAGE. 29 30 import roslib 31 #roslib.load_manifest('turtlebot_teleop') # No longer needed in catkin! 32 import rospy 33 34 from geometry_msgs.msg import Twist 35 36 import sys, select, termios, tty 37 38 msg = """ 39 Control Your Turtlebot! 40 --------------------------- 41 Moving around: 42 u i o 43 j k l 44 m , . 45 46 q/z : increase/decrease max speeds by 10% 47 w/x : increase/decrease only linear speed by 10% 48 e/c : increase/decrease only angular speed by 10% 49 space key, k : force stop 50 anything else : stop smoothly 51 52 CTRL-C to quit 53 """ 54 55 moveBindings = { 56 'i':(1,0), 57 'o':(1,-1), 58 'j':(0,1), 59 'l':(0,-1), 60 'u':(1,1), 61 ',':(-1,0), 62 '.':(-1,1), 63 'm':(-1,-1), 64 } 65 66 speedBindings={ 67 'q':(1.1,1.1), 68 'z':(.9,.9), 69 'w':(1.1,1), 70 'x':(.9,1), 71 'e':(1,1.1), 72 'c':(1,.9), 73 } 74 75 def getKey(): 76 tty.setraw(sys.stdin.fileno()) 77 rlist, _, _ = select.select([sys.stdin], [], [], 0.1) 78 if rlist: 79 key = sys.stdin.read(1) 80 else: 81 key = '' 82 83 termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 84 return key 85 86 #speed = .2 87 speed = 1 88 turn = 1 89 90 def vels(speed,turn): 91 return "currently:\tspeed %s\tturn %s " % (speed,turn) 92 93 if __name__=="__main__": 94 settings = termios.tcgetattr(sys.stdin) 95 96 rospy.init_node('slam_robot_teleop') 97 pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) 98 99 x = 0 100 th = 0 101 status = 0 102 count = 0 103 acc = 0.1 104 target_speed = 0 105 target_turn = 0 106 control_speed = 0 107 control_turn = 0 108 try: 109 print msg 110 print vels(speed,turn) 111 while(1): 112 key = getKey() 113 if key in moveBindings.keys(): 114 x = moveBindings[key][0] 115 th = moveBindings[key][1] 116 count = 0 117 elif key in speedBindings.keys(): 118 speed = speed * speedBindings[key][0] 119 turn = turn * speedBindings[key][1] 120 count = 0 121 122 print vels(speed,turn) 123 if (status == 14): 124 print msg 125 status = (status + 1) % 15 126 elif key == ' ' or key == 'k' : 127 x = 0 128 th = 0 129 control_speed = 0 130 control_turn = 0 131 else: 132 count = count + 1 133 if count > 4: 134 x = 0 135 th = 0 136 if (key == '\x03'): 137 break 138 139 target_speed = speed * x 140 target_turn = turn * th 141 142 if target_speed > control_speed: 143 control_speed = min( target_speed, control_speed + 0.02 ) 144 elif target_speed < control_speed: 145 control_speed = max( target_speed, control_speed - 0.02 ) 146 else: 147 control_speed = target_speed 148 149 if target_turn > control_turn: 150 control_turn = min( target_turn, control_turn + 0.1 ) 151 elif target_turn < control_turn: 152 control_turn = max( target_turn, control_turn - 0.1 ) 153 else: 154 control_turn = target_turn 155 156 twist = Twist() 157 twist.linear.x = control_speed; twist.linear.y = 0; twist.linear.z = 0 158 twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turn 159 pub.publish(twist) 160 161 #print("loop: {0}".format(count)) 162 #print("target: vx: {0}, wz: {1}".format(target_speed, target_turn)) 163 #print("publihsed: vx: {0}, wz: {1}".format(twist.linear.x, twist.angular.z)) 164 165 except: 166 print e 167 168 finally: 169 twist = Twist() 170 twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0 171 twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0 172 pub.publish(twist) 173 174 termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
python脚本需要给执行权限：
sudo chmod 777 /home/oay/catkin_ws/src/slam_robot/scripts/slam_robot_teleop.py
4、运行launch文件，打开仿真环境。
roslaunch slam_robot slam_robot_gazebo.launch
运行手动发布速度节点，进行速度指令的发布，开启新的终端输入：
rosrun slam_robot slam_robot_teleop.py
此时，可以利用键盘控制仿真环境中的小车进行运动，如下图所示：
可以看到此时gazebo发布了激光雷达、深度相机等传感器信息：
