├── ch3 ├── test1 │ ├── CMakeLists.txt │ ├── package.xml │ └── src │ │ └── vel_pose.cpp ├── test2 │ ├── CMakeLists.txt │ ├── package.xml │ └── src │ │ └── spawn_client.cpp └── test3 │ ├── CMakeLists.txt │ ├── package.xml │ ├── src │ ├── circular_motion.cpp │ ├── motion_server.cpp │ └── turtle_spawn.cpp │ └── srv │ └── turtlesrv.srv ├── ch4 ├── learning_launch │ ├── CMakeLists.txt │ ├── launch │ │ ├── test1.launch │ │ ├── test2.launch │ │ ├── test3_circular_motion.launch │ │ ├── test3_motion_server.launch │ │ ├── test3_turtle_spawn.launch │ │ └── turtlesim.launch │ └── package.xml └── learning_tf │ ├── CMakeLists.txt │ ├── launch │ └── learning_tf.launch │ ├── package.xml │ └── src │ ├── tf_broadcaster.cpp │ └── tf_listener.cpp ├── ch5 ├── CMakeLists.txt ├── config │ └── robot_urdf.rviz ├── launch │ └── display_robot_body.launch ├── meshes │ ├── kinect.dae │ ├── kinect.jpg │ └── kinect.tga ├── package.xml └── urdf │ └── robot_body.urdf ├── ch6 ├── robot_description │ ├── CMakeLists.txt │ ├── config │ │ └── robot.rviz │ ├── meshes │ │ ├── kinect.dae │ │ ├── kinect.jpg │ │ └── kinect.tga │ ├── package.xml │ └── urdf │ │ ├── robot.xacro │ │ ├── robot_body.xacro │ │ ├── robot_kinect_rplidar.xacro │ │ └── sensor │ │ ├── kinect.xacro │ │ └── rplidar.xacro ├── robot_gazebo │ ├── CMakeLists.txt │ ├── launch │ │ ├── robot_rviz.launch │ │ ├── view_robot_gazebo_empty_world.launch │ │ └── view_robot_kinect_rplidar.launch │ ├── package.xml │ └── worlds │ │ ├── cloister.world │ │ ├── playground.world │ │ ├── playpen.world │ │ └── room.world └── robot_teleop │ ├── CMakeLists.txt │ ├── launch │ └── robot_teleop.launch │ ├── package.xml │ └── scripts │ └── robot_teleop.py ├── ch7 ├── robot_vision │ ├── CMakeLists.txt │ ├── camera_calibration.yaml │ ├── config │ │ ├── ar_track_camera.rviz │ │ └── ar_track_kinect.rviz │ ├── data │ │ └── haar_detectors │ │ │ ├── haarcascade_frontalface_alt.xml │ │ │ └── haarcascade_profileface.xml │ ├── doc │ │ └── checkerboard.pdf │ ├── kinect_depth_calibration.yaml │ ├── kinect_rgb_calibration.yaml │ ├── launch │ │ ├── ar_track_camera.launch │ │ ├── ar_track_kinect.launch │ │ ├── face_detector.launch │ │ ├── freenect.launch │ │ ├── freenect_with_calibration.launch │ │ ├── motion_detector.launch │ │ ├── usb_cam.launch │ │ └── usb_cam_with_calibration.launch │ ├── package.xml │ └── scripts │ │ ├── cv_bridge_test.py │ │ ├── face_detector.py │ │ └── motion_detector.py ├── test1 │ ├── CMakeLists.txt │ ├── msg │ │ └── Size.msg │ ├── package.xml │ └── src │ │ └── test1.cpp └── test2 │ ├── CMakeLists.txt │ ├── msg │ └── pose.msg │ ├── package.xml │ └── src │ └── test2.cpp ├── ch8 ├── libmsc.so └── robot_voice │ ├── CMakeLists.txt │ ├── include │ └── robot_voice │ │ ├── formats.h │ │ ├── linuxrec.h │ │ ├── msp_cmn.h │ │ ├── msp_errors.h │ │ ├── msp_types.h │ │ ├── qise.h │ │ ├── qisr.h │ │ ├── qtts.h │ │ └── speech_recognizer.h │ ├── launch │ ├── repeat_voice.launch │ ├── voice_assistant.launch │ └── voice_teleop.launch │ ├── package.xml │ └── src │ ├── iat_publish.cpp │ ├── linuxrec.c │ ├── speech_recognizer.c │ ├── tts_subscribe.cpp │ ├── voice_assistant.cpp │ └── voice_teleop.cpp ├── ch9 ├── robot_description │ ├── CMakeLists.txt │ ├── config │ │ └── robot.rviz │ ├── meshes │ │ ├── kinect.dae │ │ ├── kinect.jpg │ │ └── kinect.tga │ ├── package.xml │ └── urdf │ │ ├── robot.xacro │ │ ├── robot_body.xacro │ │ ├── robot_rplidar.xacro │ │ └── sensor │ │ ├── kinect.xacro │ │ └── rplidar.xacro ├── robot_gazebo │ ├── CMakeLists.txt │ ├── launch │ │ ├── robot_rplidar_nav.launch │ │ ├── robot_rviz.launch │ │ ├── view_robot_gazebo_empty_world.launch │ │ └── view_robot_rplidar.launch │ ├── models │ │ ├── ISCAS_Museum │ │ │ ├── meshes │ │ │ │ ├── ISCASmuseum.mtl │ │ │ │ ├── ISCASmuseum.obj │ │ │ │ ├── zd_011.jpg │ │ │ │ ├── zd_021.jpg │ │ │ │ ├── zd_031.jpg │ │ │ │ ├── zd_041.jpg │ │ │ │ └── 轮胎贴图.jpg │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── ISCAS_groundplane │ │ │ ├── materials │ │ │ │ ├── scripts │ │ │ │ │ └── ISCAS_groundplane.material │ │ │ │ └── textures │ │ │ │ │ └── ISCAS_groundplane.png │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── ISCAS_post │ │ │ ├── meshes │ │ │ │ └── post.dae │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── meshes │ │ │ ├── .mayaSwatches │ │ │ │ └── uvtietu.jpg.swatch │ │ │ ├── base_link.dae │ │ │ ├── base_link.mtl │ │ │ ├── base_link.obj │ │ │ ├── pitch_platform.dae │ │ │ ├── pitch_platform.mtl │ │ │ ├── pitch_platform.obj │ │ │ ├── realsense.dae │ │ │ ├── realsense.mtl │ │ │ ├── realsense.obj │ │ │ ├── rplidar.dae │ │ │ ├── rplidar.mtl │ │ │ ├── rplidar.obj │ │ │ ├── uvtietu.jpg │ │ │ ├── wheel.dae │ │ │ ├── wheel.jpg │ │ │ ├── wheel.mtl │ │ │ ├── wheel.obj │ │ │ ├── xtion_pro_camera.dae │ │ │ ├── xtion_pro_camera.jpg │ │ │ ├── xtion_pro_camera.mtl │ │ │ ├── xtion_pro_camera.obj │ │ │ ├── yaw_platform.dae │ │ │ ├── yaw_platform.mtl │ │ │ └── yaw_platform.obj │ │ └── sai_di │ │ │ ├── meshes │ │ │ ├── sai_di1.dae │ │ │ ├── sai_di2.dae │ │ │ ├── sai_di3.dae │ │ │ ├── sai_di4.dae │ │ │ ├── tielouti.jpg │ │ │ ├── tietu3.jpg │ │ │ ├── 不锈钢.JPG │ │ │ ├── 地板底.jpg │ │ │ ├── 贴图1.jpg │ │ │ └── 贴图2.jpg │ │ │ ├── model.config │ │ │ └── model.sdf │ ├── package.xml │ └── worlds │ │ ├── museum.world │ │ ├── playground.world │ │ └── room.world ├── robot_navigation │ ├── CMakeLists.txt │ ├── config │ │ ├── lidar.lua │ │ └── robot │ │ │ ├── base_local_planner_params.yaml │ │ │ ├── costmap_common_params.yaml │ │ │ ├── dwa_local_planner_params.yaml │ │ │ ├── global_costmap_params.yaml │ │ │ ├── local_costmap_params.yaml │ │ │ └── move_base_params.yaml │ ├── launch │ │ ├── gmapping.launch │ │ ├── gmapping_demo.launch │ │ ├── hector.launch │ │ ├── hector_demo.launch │ │ └── include │ │ │ └── move_base.launch │ ├── maps │ │ ├── museum_gmapping.pgm │ │ ├── museum_gmapping.yaml │ │ ├── museum_hector.pgm │ │ ├── museum_hector.yaml │ │ ├── room_gmapping.pgm │ │ └── room_gmapping.yaml │ ├── package.xml │ ├── param │ │ ├── costmap_common_params.yaml │ │ ├── dwa_local_planner_params.yaml │ │ ├── global_costmap_params.yaml │ │ ├── global_planner_params.yaml │ │ ├── local_costmap_params.yaml │ │ ├── move_base_params.yaml │ │ └── navfn_global_planner_params.yaml │ └── rviz │ │ ├── gmapping.rviz │ │ └── nav.rviz └── robot_teleop │ ├── CMakeLists.txt │ ├── launch │ └── robot_teleop.launch │ ├── package.xml │ └── scripts │ └── robot_teleop.py └── linux_raisimCheckMyMachine /ch3/test1/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | test1 4 | 0.0.0 5 | The test1 package 6 | 7 | 8 | 9 | 10 | yuemingbi 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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | roscpp 54 | geometry_msgs 55 | roscpp 56 | geometry_msgs 57 | roscpp 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /ch3/test1/src/vel_pose.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "turtlesim/Pose.h" 3 | #include "geometry_msgs/Twist.h" 4 | 5 | void callback(const turtlesim::Pose::ConstPtr& pose){ 6 | ROS_INFO("x: %f", pose->x); 7 | ROS_INFO("y: %f", pose->y); 8 | ROS_INFO("theta: %f", pose->theta); 9 | ROS_INFO("linear_velocity: %f", pose->linear_velocity); 10 | ROS_INFO("angular_velocity: %f", pose->angular_velocity); 11 | ROS_INFO("---"); 12 | } 13 | 14 | int main(int argc, char ** argv){ 15 | ros::init(argc, argv, "pose_subscriber"); 16 | ros::NodeHandle n; 17 | ros::Publisher pub = n.advertise("turtle1/cmd_vel", 1000); 18 | ros::Subscriber sub = n.subscribe("turtle1/pose", 1000, callback); 19 | ros::Rate loop_rate(10); 20 | 21 | while(ros::ok()){ 22 | geometry_msgs::Twist msg; 23 | msg.linear.x = 0.5; 24 | msg.angular.z = 0.5; 25 | //ROS_INFO("vel_publish: linear.x=%.1f angular.z=%.1f", msg.linear.x, msg.angular.z); 26 | pub.publish(msg); 27 | loop_rate.sleep(); 28 | ros::spinOnce(); 29 | } 30 | 31 | return 0; 32 | } 33 | -------------------------------------------------------------------------------- /ch3/test2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | test2 4 | 0.0.0 5 | The test2 package 6 | 7 | 8 | 9 | 10 | yuemingbi 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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | std_srvs 54 | roscpp 55 | std_srvs 56 | roscpp 57 | std_srvs 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /ch3/test2/src/spawn_client.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "turtlesim/Spawn.h" 3 | #include "ctime" 4 | #include "sstream" 5 | 6 | int main(int argc, char **argv){ 7 | ros::init(argc, argv, "spawn_client"); 8 | ros::NodeHandle n; 9 | ros::ServiceClient client = n.serviceClient("spawn"); 10 | turtlesim::Spawn srv; 11 | 12 | int number[2]; 13 | int count = 0; 14 | int input1 = 12; 15 | int input2 = 999999999; 16 | srand(time(0)); 17 | number[0] = rand()%input1; 18 | number[1] = rand()%input1; 19 | number[2] = rand()%input1; 20 | count = rand()%input2; 21 | 22 | std::stringstream ss; 23 | ss << "turtle"<< count; 24 | 25 | srv.request.x = number[0]; 26 | srv.request.y = number[1]; 27 | srv.request.theta = number[2]; 28 | srv.request.name = ss.str(); 29 | client.call(srv); 30 | ROS_INFO("show result: %s", srv.response.name.c_str()); 31 | 32 | return 0; 33 | } 34 | -------------------------------------------------------------------------------- /ch3/test3/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | test3 4 | 0.0.0 5 | The test3 package 6 | 7 | 8 | 9 | 10 | yuemingbi 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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | roscpp 54 | std_msgs 55 | std_srvs 56 | message_generation 57 | geometry_msgs 58 | roscpp 59 | std_msgs 60 | std_srvs 61 | 62 | geometry_msgs 63 | roscpp 64 | std_msgs 65 | std_srvs 66 | message_runtime 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /ch3/test3/src/circular_motion.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "test3/turtlesrv.h" 3 | #include "sstream" 4 | 5 | int main(int argc, char **argv){ 6 | ros::init(argc, argv, "circular_motion"); 7 | ros::NodeHandle n; 8 | std::stringstream ss; 9 | ss << argv[1] << "/cmd_vel"; 10 | ros::ServiceClient client = n.serviceClient("motion_control"); 11 | test3::turtlesrv srv; 12 | srv.request.turtlename = ss.str(); 13 | srv.request.x = atof(argv[3]); 14 | srv.request.z = atof(argv[4]); 15 | std::string name = argv[2]; 16 | 17 | if(name=="begin"){ 18 | srv.request.data = true; 19 | ROS_INFO("Start moving"); 20 | } 21 | else if(name=="stop"){ 22 | srv.request.data = false; 23 | ROS_INFO("Stop moving"); 24 | } 25 | 26 | if(client.call(srv)){ 27 | ROS_INFO("Start moving"); 28 | } 29 | else{ 30 | ROS_INFO("Stop moving"); 31 | } 32 | 33 | return 0; 34 | } 35 | -------------------------------------------------------------------------------- /ch3/test3/src/motion_server.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "test3/turtlesrv.h" 3 | #include "geometry_msgs/Twist.h" 4 | 5 | bool control(test3::turtlesrv::Request &req, test3::turtlesrv::Response &res){ 6 | if(req.data){ 7 | ros::NodeHandle n; 8 | ros::Publisher pub = n.advertise(req.turtlename, 1000); 9 | ros::Rate loop_rate(10); 10 | while(ros::ok()){ 11 | geometry_msgs::Twist msg; 12 | msg.linear.x = req.x; 13 | msg.angular.z = req.z; 14 | ROS_INFO("vel_publish: linear.x=%.1f angular.z=%.1f", msg.linear.x, msg.angular.z); 15 | pub.publish(msg); 16 | loop_rate.sleep(); 17 | ros::spinOnce(); 18 | } 19 | 20 | return true; 21 | } 22 | else{ 23 | ros::NodeHandle n; 24 | ros::Publisher pub = n.advertise(req.turtlename, 1000); 25 | ros::Rate loop_rate(10); 26 | while(ros::ok()){ 27 | geometry_msgs::Twist msg; 28 | msg.linear.x = 0; 29 | msg.angular.z = 0; 30 | ROS_INFO("vel_publish: linear.x=%.1f angular.z=%.1f", msg.linear.x, msg.angular.z); 31 | pub.publish(msg); 32 | loop_rate.sleep(); 33 | ros::spinOnce(); 34 | } 35 | } 36 | 37 | return false; 38 | } 39 | 40 | int main(int argc, char **argv){ 41 | ros::init(argc, argv, "motion_server"); 42 | ros::NodeHandle n; 43 | ros::ServiceServer service = n.advertiseService("motion_control", control); 44 | ros::spin(); 45 | 46 | return 0; 47 | } 48 | -------------------------------------------------------------------------------- /ch3/test3/src/turtle_spawn.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "turtlesim/Spawn.h" 3 | #include "ctime" 4 | 5 | int main(int argc, char** argv){ 6 | ros::init(argc, argv, "turtle_spawn"); 7 | ros::NodeHandle n; 8 | ros::ServiceClient client = n.serviceClient("spawn"); 9 | turtlesim::Spawn srv; 10 | 11 | int input1 = 10; 12 | int number[2]; 13 | srand(time(0)); 14 | number[0] = rand()%input1; 15 | number[1] = rand()%input1; 16 | number[2] = rand()%input1; 17 | 18 | srv.request.x = number[0]; 19 | srv.request.y = number[1]; 20 | srv.request.theta = number[2]; 21 | srv.request.name = argv[1]; 22 | client.call(srv); 23 | ROS_INFO("show result: %s", srv.response.name.c_str()); 24 | 25 | return 0; 26 | } 27 | -------------------------------------------------------------------------------- /ch3/test3/srv/turtlesrv.srv: -------------------------------------------------------------------------------- 1 | bool data 2 | string turtlename 3 | float64 x 4 | float64 z 5 | --- 6 | bool success 7 | -------------------------------------------------------------------------------- /ch4/learning_launch/launch/test1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /ch4/learning_launch/launch/test2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /ch4/learning_launch/launch/test3_circular_motion.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /ch4/learning_launch/launch/test3_motion_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /ch4/learning_launch/launch/test3_turtle_spawn.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /ch4/learning_launch/launch/turtlesim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /ch4/learning_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | learning_launch 4 | 0.0.0 5 | The learning_launch package 6 | 7 | 8 | 9 | 10 | yuemingbi 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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /ch4/learning_tf/launch/learning_tf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /ch4/learning_tf/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | learning_tf 4 | 0.0.0 5 | The learning_tf package 6 | 7 | 8 | 9 | 10 | yuemingbi 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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | roscpp 54 | tf 55 | geometry_msgs 56 | roscpp 57 | tf 58 | geometry_msgs 59 | roscpp 60 | tf 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /ch4/learning_tf/src/tf_broadcaster.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | int main(int argc, char** argv){ 5 | ros::init(argc, argv, "tf_broadcaster"); 6 | ros::NodeHandle n; 7 | ros::Rate loop_rate(100); 8 | tf::TransformBroadcaster broadcaster; 9 | 10 | while(n.ok()){ 11 | broadcaster.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),ros::Time::now(),"base_link", "base_laser")); 12 | loop_rate.sleep(); 13 | } 14 | } 15 | -------------------------------------------------------------------------------- /ch4/learning_tf/src/tf_listener.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "geometry_msgs/PointStamped.h" 3 | #include "tf/transform_listener.h" 4 | 5 | void transformPoint(const tf::TransformListener& listener){ 6 | geometry_msgs::PointStamped laser_point; 7 | laser_point.header.frame_id = "base_laser"; 8 | 9 | laser_point.header.stamp = ros::Time(); 10 | 11 | laser_point.point.x = 1.0; 12 | laser_point.point.y = 0.2; 13 | laser_point.point.z = 0.0; 14 | 15 | try{ 16 | geometry_msgs::PointStamped base_point; 17 | listener.transformPoint("base_link", laser_point, base_point); 18 | ROS_INFO("base_laser:(%.2f, %.2f, %.2f)--->base_link:(%.2f, %.2f, %.2f) at time %.2f", laser_point.point.x, laser_point.point.y, laser_point.point.z, base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec()); 19 | } 20 | catch(tf::TransformException& ex){ 21 | ROS_ERROR("Receive an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what()); 22 | } 23 | } 24 | 25 | int main(int argc, char **argv){ 26 | ros::init(argc, argv, "tf_listener"); 27 | ros::NodeHandle n; 28 | tf::TransformListener listener(ros::Duration(10)); 29 | 30 | ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener))); 31 | 32 | ros::spin(); 33 | } 34 | -------------------------------------------------------------------------------- /ch5/config/robot_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 | Splitter Ratio: 0.5 10 | Tree Height: 549 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Alpha: 1 56 | Class: rviz/RobotModel 57 | Collision Enabled: false 58 | Enabled: true 59 | Links: 60 | All Links Enabled: true 61 | Expand Joint Details: false 62 | Expand Link Details: false 63 | Expand Tree: false 64 | Link Tree Style: Links in Alphabetic Order 65 | base_footprint: 66 | Alpha: 1 67 | Show Axes: false 68 | Show Trail: false 69 | Value: true 70 | base_link: 71 | Alpha: 1 72 | Show Axes: false 73 | Show Trail: false 74 | Value: true 75 | Name: RobotModel 76 | Robot Description: robot_description 77 | TF Prefix: "" 78 | Update Interval: 0 79 | Value: true 80 | Visual Enabled: true 81 | Enabled: true 82 | Global Options: 83 | Background Color: 48; 48; 48 84 | Default Light: true 85 | Fixed Frame: base_link 86 | Frame Rate: 30 87 | Name: root 88 | Tools: 89 | - Class: rviz/Interact 90 | Hide Inactive Objects: true 91 | - Class: rviz/MoveCamera 92 | - Class: rviz/Select 93 | - Class: rviz/FocusCamera 94 | - Class: rviz/Measure 95 | - Class: rviz/SetInitialPose 96 | Theta std deviation: 0.2617993950843811 97 | Topic: /initialpose 98 | X std deviation: 0.5 99 | Y std deviation: 0.5 100 | - Class: rviz/SetGoal 101 | Topic: /move_base_simple/goal 102 | - Class: rviz/PublishPoint 103 | Single click: true 104 | Topic: /clicked_point 105 | Value: true 106 | Views: 107 | Current: 108 | Class: rviz/Orbit 109 | Distance: 0.8438647985458374 110 | Enable Stereo Rendering: 111 | Stereo Eye Separation: 0.05999999865889549 112 | Stereo Focal Distance: 1 113 | Swap Stereo Eyes: false 114 | Value: false 115 | Focal Point: 116 | X: 0 117 | Y: 0 118 | Z: 0 119 | Focal Shape Fixed Size: true 120 | Focal Shape Size: 0.05000000074505806 121 | Invert Z Axis: false 122 | Name: Current View 123 | Near Clip Distance: 0.009999999776482582 124 | Pitch: 0.24539749324321747 125 | Target Frame: 126 | Value: Orbit (rviz) 127 | Yaw: 1.2703975439071655 128 | Saved: ~ 129 | Window Geometry: 130 | Displays: 131 | collapsed: false 132 | Height: 846 133 | Hide Left Dock: false 134 | Hide Right Dock: false 135 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 136 | Selection: 137 | collapsed: false 138 | Time: 139 | collapsed: false 140 | Tool Properties: 141 | collapsed: false 142 | Views: 143 | collapsed: false 144 | Width: 1200 145 | X: 466 146 | Y: 430 147 | -------------------------------------------------------------------------------- /ch5/launch/display_robot_body.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /ch5/meshes/kinect.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch5/meshes/kinect.jpg -------------------------------------------------------------------------------- /ch5/meshes/kinect.tga: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch5/meshes/kinect.tga -------------------------------------------------------------------------------- /ch5/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ch5 4 | 0.0.0 5 | The ch5 package 6 | 7 | 8 | 9 | 10 | yuemingbi 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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | urdf 53 | xacro 54 | urdf 55 | xacro 56 | urdf 57 | xacro 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /ch6/robot_description/meshes/kinect.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch6/robot_description/meshes/kinect.jpg -------------------------------------------------------------------------------- /ch6/robot_description/meshes/kinect.tga: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch6/robot_description/meshes/kinect.tga -------------------------------------------------------------------------------- /ch6/robot_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_description 4 | 0.0.0 5 | The robot_description package 6 | 7 | 8 | 9 | 10 | yuemingbi 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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | urdf 53 | xacro 54 | urdf 55 | xacro 56 | urdf 57 | xacro 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /ch6/robot_description/urdf/robot.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /ch6/robot_description/urdf/robot_kinect_rplidar.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 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /ch6/robot_description/urdf/sensor/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 | -------------------------------------------------------------------------------- /ch6/robot_description/urdf/sensor/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 | -------------------------------------------------------------------------------- /ch6/robot_gazebo/launch/robot_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /ch6/robot_gazebo/launch/view_robot_gazebo_empty_world.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 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /ch6/robot_gazebo/launch/view_robot_kinect_rplidar.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 | -------------------------------------------------------------------------------- /ch6/robot_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_gazebo 4 | 0.0.0 5 | The robot_gazebo package 6 | 7 | 8 | 9 | 10 | yuemingbi 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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /ch6/robot_teleop/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(robot_teleop) 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 | geometry_msgs 12 | roscpp 13 | rospy 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 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 your 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 mbot_teleop 108 | # CATKIN_DEPENDS geometry_msgs roscpp rospy 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( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/mbot_teleop.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(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | # add_executable(${PROJECT_NAME}_node src/mbot_teleop_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | # target_link_libraries(${PROJECT_NAME}_node 150 | # ${catkin_LIBRARIES} 151 | # ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | # install(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables and/or libraries for installation 168 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 169 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 170 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark cpp header files for installation 175 | # install(DIRECTORY include/${PROJECT_NAME}/ 176 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 177 | # FILES_MATCHING PATTERN "*.h" 178 | # PATTERN ".svn" EXCLUDE 179 | # ) 180 | 181 | ## Mark other files for installation (e.g. launch and bag files, etc.) 182 | # install(FILES 183 | # # myfile1 184 | # # myfile2 185 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 186 | # ) 187 | 188 | ############# 189 | ## Testing ## 190 | ############# 191 | 192 | ## Add gtest based cpp test target and link libraries 193 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_mbot_teleop.cpp) 194 | # if(TARGET ${PROJECT_NAME}-test) 195 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 196 | # endif() 197 | 198 | ## Add folders to be run by python nosetests 199 | # catkin_add_nosetests(test) 200 | -------------------------------------------------------------------------------- /ch6/robot_teleop/launch/robot_teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /ch6/robot_teleop/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_teleop 4 | 0.0.0 5 | The robot_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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | roscpp 54 | rospy 55 | geometry_msgs 56 | roscpp 57 | rospy 58 | geometry_msgs 59 | roscpp 60 | rospy 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /ch6/robot_teleop/scripts/robot_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('robot_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 | -------------------------------------------------------------------------------- /ch7/robot_vision/camera_calibration.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: head_camera 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [514.453865, 0.000000, 368.311232, 0.000000, 514.428903, 224.580904, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [0.031723, -0.045138, 0.004146, 0.006205, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [516.422974, 0.000000, 372.887246, 0.000000, 0.000000, 520.513672, 226.444701, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | 22 | -------------------------------------------------------------------------------- /ch7/robot_vision/config/ar_track_camera.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.5 8 | Tree Height: 298 9 | - Class: rviz/Selection 10 | Name: Selection 11 | - Class: rviz/Tool Properties 12 | Expanded: 13 | - /2D Pose Estimate1 14 | - /2D Nav Goal1 15 | - /Publish Point1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.588679016 18 | - Class: rviz/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: Camera 28 | Visualization Manager: 29 | Class: "" 30 | Displays: 31 | - Alpha: 0.5 32 | Cell Size: 1 33 | Class: rviz/Grid 34 | Color: 160; 160; 164 35 | Enabled: true 36 | Line Style: 37 | Line Width: 0.0299999993 38 | Value: Lines 39 | Name: Grid 40 | Normal Cell Count: 0 41 | Offset: 42 | X: 0 43 | Y: 0 44 | Z: 0 45 | Plane: XY 46 | Plane Cell Count: 10 47 | Reference Frame: 48 | Value: true 49 | - Class: rviz/TF 50 | Enabled: true 51 | Frame Timeout: 15 52 | Frames: 53 | All Enabled: true 54 | ar_marker_0: 55 | Value: true 56 | ar_marker_1: 57 | Value: true 58 | ar_marker_192: 59 | Value: true 60 | ar_marker_2: 61 | Value: true 62 | ar_marker_4: 63 | Value: true 64 | ar_marker_5: 65 | Value: true 66 | ar_marker_7: 67 | Value: true 68 | ar_marker_8: 69 | Value: true 70 | usb_cam: 71 | Value: true 72 | world: 73 | Value: true 74 | Marker Scale: 0.300000012 75 | Name: TF 76 | Show Arrows: true 77 | Show Axes: true 78 | Show Names: true 79 | Tree: 80 | world: 81 | usb_cam: 82 | ar_marker_0: 83 | {} 84 | ar_marker_1: 85 | {} 86 | ar_marker_192: 87 | {} 88 | ar_marker_2: 89 | {} 90 | ar_marker_4: 91 | {} 92 | ar_marker_5: 93 | {} 94 | ar_marker_7: 95 | {} 96 | ar_marker_8: 97 | {} 98 | Update Interval: 0 99 | Value: true 100 | - Class: rviz/Marker 101 | Enabled: true 102 | Marker Topic: /visualization_marker 103 | Name: Marker 104 | Namespaces: 105 | basic_shapes: true 106 | Queue Size: 100 107 | Value: true 108 | - Class: rviz/Camera 109 | Enabled: true 110 | Image Rendering: background and overlay 111 | Image Topic: /usb_cam/image_raw 112 | Name: Camera 113 | Overlay Alpha: 0.5 114 | Queue Size: 2 115 | Transport Hint: raw 116 | Unreliable: false 117 | Value: true 118 | Visibility: 119 | Grid: true 120 | Marker: true 121 | TF: true 122 | Value: true 123 | Zoom Factor: 1 124 | Enabled: true 125 | Global Options: 126 | Background Color: 48; 48; 48 127 | Fixed Frame: world 128 | Frame Rate: 30 129 | Name: root 130 | Tools: 131 | - Class: rviz/Interact 132 | Hide Inactive Objects: true 133 | - Class: rviz/MoveCamera 134 | - Class: rviz/Select 135 | - Class: rviz/FocusCamera 136 | - Class: rviz/Measure 137 | - Class: rviz/SetInitialPose 138 | Topic: /initialpose 139 | - Class: rviz/SetGoal 140 | Topic: /move_base_simple/goal 141 | - Class: rviz/PublishPoint 142 | Single click: true 143 | Topic: /clicked_point 144 | Value: true 145 | Views: 146 | Current: 147 | Class: rviz/Orbit 148 | Distance: 1.3831408 149 | Enable Stereo Rendering: 150 | Stereo Eye Separation: 0.0599999987 151 | Stereo Focal Distance: 1 152 | Swap Stereo Eyes: false 153 | Value: false 154 | Focal Point: 155 | X: 0.158190101 156 | Y: -0.0226284917 157 | Z: 0.342923284 158 | Focal Shape Fixed Size: true 159 | Focal Shape Size: 0.0500000007 160 | Invert Z Axis: false 161 | Name: Current View 162 | Near Clip Distance: 0.00999999978 163 | Pitch: 0.484797359 164 | Target Frame: 165 | Value: Orbit (rviz) 166 | Yaw: 2.52039075 167 | Saved: ~ 168 | Window Geometry: 169 | Camera: 170 | collapsed: false 171 | Displays: 172 | collapsed: false 173 | Height: 846 174 | Hide Left Dock: false 175 | Hide Right Dock: true 176 | QMainWindow State: 000000ff00000000fd000000040000000000000231000002c4fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000016b000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000199000001530000001600ffffff000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005b60000003efc0100000002fb0000000800540069006d00650100000000000005b60000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000037f000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 177 | Selection: 178 | collapsed: false 179 | Time: 180 | collapsed: false 181 | Tool Properties: 182 | collapsed: false 183 | Views: 184 | collapsed: true 185 | Width: 1462 186 | X: 268 187 | Y: 154 188 | -------------------------------------------------------------------------------- /ch7/robot_vision/config/ar_track_kinect.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Camera1/Visibility1 8 | Splitter Ratio: 0.5 9 | Tree Height: 203 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.588679016 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: Camera 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.5 33 | Cell Size: 1 34 | Class: rviz/Grid 35 | Color: 160; 160; 164 36 | Enabled: true 37 | Line Style: 38 | Line Width: 0.0299999993 39 | Value: Lines 40 | Name: Grid 41 | Normal Cell Count: 0 42 | Offset: 43 | X: 0 44 | Y: 0 45 | Z: 0 46 | Plane: XY 47 | Plane Cell Count: 10 48 | Reference Frame: 49 | Value: true 50 | - Class: rviz/TF 51 | Enabled: true 52 | Frame Timeout: 15 53 | Frames: 54 | All Enabled: true 55 | ar_marker_0: 56 | Value: true 57 | ar_marker_1: 58 | Value: true 59 | ar_marker_2: 60 | Value: true 61 | ar_marker_3: 62 | Value: true 63 | ar_marker_4: 64 | Value: true 65 | ar_marker_5: 66 | Value: true 67 | ar_marker_6: 68 | Value: true 69 | ar_marker_7: 70 | Value: true 71 | ar_marker_8: 72 | Value: true 73 | camera_rgb_optical_frame: 74 | Value: true 75 | world: 76 | Value: true 77 | Marker Scale: 0.300000012 78 | Name: TF 79 | Show Arrows: true 80 | Show Axes: true 81 | Show Names: true 82 | Tree: 83 | world: 84 | camera_rgb_optical_frame: 85 | ar_marker_0: 86 | {} 87 | ar_marker_1: 88 | {} 89 | ar_marker_2: 90 | {} 91 | ar_marker_3: 92 | {} 93 | ar_marker_4: 94 | {} 95 | ar_marker_5: 96 | {} 97 | ar_marker_6: 98 | {} 99 | ar_marker_7: 100 | {} 101 | ar_marker_8: 102 | {} 103 | Update Interval: 0 104 | Value: true 105 | - Class: rviz/Marker 106 | Enabled: true 107 | Marker Topic: /visualization_marker 108 | Name: Marker 109 | Namespaces: 110 | basic_shapes: true 111 | Queue Size: 100 112 | Value: true 113 | - Class: rviz/Camera 114 | Enabled: true 115 | Image Rendering: background and overlay 116 | Image Topic: /camera/rgb/image_color 117 | Name: Camera 118 | Overlay Alpha: 0.5 119 | Queue Size: 2 120 | Transport Hint: raw 121 | Unreliable: false 122 | Value: true 123 | Visibility: 124 | Grid: true 125 | Marker: true 126 | TF: true 127 | Value: true 128 | Zoom Factor: 1 129 | Enabled: true 130 | Global Options: 131 | Background Color: 48; 48; 48 132 | Fixed Frame: world 133 | Frame Rate: 30 134 | Name: root 135 | Tools: 136 | - Class: rviz/Interact 137 | Hide Inactive Objects: true 138 | - Class: rviz/MoveCamera 139 | - Class: rviz/Select 140 | - Class: rviz/FocusCamera 141 | - Class: rviz/Measure 142 | - Class: rviz/SetInitialPose 143 | Topic: /initialpose 144 | - Class: rviz/SetGoal 145 | Topic: /move_base_simple/goal 146 | - Class: rviz/PublishPoint 147 | Single click: true 148 | Topic: /clicked_point 149 | Value: true 150 | Views: 151 | Current: 152 | Class: rviz/Orbit 153 | Distance: 0.970315576 154 | Enable Stereo Rendering: 155 | Stereo Eye Separation: 0.0599999987 156 | Stereo Focal Distance: 1 157 | Swap Stereo Eyes: false 158 | Value: false 159 | Focal Point: 160 | X: 0.158190101 161 | Y: -0.0226284917 162 | Z: 0.342923284 163 | Focal Shape Fixed Size: true 164 | Focal Shape Size: 0.0500000007 165 | Name: Current View 166 | Near Clip Distance: 0.00999999978 167 | Pitch: 0.544797301 168 | Target Frame: 169 | Value: Orbit (rviz) 170 | Yaw: 2.24539399 171 | Saved: ~ 172 | Window Geometry: 173 | Camera: 174 | collapsed: false 175 | Displays: 176 | collapsed: false 177 | Height: 846 178 | Hide Left Dock: false 179 | Hide Right Dock: true 180 | QMainWindow State: 000000ff00000000fd000000040000000000000239000002c4fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000010c000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000013a000001b20000001600ffffff000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f40000003efc0100000002fb0000000800540069006d00650100000000000004f40000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000002b5000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 181 | Selection: 182 | collapsed: false 183 | Time: 184 | collapsed: false 185 | Tool Properties: 186 | collapsed: false 187 | Views: 188 | collapsed: true 189 | Width: 1268 190 | X: 243 191 | Y: 136 192 | -------------------------------------------------------------------------------- /ch7/robot_vision/doc/checkerboard.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch7/robot_vision/doc/checkerboard.pdf -------------------------------------------------------------------------------- /ch7/robot_vision/kinect_depth_calibration.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 488 3 | camera_name: depth_A70774707163327A 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [588.899202, 0.000000, 344.407161, 0.000000, 592.561316, 236.892528, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.033959, 0.002807, 0.004325, 0.008295, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [581.809387, 0.000000, 349.505222, 0.000000, 0.000000, 590.687866, 238.331564, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /ch7/robot_vision/kinect_rgb_calibration.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: rgb_A70774707163327A 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [545.248453, 0.000000, 314.100720, 0.000000, 545.239286, 251.980763, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [0.078522, -0.079510, 0.003257, 0.000524, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [557.277771, 0.000000, 314.428831, 0.000000, 0.000000, 555.953369, 253.544776, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /ch7/robot_vision/launch/ar_track_camera.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /ch7/robot_vision/launch/ar_track_kinect.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /ch7/robot_vision/launch/face_detector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | haar_scaleFactor: 1.2 6 | haar_minNeighbors: 2 7 | haar_minSize: 40 8 | haar_maxSize: 60 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /ch7/robot_vision/launch/freenect.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /ch7/robot_vision/launch/freenect_with_calibration.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 21 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /ch7/robot_vision/launch/motion_detector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | minArea: 500 6 | threshold: 25 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /ch7/robot_vision/launch/usb_cam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /ch7/robot_vision/launch/usb_cam_with_calibration.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /ch7/robot_vision/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_vision 4 | 0.0.0 5 | The robot_vision 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 | cv_bridge 44 | image_transport 45 | roscpp 46 | rospy 47 | sensor_msgs 48 | std_msgs 49 | cv_bridge 50 | image_transport 51 | roscpp 52 | rospy 53 | sensor_msgs 54 | std_msgs 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /ch7/robot_vision/scripts/cv_bridge_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import rospy 5 | import cv2 6 | from cv_bridge import CvBridge, CvBridgeError 7 | from sensor_msgs.msg import Image 8 | 9 | class image_converter: 10 | def __init__(self): 11 | # 创建cv_bridge，声明图像的发布者和订阅者 12 | self.image_pub = rospy.Publisher("cv_bridge_image", Image, queue_size=1) 13 | self.bridge = CvBridge() 14 | self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.callback) 15 | 16 | def callback(self,data): 17 | # 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式 18 | try: 19 | cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") 20 | except CvBridgeError as e: 21 | print e 22 | 23 | # 在opencv的显示窗口中绘制一个圆，作为标记 24 | (rows,cols,channels) = cv_image.shape 25 | if cols > 60 and rows > 60 : 26 | cv2.circle(cv_image, (60, 60), 30, (0,0,255), -1) 27 | 28 | # 显示Opencv格式的图像 29 | cv2.imshow("Image window", cv_image) 30 | cv2.waitKey(3) 31 | 32 | # 再将opencv格式额数据转换成ros image格式的数据发布 33 | try: 34 | self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8")) 35 | except CvBridgeError as e: 36 | print e 37 | 38 | if __name__ == '__main__': 39 | try: 40 | # 初始化ros节点 41 | rospy.init_node("cv_bridge_test") 42 | rospy.loginfo("Starting cv_bridge_test node") 43 | image_converter() 44 | rospy.spin() 45 | except KeyboardInterrupt: 46 | print "Shutting down cv_bridge_test node." 47 | cv2.destroyAllWindows() -------------------------------------------------------------------------------- /ch7/robot_vision/scripts/face_detector.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | import rospy 4 | import cv2 5 | import numpy as np 6 | from sensor_msgs.msg import Image, RegionOfInterest 7 | from cv_bridge import CvBridge, CvBridgeError 8 | from test1.msg import Size 9 | 10 | class faceDetector: 11 | def __init__(self): 12 | rospy.on_shutdown(self.cleanup); 13 | 14 | # 创建cv_bridge 15 | self.bridge = CvBridge() 16 | self.size = Size() 17 | self.image_pub = rospy.Publisher("cv_bridge_image", Image, queue_size=1) 18 | self.size_pub = rospy.Publisher("Size", Size, queue_size=1) 19 | 20 | # 获取haar特征的级联表的XML文件，文件路径在launch文件中传入 21 | cascade_1 = rospy.get_param("~cascade_1", "") 22 | cascade_2 = rospy.get_param("~cascade_2", "") 23 | 24 | # 使用级联表初始化haar特征检测器 25 | self.cascade_1 = cv2.CascadeClassifier(cascade_1) 26 | self.cascade_2 = cv2.CascadeClassifier(cascade_2) 27 | 28 | # 设置级联表的参数，优化人脸识别，可以在launch文件中重新配置 29 | self.haar_scaleFactor = rospy.get_param("~haar_scaleFactor", 1.2) 30 | self.haar_minNeighbors = rospy.get_param("~haar_minNeighbors", 2) 31 | self.haar_minSize = rospy.get_param("~haar_minSize", 40) 32 | self.haar_maxSize = rospy.get_param("~haar_maxSize", 60) 33 | self.color = (50, 255, 50) 34 | 35 | # 初始化订阅rgb格式图像数据的订阅者，此处图像topic的话题名可以在launch文件中重映射 36 | self.image_sub = rospy.Subscriber("input_rgb_image", Image, self.image_callback, queue_size=1) 37 | 38 | def image_callback(self, data): 39 | # 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式 40 | try: 41 | cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") 42 | frame = np.array(cv_image, dtype=np.uint8) 43 | except CvBridgeError, e: 44 | print e 45 | 46 | # 创建灰度图像 47 | grey_image = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) 48 | 49 | # 创建平衡直方图，减少光线影响 50 | grey_image = cv2.equalizeHist(grey_image) 51 | 52 | # 尝试检测人脸 53 | faces_result = self.detect_face(grey_image) 54 | 55 | # 在opencv的窗口中框出所有人脸区域 56 | if len(faces_result)>0: 57 | for face in faces_result: 58 | x, y, w, h = face 59 | self.size.width = abs(int(w)) 60 | self.size.high = abs(int(h)) 61 | self.size.x = abs(int(x)) 62 | self.size_pub.publish(self.size) 63 | cv2.rectangle(cv_image, (x, y), (x+w, y+h), self.color, 2) 64 | 65 | # 将识别后的图像转换成ROS消息并发布 66 | self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8")) 67 | 68 | def detect_face(self, input_image): 69 | # 首先匹配正面人脸的模型 70 | if self.cascade_1: 71 | faces = self.cascade_1.detectMultiScale(input_image, 72 | self.haar_scaleFactor, 73 | self.haar_minNeighbors, 74 | cv2.CASCADE_SCALE_IMAGE, 75 | (self.haar_minSize, self.haar_maxSize)) 76 | 77 | # 如果正面人脸匹配失败，那么就尝试匹配侧面人脸的模型 78 | if len(faces) == 0 and self.cascade_2: 79 | faces = self.cascade_2.detectMultiScale(input_image, 80 | self.haar_scaleFactor, 81 | self.haar_minNeighbors, 82 | cv2.CASCADE_SCALE_IMAGE, 83 | (self.haar_minSize, self.haar_maxSize)) 84 | 85 | return faces 86 | 87 | def cleanup(self): 88 | print "Shutting down vision node." 89 | cv2.destroyAllWindows() 90 | 91 | if __name__ == '__main__': 92 | try: 93 | # 初始化ros节点 94 | rospy.init_node("face_detector") 95 | faceDetector() 96 | rospy.loginfo("Face detector is started..") 97 | rospy.loginfo("Please subscribe the ROS image.") 98 | rospy.spin() 99 | except KeyboardInterrupt: 100 | print "Shutting down face detector node." 101 | cv2.destroyAllWindows() 102 | -------------------------------------------------------------------------------- /ch7/robot_vision/scripts/motion_detector.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | import rospy 4 | import cv2 5 | import numpy as np 6 | from sensor_msgs.msg import Image, RegionOfInterest 7 | from cv_bridge import CvBridge, CvBridgeError 8 | 9 | class motionDetector: 10 | def __init__(self): 11 | rospy.on_shutdown(self.cleanup); 12 | 13 | # 创建cv_bridge 14 | self.bridge = CvBridge() 15 | self.image_pub = rospy.Publisher("cv_bridge_image", Image, queue_size=1) 16 | 17 | # 设置参数：最小区域、阈值 18 | self.minArea = rospy.get_param("~minArea", 500) 19 | self.threshold = rospy.get_param("~threshold", 25) 20 | 21 | self.firstFrame = None 22 | self.text = "Unoccupied" 23 | 24 | # 初始化订阅rgb格式图像数据的订阅者，此处图像topic的话题名可以在launch文件中重映射 25 | self.image_sub = rospy.Subscriber("input_rgb_image", Image, self.image_callback, queue_size=1) 26 | 27 | def image_callback(self, data): 28 | # 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式 29 | try: 30 | cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") 31 | frame = np.array(cv_image, dtype=np.uint8) 32 | except CvBridgeError, e: 33 | print e 34 | 35 | # 创建灰度图像 36 | gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) 37 | gray = cv2.GaussianBlur(gray, (21, 21), 0) 38 | 39 | # 使用两帧图像做比较，检测移动物体的区域 40 | if self.firstFrame is None: 41 | self.firstFrame = gray 42 | return 43 | frameDelta = cv2.absdiff(self.firstFrame, gray) 44 | thresh = cv2.threshold(frameDelta, self.threshold, 255, cv2.THRESH_BINARY)[1] 45 | 46 | thresh = cv2.dilate(thresh, None, iterations=2) 47 | binary, cnts, hierarchy= cv2.findContours(thresh.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) 48 | 49 | for c in cnts: 50 | # 如果检测到的区域小于设置值，则忽略 51 | if cv2.contourArea(c) < self.minArea: 52 | continue 53 | 54 | # 在输出画面上框出识别到的物体 55 | (x, y, w, h) = cv2.boundingRect(c) 56 | cv2.rectangle(frame, (x, y), (x + w, y + h), (50, 255, 50), 2) 57 | self.text = "Occupied" 58 | 59 | # 在输出画面上打当前状态和时间戳信息 60 | cv2.putText(frame, "Status: {}".format(self.text), (10, 20), 61 | cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2) 62 | 63 | # 将识别后的图像转换成ROS消息并发布 64 | self.image_pub.publish(self.bridge.cv2_to_imgmsg(frame, "bgr8")) 65 | 66 | def cleanup(self): 67 | print "Shutting down vision node." 68 | cv2.destroyAllWindows() 69 | 70 | if __name__ == '__main__': 71 | try: 72 | # 初始化ros节点 73 | rospy.init_node("motion_detector") 74 | rospy.loginfo("motion_detector node is started...") 75 | rospy.loginfo("Please subscribe the ROS image.") 76 | motionDetector() 77 | rospy.spin() 78 | except KeyboardInterrupt: 79 | print "Shutting down motion detector node." 80 | cv2.destroyAllWindows() 81 | 82 | -------------------------------------------------------------------------------- /ch7/test1/msg/Size.msg: -------------------------------------------------------------------------------- 1 | int32 width 2 | int32 high 3 | int32 x 4 | -------------------------------------------------------------------------------- /ch7/test1/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | test1 4 | 0.0.0 5 | The test1 package 6 | 7 | 8 | 9 | 10 | bym 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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | roscpp 54 | rospy 55 | std_msgs 56 | message_generation 57 | 58 | geometry_msgs 59 | roscpp 60 | rospy 61 | std_msgs 62 | 63 | geometry_msgs 64 | roscpp 65 | rospy 66 | std_msgs 67 | message_runtime 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | -------------------------------------------------------------------------------- /ch7/test1/src/test1.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "test1/Size.h" 3 | #include "geometry_msgs/Twist.h" 4 | #include "std_msgs/Int32.h" 5 | 6 | int flag = 0; 7 | std_msgs::Int32 size_width, size_high, size_x; 8 | 9 | class SubscribeAndPublish 10 | { 11 | public: 12 | SubscribeAndPublish() 13 | { 14 | pub = n.advertise("cmd_vel", 1); 15 | sub = n.subscribe("Size", 1, &SubscribeAndPublish::callback, this); 16 | } 17 | 18 | void callback(const test1::Size::ConstPtr& size) 19 | { 20 | if(flag==0){ 21 | size_width.data = size->width; 22 | size_high.data = size->high; 23 | size_x.data = size->x; 24 | flag = 1; 25 | } 26 | else{ 27 | if((size->width>size_width.data+50)&&(size->high>size_high.data+50)){ 28 | ROS_INFO("go forward"); 29 | //ROS_INFO("size->width:%d size_width:%d", size->width, size_width.data); 30 | size_width.data = size->width; 31 | size_high.data = size->high; 32 | size_x.data = size->x; 33 | geometry_msgs::Twist msg; 34 | msg.linear.x = 0.3; 35 | msg.angular.z = 0; 36 | pub.publish(msg); 37 | } 38 | else if((size->widthhighwidth; 41 | size_high.data = size->high; 42 | size_x.data = size->x; 43 | geometry_msgs::Twist msg; 44 | msg.linear.x = -0.3; 45 | msg.angular.z = 0; 46 | pub.publish(msg); 47 | } 48 | else if( (size->x + size->width/2) < (size_x.data + size_width.data/2 - 50) ){ 49 | ROS_INFO("turn right"); 50 | size_width.data = size->width; 51 | size_high.data = size->high; 52 | size_x.data = size->x; 53 | geometry_msgs::Twist msg; 54 | msg.linear.x = 0; 55 | msg.angular.z = -0.3; 56 | pub.publish(msg); 57 | } 58 | else if( (size->x + size->width/2) > (size_x.data + size_width.data/2 + 50) ){ 59 | ROS_INFO("turn left"); 60 | size_width.data = size->width; 61 | size_high.data = size->high; 62 | size_x.data = size->x; 63 | geometry_msgs::Twist msg; 64 | msg.linear.x = 0; 65 | msg.angular.z = 0.3; 66 | pub.publish(msg); 67 | } 68 | } 69 | } 70 | 71 | private: 72 | ros::NodeHandle n; 73 | ros::Publisher pub; 74 | ros::Subscriber sub; 75 | 76 | }; 77 | 78 | int main(int argc, char **argv) 79 | { 80 | ros::init(argc, argv, "test1"); 81 | SubscribeAndPublish SAPObject; 82 | ros::spin(); 83 | 84 | return 0; 85 | } 86 | -------------------------------------------------------------------------------- /ch7/test2/msg/pose.msg: -------------------------------------------------------------------------------- 1 | int64 pose_x 2 | int64 pose_y 3 | int64 size_x 4 | int64 size_y 5 | string id 6 | -------------------------------------------------------------------------------- /ch7/test2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | test2 4 | 0.0.0 5 | The test2 package 6 | 7 | 8 | 9 | 10 | bym 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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | roscpp 54 | std_msgs 55 | vision_msgs 56 | message_generation 57 | 58 | geometry_msgs 59 | roscpp 60 | std_msgs 61 | 62 | geometry_msgs 63 | roscpp 64 | std_msgs 65 | vision_msgs 66 | message_runtime 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /ch7/test2/src/test2.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/Int64.h" 3 | #include "std_msgs/String.h" 4 | #include "geometry_msgs/Twist.h" 5 | #include "test2/pose.h" 6 | 7 | int flag = 0; 8 | std_msgs::Int64 pose_width, pose_high, pose_x; 9 | 10 | class SubscribeAndPublish 11 | { 12 | public: 13 | SubscribeAndPublish() 14 | { 15 | pub = n.advertise("cmd_vel", 1); 16 | sub = n.subscribe("pose", 1, &SubscribeAndPublish::callback, this); 17 | } 18 | 19 | void callback(const test2::pose::ConstPtr& pose) 20 | { 21 | if(flag==0){ 22 | pose_width.data = pose->size_x; 23 | pose_high.data = pose->size_y; 24 | pose_x.data = pose->pose_x; 25 | flag = 1; 26 | } 27 | else{ 28 | if((pose->size_x>pose_width.data+20)&&(pose->size_y>pose_high.data+20)){ 29 | ROS_INFO("go forward"); 30 | pose_width.data = pose->size_x; 31 | pose_high.data = pose->size_y; 32 | pose_x.data = pose->pose_x; 33 | geometry_msgs::Twist msg; 34 | msg.linear.x = 0.3; 35 | msg.angular.z = 0; 36 | pub.publish(msg); 37 | } 38 | else if((pose->size_xsize_ysize_x; 41 | pose_high.data = pose->size_y; 42 | pose_x.data = pose->pose_x; 43 | geometry_msgs::Twist msg; 44 | msg.linear.x = -0.3; 45 | msg.angular.z = 0; 46 | pub.publish(msg); 47 | } 48 | else if( (pose->pose_x + pose->size_x/2) < (pose_x.data + pose_width.data/2 - 30) ){ 49 | ROS_INFO("turn left"); 50 | pose_width.data = pose->size_x; 51 | pose_high.data = pose->size_y; 52 | pose_x.data = pose->pose_x; 53 | geometry_msgs::Twist msg; 54 | msg.linear.x = 0; 55 | msg.angular.z = 0.3; 56 | pub.publish(msg); 57 | } 58 | else if( (pose->pose_x + pose->size_x/2) > (pose_x.data + pose_width.data/2 + 30) ){ 59 | ROS_INFO("turn right"); 60 | pose_width.data = pose->size_x; 61 | pose_high.data = pose->size_y; 62 | pose_x.data = pose->pose_x; 63 | geometry_msgs::Twist msg; 64 | msg.linear.x = 0; 65 | msg.angular.z = -0.3; 66 | pub.publish(msg); 67 | } 68 | } 69 | } 70 | 71 | private: 72 | ros::NodeHandle n; 73 | ros::Publisher pub; 74 | ros::Subscriber sub; 75 | 76 | }; 77 | 78 | int main(int argc, char** argv){ 79 | ros::init(argc, argv, "test2"); 80 | SubscribeAndPublish SAPObject; 81 | ros::spin(); 82 | 83 | return 0; 84 | } 85 | -------------------------------------------------------------------------------- /ch8/libmsc.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch8/libmsc.so -------------------------------------------------------------------------------- /ch8/robot_voice/include/robot_voice/formats.h: -------------------------------------------------------------------------------- 1 | #ifndef FORMATS_H_160601_TT 2 | #define FORMATS_H_160601_TT 1 3 | 4 | #ifndef WAVE_FORMAT_PCM 5 | #define WAVE_FORMAT_PCM 1 6 | typedef struct tWAVEFORMATEX { 7 | unsigned short wFormatTag; 8 | unsigned short nChannels; 9 | unsigned int nSamplesPerSec; 10 | unsigned int nAvgBytesPerSec; 11 | unsigned short nBlockAlign; 12 | unsigned short wBitsPerSample; 13 | unsigned short cbSize; 14 | } WAVEFORMATEX; 15 | #endif 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /ch8/robot_voice/include/robot_voice/linuxrec.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @file 3 | * @brief a record demo in linux 4 | * 5 | * a simple record code. using alsa-lib APIs. 6 | * keep the function same as winrec.h 7 | * 8 | * Common steps: 9 | * create_recorder, 10 | * open_recorder, 11 | * start_record, 12 | * stop_record, 13 | * close_recorder, 14 | * destroy_recorder 15 | * 16 | * @author taozhang9 17 | * @date 2016/06/01 18 | */ 19 | 20 | #ifndef __IFLY_WINREC_H__ 21 | #define __IFLY_WINREC_H__ 22 | 23 | #include "formats.h" 24 | /* error code */ 25 | enum { 26 | RECORD_ERR_BASE = 0, 27 | RECORD_ERR_GENERAL, 28 | RECORD_ERR_MEMFAIL, 29 | RECORD_ERR_INVAL, 30 | RECORD_ERR_NOT_READY 31 | }; 32 | 33 | typedef struct { 34 | union { 35 | char * name; 36 | int index; 37 | void * resv; 38 | }u; 39 | }record_dev_id; 40 | 41 | /* recorder object. */ 42 | struct recorder { 43 | void (*on_data_ind)(char *data, unsigned long len, void *user_para); 44 | void * user_cb_para; 45 | volatile int state; /* internal record state */ 46 | 47 | void * wavein_hdl; 48 | /* thread id may be a struct. by implementation 49 | * void * will not be ported!! */ 50 | pthread_t rec_thread; 51 | /*void * rec_thread_hdl;*/ 52 | 53 | void * bufheader; 54 | unsigned int bufcount; 55 | 56 | char *audiobuf; 57 | int bits_per_frame; 58 | unsigned int buffer_time; 59 | unsigned int period_time; 60 | size_t period_frames; 61 | size_t buffer_frames; 62 | }; 63 | 64 | #ifdef __cplusplus 65 | extern "C" { 66 | #endif /* C++ */ 67 | 68 | /** 69 | * @fn 70 | * @brief Get the default input device ID 71 | * 72 | * @return returns "default" in linux. 73 | * 74 | */ 75 | record_dev_id get_default_input_dev(); 76 | 77 | /** 78 | * @fn 79 | * @brief Get the total number of active input devices. 80 | * @return 81 | */ 82 | int get_input_dev_num(); 83 | 84 | /** 85 | * @fn 86 | * @brief Create a recorder object. 87 | * 88 | * Never call the close_recorder in the callback function. as close 89 | * action will wait for the callback thread to quit. 90 | * 91 | * @return int - Return 0 in success, otherwise return error code. 92 | * @param out_rec - [out] recorder object holder 93 | * @param on_data_ind - [in] callback. called when data coming. 94 | * @param user_cb_para - [in] user params for the callback. 95 | * @see 96 | */ 97 | int create_recorder(struct recorder ** out_rec, 98 | void (*on_data_ind)(char *data, unsigned long len, void *user_para), 99 | void* user_cb_para); 100 | 101 | /** 102 | * @fn 103 | * @brief Destroy recorder object. free memory. 104 | * @param rec - [in]recorder object 105 | */ 106 | void destroy_recorder(struct recorder *rec); 107 | 108 | /** 109 | * @fn 110 | * @brief open the device. 111 | * @return int - Return 0 in success, otherwise return error code. 112 | * @param rec - [in] recorder object 113 | * @param dev - [in] device id, from 0. 114 | * @param fmt - [in] record format. 115 | * @see 116 | * get_default_input_dev() 117 | */ 118 | int open_recorder(struct recorder * rec, record_dev_id dev, WAVEFORMATEX * fmt); 119 | 120 | /** 121 | * @fn 122 | * @brief close the device. 123 | * @param rec - [in] recorder object 124 | */ 125 | 126 | void close_recorder(struct recorder *rec); 127 | 128 | /** 129 | * @fn 130 | * @brief start record. 131 | * @return int - Return 0 in success, otherwise return error code. 132 | * @param rec - [in] recorder object 133 | */ 134 | int start_record(struct recorder * rec); 135 | 136 | /** 137 | * @fn 138 | * @brief stop record. 139 | * @return int - Return 0 in success, otherwise return error code. 140 | * @param rec - [in] recorder object 141 | */ 142 | int stop_record(struct recorder * rec); 143 | 144 | /** 145 | * @fn 146 | * @brief test if the recording has been stopped. 147 | * @return int - 1: stopped. 0 : recording. 148 | * @param rec - [in] recorder object 149 | */ 150 | int is_record_stopped(struct recorder *rec); 151 | 152 | #ifdef __cplusplus 153 | } /* extern "C" */ 154 | #endif /* C++ */ 155 | 156 | #endif 157 | -------------------------------------------------------------------------------- /ch8/robot_voice/include/robot_voice/msp_errors.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch8/robot_voice/include/robot_voice/msp_errors.h -------------------------------------------------------------------------------- /ch8/robot_voice/include/robot_voice/msp_types.h: -------------------------------------------------------------------------------- 1 | #ifndef __MSP_TYPES_H__ 2 | #define __MSP_TYPES_H__ 3 | 4 | #if !defined(MSPAPI) 5 | #if defined(WIN32) || defined(WINPHONE8) || defined(WIN8) 6 | #define MSPAPI __stdcall 7 | #else 8 | #define MSPAPI 9 | #endif /* WIN32 */ 10 | #endif /* MSPAPI */ 11 | 12 | 13 | /** 14 | * MSPSampleStatus indicates how the sample buffer should be handled 15 | * MSP_AUDIO_SAMPLE_FIRST - The sample buffer is the start of audio 16 | * If recognizer was already recognizing, it will discard 17 | * audio received to date and re-start the recognition 18 | * MSP_AUDIO_SAMPLE_CONTINUE - The sample buffer is continuing audio 19 | * MSP_AUDIO_SAMPLE_LAST - The sample buffer is the end of audio 20 | * The recognizer will cease processing audio and 21 | * return results 22 | * Note that sample statii can be combined; for example, for file-based input 23 | * the entire file can be written with SAMPLE_FIRST | SAMPLE_LAST as the 24 | * status. 25 | * Other flags may be added in future to indicate other special audio 26 | * conditions such as the presence of AGC 27 | */ 28 | enum 29 | { 30 | MSP_AUDIO_SAMPLE_INIT = 0x00, 31 | MSP_AUDIO_SAMPLE_FIRST = 0x01, 32 | MSP_AUDIO_SAMPLE_CONTINUE = 0x02, 33 | MSP_AUDIO_SAMPLE_LAST = 0x04, 34 | }; 35 | 36 | /* 37 | * The enumeration MSPRecognizerStatus contains the recognition status 38 | * MSP_REC_STATUS_SUCCESS - successful recognition with partial results 39 | * MSP_REC_STATUS_NO_MATCH - recognition rejected 40 | * MSP_REC_STATUS_INCOMPLETE - recognizer needs more time to compute results 41 | * MSP_REC_STATUS_NON_SPEECH_DETECTED - discard status, no more in use 42 | * MSP_REC_STATUS_SPEECH_DETECTED - recognizer has detected audio, this is delayed status 43 | * MSP_REC_STATUS_COMPLETE - recognizer has return all result 44 | * MSP_REC_STATUS_MAX_CPU_TIME - CPU time limit exceeded 45 | * MSP_REC_STATUS_MAX_SPEECH - maximum speech length exceeded, partial results may be returned 46 | * MSP_REC_STATUS_STOPPED - recognition was stopped 47 | * MSP_REC_STATUS_REJECTED - recognizer rejected due to low confidence 48 | * MSP_REC_STATUS_NO_SPEECH_FOUND - recognizer still found no audio, this is delayed status 49 | */ 50 | enum 51 | { 52 | MSP_REC_STATUS_SUCCESS = 0, 53 | MSP_REC_STATUS_NO_MATCH = 1, 54 | MSP_REC_STATUS_INCOMPLETE = 2, 55 | MSP_REC_STATUS_NON_SPEECH_DETECTED = 3, 56 | MSP_REC_STATUS_SPEECH_DETECTED = 4, 57 | MSP_REC_STATUS_COMPLETE = 5, 58 | MSP_REC_STATUS_MAX_CPU_TIME = 6, 59 | MSP_REC_STATUS_MAX_SPEECH = 7, 60 | MSP_REC_STATUS_STOPPED = 8, 61 | MSP_REC_STATUS_REJECTED = 9, 62 | MSP_REC_STATUS_NO_SPEECH_FOUND = 10, 63 | MSP_REC_STATUS_FAILURE = MSP_REC_STATUS_NO_MATCH, 64 | }; 65 | 66 | /** 67 | * The enumeration MSPepState contains the current endpointer state 68 | * MSP_EP_LOOKING_FOR_SPEECH - Have not yet found the beginning of speech 69 | * MSP_EP_IN_SPEECH - Have found the beginning, but not the end of speech 70 | * MSP_EP_AFTER_SPEECH - Have found the beginning and end of speech 71 | * MSP_EP_TIMEOUT - Have not found any audio till timeout 72 | * MSP_EP_ERROR - The endpointer has encountered a serious error 73 | * MSP_EP_MAX_SPEECH - Have arrive the max size of speech 74 | */ 75 | enum 76 | { 77 | MSP_EP_LOOKING_FOR_SPEECH = 0, 78 | MSP_EP_IN_SPEECH = 1, 79 | MSP_EP_AFTER_SPEECH = 3, 80 | MSP_EP_TIMEOUT = 4, 81 | MSP_EP_ERROR = 5, 82 | MSP_EP_MAX_SPEECH = 6, 83 | MSP_EP_IDLE = 7 // internal state after stop and before start 84 | }; 85 | 86 | /* Synthesizing process flags */ 87 | enum 88 | { 89 | MSP_TTS_FLAG_STILL_HAVE_DATA = 1, 90 | MSP_TTS_FLAG_DATA_END = 2, 91 | MSP_TTS_FLAG_CMD_CANCELED = 4, 92 | }; 93 | 94 | /* Handwriting process flags */ 95 | enum 96 | { 97 | MSP_HCR_DATA_FIRST = 1, 98 | MSP_HCR_DATA_CONTINUE = 2, 99 | MSP_HCR_DATA_END = 4, 100 | }; 101 | 102 | /*ivw message type */ 103 | enum 104 | { 105 | MSP_IVW_MSG_WAKEUP = 1, 106 | MSP_IVW_MSG_ERROR = 2, 107 | MSP_IVW_MSG_ISR_RESULT = 3, 108 | MSP_IVW_MSG_ISR_EPS = 4, 109 | MSP_IVW_MSG_VOLUME = 5, 110 | MSP_IVW_MSG_ENROLL = 6, 111 | MSP_IVW_MSG_RESET = 7 112 | }; 113 | 114 | /* Upload data process flags */ 115 | enum 116 | { 117 | MSP_DATA_SAMPLE_INIT = 0x00, 118 | MSP_DATA_SAMPLE_FIRST = 0x01, 119 | MSP_DATA_SAMPLE_CONTINUE = 0x02, 120 | MSP_DATA_SAMPLE_LAST = 0x04, 121 | }; 122 | 123 | #endif /* __MSP_TYPES_H__ */ 124 | -------------------------------------------------------------------------------- /ch8/robot_voice/include/robot_voice/speech_recognizer.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch8/robot_voice/include/robot_voice/speech_recognizer.h -------------------------------------------------------------------------------- /ch8/robot_voice/launch/repeat_voice.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /ch8/robot_voice/launch/voice_assistant.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /ch8/robot_voice/launch/voice_teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /ch8/robot_voice/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_voice 4 | 0.0.0 5 | The robot_voice package 6 | 7 | 8 | 9 | 10 | bym 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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | std_msgs 54 | roscpp 55 | std_msgs 56 | roscpp 57 | std_msgs 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /ch8/robot_voice/src/tts_subscribe.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * 语音合成（Text To Speech，TTS）技术能够自动将任意文字实时转换为连续的 3 | * 自然语音，是一种能够在任何时间、任何地点，向任何人提供语音信息服务的 4 | * 高效便捷手段，非常符合信息时代海量数据、动态更新和个性化查询的需求。 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "robot_voice/qtts.h" 13 | #include "robot_voice/msp_cmn.h" 14 | #include "robot_voice/msp_errors.h" 15 | 16 | #include "ros/ros.h" 17 | #include "std_msgs/String.h" 18 | 19 | /* wav音频头部格式 */ 20 | typedef struct _wave_pcm_hdr 21 | { 22 | char riff[4]; // = "RIFF" 23 | int size_8; // = FileSize - 8 24 | char wave[4]; // = "WAVE" 25 | char fmt[4]; // = "fmt " 26 | int fmt_size; // = 下一个结构体的大小 : 16 27 | 28 | short int format_tag; // = PCM : 1 29 | short int channels; // = 通道数 : 1 30 | int samples_per_sec; // = 采样率 : 8000 | 6000 | 11025 | 16000 31 | int avg_bytes_per_sec; // = 每秒字节数 : samples_per_sec * bits_per_sample / 8 32 | short int block_align; // = 每采样点字节数 : wBitsPerSample / 8 33 | short int bits_per_sample; // = 量化比特数: 8 | 16 34 | 35 | char data[4]; // = "data"; 36 | int data_size; // = 纯数据长度 : FileSize - 44 37 | } wave_pcm_hdr; 38 | 39 | /* 默认wav音频头部数据 */ 40 | wave_pcm_hdr default_wav_hdr = 41 | { 42 | { 'R', 'I', 'F', 'F' }, 43 | 0, 44 | {'W', 'A', 'V', 'E'}, 45 | {'f', 'm', 't', ' '}, 46 | 16, 47 | 1, 48 | 1, 49 | 16000, 50 | 32000, 51 | 2, 52 | 16, 53 | {'d', 'a', 't', 'a'}, 54 | 0 55 | }; 56 | /* 文本合成 */ 57 | int text_to_speech(const char* src_text, const char* des_path, const char* params) 58 | { 59 | int ret = -1; 60 | FILE* fp = NULL; 61 | const char* sessionID = NULL; 62 | unsigned int audio_len = 0; 63 | wave_pcm_hdr wav_hdr = default_wav_hdr; 64 | int synth_status = MSP_TTS_FLAG_STILL_HAVE_DATA; 65 | 66 | if (NULL == src_text || NULL == des_path) 67 | { 68 | printf("params is error!\n"); 69 | return ret; 70 | } 71 | fp = fopen(des_path, "wb"); 72 | if (NULL == fp) 73 | { 74 | printf("open %s error.\n", des_path); 75 | return ret; 76 | } 77 | /* 开始合成 */ 78 | sessionID = QTTSSessionBegin(params, &ret); 79 | if (MSP_SUCCESS != ret) 80 | { 81 | printf("QTTSSessionBegin failed, error code: %d.\n", ret); 82 | fclose(fp); 83 | return ret; 84 | } 85 | ret = QTTSTextPut(sessionID, src_text, (unsigned int)strlen(src_text), NULL); 86 | if (MSP_SUCCESS != ret) 87 | { 88 | printf("QTTSTextPut failed, error code: %d.\n",ret); 89 | QTTSSessionEnd(sessionID, "TextPutError"); 90 | fclose(fp); 91 | return ret; 92 | } 93 | printf("正在合成 ...\n"); 94 | fwrite(&wav_hdr, sizeof(wav_hdr) ,1, fp); //添加wav音频头，使用采样率为16000 95 | while (1) 96 | { 97 | /* 获取合成音频 */ 98 | const void* data = QTTSAudioGet(sessionID, &audio_len, &synth_status, &ret); 99 | if (MSP_SUCCESS != ret) 100 | break; 101 | if (NULL != data) 102 | { 103 | fwrite(data, audio_len, 1, fp); 104 | wav_hdr.data_size += audio_len; //计算data_size大小 105 | } 106 | if (MSP_TTS_FLAG_DATA_END == synth_status) 107 | break; 108 | printf(">"); 109 | usleep(150*1000); //防止频繁占用CPU 110 | } 111 | printf("\n"); 112 | if (MSP_SUCCESS != ret) 113 | { 114 | printf("QTTSAudioGet failed, error code: %d.\n",ret); 115 | QTTSSessionEnd(sessionID, "AudioGetError"); 116 | fclose(fp); 117 | return ret; 118 | } 119 | /* 修正wav文件头数据的大小 */ 120 | wav_hdr.size_8 += wav_hdr.data_size + (sizeof(wav_hdr) - 8); 121 | 122 | /* 将修正过的数据写回文件头部,音频文件为wav格式 */ 123 | fseek(fp, 4, 0); 124 | fwrite(&wav_hdr.size_8,sizeof(wav_hdr.size_8), 1, fp); //写入size_8的值 125 | fseek(fp, 40, 0); //将文件指针偏移到存储data_size值的位置 126 | fwrite(&wav_hdr.data_size,sizeof(wav_hdr.data_size), 1, fp); //写入data_size的值 127 | fclose(fp); 128 | fp = NULL; 129 | /* 合成完毕 */ 130 | ret = QTTSSessionEnd(sessionID, "Normal"); 131 | if (MSP_SUCCESS != ret) 132 | { 133 | printf("QTTSSessionEnd failed, error code: %d.\n",ret); 134 | } 135 | 136 | return ret; 137 | } 138 | 139 | void voiceWordsCallback(const std_msgs::String::ConstPtr& msg) 140 | { 141 | char cmd[2000]; 142 | const char* text; 143 | int ret = MSP_SUCCESS; 144 | const char* session_begin_params = "voice_name = xiaoyan, text_encoding = utf8, sample_rate = 16000, speed = 50, volume = 50, pitch = 50, rdn = 2"; 145 | const char* filename = "tts_sample.wav"; //合成的语音文件名称 146 | 147 | std::cout<<"I heard :"<data.c_str()<data.c_str(); 149 | 150 | /* 文本合成 */ 151 | printf("开始合成 ...\n"); 152 | ret = text_to_speech(text, filename, session_begin_params); 153 | if (MSP_SUCCESS != ret) 154 | { 155 | printf("text_to_speech failed, error code: %d.\n", ret); 156 | } 157 | printf("合成完毕\n"); 158 | 159 | popen("play tts_sample.wav","r"); 160 | sleep(1); 161 | } 162 | 163 | void toExit() 164 | { 165 | printf("按任意键退出 ...\n"); 166 | getchar(); 167 | MSPLogout(); //退出登录 168 | } 169 | 170 | int main(int argc, char* argv[]) 171 | { 172 | int ret = MSP_SUCCESS; 173 | const char* login_params = "appid = 5d3ff09c, work_dir = .";//登录参数,appid与msc库绑定,请勿随意改动 174 | /* 175 | * rdn: 合成音频数字发音方式 176 | * volume: 合成音频的音量 177 | * pitch: 合成音频的音调 178 | * speed: 合成音频对应的语速 179 | * voice_name: 合成发音人 180 | * sample_rate: 合成音频采样率 181 | * text_encoding: 合成文本编码格式 182 | * 183 | */ 184 | //const char* session_begin_params = "voice_name = xiaoyan, text_encoding = utf8, sample_rate = 16000, speed = 50, volume = 50, pitch = 50, rdn = 2"; 185 | //const char* filename = "tts_sample.wav"; //合成的语音文件名称 186 | //const char* text = "亲爱的用户，您好，这是一个语音合成示例，感谢您对科大讯飞语音技术的支持！科大讯飞是亚太地区最大的语音上市公司，股票代码：002230"; //合成文本 187 | 188 | /* 用户登录 */ 189 | ret = MSPLogin(NULL, NULL, login_params);//第一个参数是用户名，第二个参数是密码，第三个参数是登录参数，用户名和密码可在http://www.xfyun.cn注册获取 190 | if (MSP_SUCCESS != ret) 191 | { 192 | printf("MSPLogin failed, error code: %d.\n", ret); 193 | //goto exit ;//登录失败，退出登录 194 | toExit(); 195 | } 196 | printf("\n###########################################################################\n"); 197 | printf("## 语音合成（Text To Speech，TTS）技术能够自动将任意文字实时转换为连续的 ##\n"); 198 | printf("## 自然语音，是一种能够在任何时间、任何地点，向任何人提供语音信息服务的 ##\n"); 199 | printf("## 高效便捷手段，非常符合信息时代海量数据、动态更新和个性化查询的需求。 ##\n"); 200 | printf("###########################################################################\n\n"); 201 | 202 | ros::init(argc,argv,"TextToSpeech"); 203 | ros::NodeHandle n; 204 | ros::Subscriber sub =n.subscribe("voiceWords", 1000, voiceWordsCallback); 205 | ros::spin(); 206 | 207 | exit: 208 | printf("按任意键退出 ...\n"); 209 | getchar(); 210 | MSPLogout(); //退出登录 211 | 212 | return 0; 213 | } 214 | -------------------------------------------------------------------------------- /ch8/robot_voice/src/voice_teleop.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "geometry_msgs/Twist.h" 3 | #include "std_msgs/String.h" 4 | 5 | class SubscribeAndPublish 6 | { 7 | public: 8 | SubscribeAndPublish() 9 | { 10 | pub = n.advertise("cmd_vel", 1000); 11 | sub = n.subscribe("voiceWords", 1000, &SubscribeAndPublish::callback, this); 12 | } 13 | 14 | void callback(const std_msgs::String::ConstPtr& msg) 15 | { 16 | std::string dataString = msg->data; 17 | if(dataString.find("前进") != std::string::npos || dataString.find("向前") != std::string::npos){ 18 | //ROS_INFO("go forward"); 19 | geometry_msgs::Twist msg; 20 | msg.linear.x = 0.3; 21 | msg.angular.z = 0; 22 | pub.publish(msg); 23 | } 24 | else if(dataString.find("后退") != std::string::npos || dataString.find("向后") != std::string::npos){ 25 | //ROS_INFO("go back"); 26 | geometry_msgs::Twist msg; 27 | msg.linear.x = -0.3; 28 | msg.angular.z = 0; 29 | pub.publish(msg); 30 | } 31 | else if(dataString.find("左转") != std::string::npos || dataString.find("向左") != std::string::npos){ 32 | //ROS_INFO("turn right"); 33 | geometry_msgs::Twist msg; 34 | msg.linear.x = 0; 35 | msg.angular.z = 0.3; 36 | pub.publish(msg); 37 | } 38 | else if(dataString.find("右转") != std::string::npos || dataString.find("向左") != std::string::npos){ 39 | //ROS_INFO("turn left"); 40 | geometry_msgs::Twist msg; 41 | msg.linear.x = 0; 42 | msg.angular.z = -0.3; 43 | pub.publish(msg); 44 | } 45 | else if(dataString.find("转圈") != std::string::npos || dataString.find("转圈圈") != std::string::npos){ 46 | geometry_msgs::Twist msg; 47 | msg.linear.x = 0.3; 48 | msg.angular.z = 0.3; 49 | pub.publish(msg); 50 | } 51 | } 52 | 53 | private: 54 | ros::NodeHandle n; 55 | ros::Publisher pub; 56 | ros::Subscriber sub; 57 | 58 | }; 59 | 60 | int main(int argc, char **argv) 61 | { 62 | ros::init(argc, argv, "voice_teleop"); 63 | SubscribeAndPublish SAPObject; 64 | ros::spin(); 65 | 66 | return 0; 67 | } 68 | -------------------------------------------------------------------------------- /ch9/robot_description/meshes/kinect.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_description/meshes/kinect.jpg -------------------------------------------------------------------------------- /ch9/robot_description/meshes/kinect.tga: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_description/meshes/kinect.tga -------------------------------------------------------------------------------- /ch9/robot_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_description 4 | 0.0.0 5 | The robot_description package 6 | 7 | 8 | 9 | 10 | yuemingbi 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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | urdf 53 | xacro 54 | urdf 55 | xacro 56 | urdf 57 | xacro 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /ch9/robot_description/urdf/robot.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /ch9/robot_description/urdf/robot_rplidar.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /ch9/robot_description/urdf/sensor/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 | -------------------------------------------------------------------------------- /ch9/robot_description/urdf/sensor/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 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(robot_gazebo) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | rospy 7 | std_msgs 8 | message_generation 9 | controller_manager 10 | gazebo_ros 11 | ) 12 | 13 | catkin_package( 14 | CATKIN_DEPENDS urdf xacro 15 | ) 16 | 17 | 18 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/launch/robot_rplidar_nav.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 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/launch/robot_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/launch/view_robot_gazebo_empty_world.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 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/launch/view_robot_rplidar.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 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/ISCAS_Museum/meshes/ISCASmuseum.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 10 3 | 4 | newmtl FBXASC0514SG 5 | Ns 0.000000 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.500000 0.500000 0.500000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl FBXASC229FBXASC175FBXASC185FBXASC232FBXASC177FBXASC161022SG 15 | Ns 0.000000 16 | Ka 1.000000 1.000000 1.000000 17 | Kd 0.000000 0.000000 0.000000 18 | Ks 0.000000 0.000000 0.000000 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | 24 | newmtl FBXASC229FBXASC175FBXASC185FBXASC232FBXASC177FBXASC161022SG5 25 | Ns 0.000000 26 | Ka 1.000000 1.000000 1.000000 27 | Kd 0.640000 0.640000 0.640000 28 | Ks 0.000000 0.000000 0.000000 29 | Ke 0.000000 0.000000 0.000000 30 | Ni 1.000000 31 | d 1.000000 32 | illum 2 33 | map_Kd zd_011.jpg 34 | 35 | newmtl FBXASC229FBXASC175FBXASC185FBXASC232FBXASC177FBXASC161033SG 36 | Ns 0.000000 37 | Ka 1.000000 1.000000 1.000000 38 | Kd 0.640000 0.640000 0.640000 39 | Ks 0.000000 0.000000 0.000000 40 | Ke 0.000000 0.000000 0.000000 41 | Ni 1.000000 42 | d 1.000000 43 | illum 2 44 | map_Kd zd_021.jpg 45 | 46 | newmtl Line001SG 47 | Ns 0.000000 48 | Ka 1.000000 1.000000 1.000000 49 | Kd 0.640000 0.640000 0.640000 50 | Ks 0.000000 0.000000 0.000000 51 | Ke 0.000000 0.000000 0.000000 52 | Ni 1.000000 53 | d 1.000000 54 | illum 2 55 | map_Kd zd_041.jpg 56 | 57 | newmtl duim001SG 58 | Ns 0.000000 59 | Ka 1.000000 1.000000 1.000000 60 | Kd 0.640000 0.640000 0.640000 61 | Ks 0.000000 0.000000 0.000000 62 | Ke 0.000000 0.000000 0.000000 63 | Ni 1.000000 64 | d 1.000000 65 | illum 2 66 | map_Kd zd_031.jpg 67 | 68 | newmtl men050SG 69 | Ns 0.000000 70 | Ka 1.000000 1.000000 1.000000 71 | Kd 0.590000 0.590000 0.590000 72 | Ks 0.000000 0.000000 0.000000 73 | Ke 0.000000 0.000000 0.000000 74 | Ni 1.000000 75 | d 1.000000 76 | illum 2 77 | 78 | newmtl men050SG2 79 | Ns 0.000000 80 | Ka 1.000000 1.000000 1.000000 81 | Kd 0.590000 0.590000 0.590000 82 | Ks 0.000000 0.000000 0.000000 83 | Ke 0.000000 0.000000 0.000000 84 | Ni 1.000000 85 | d 1.000000 86 | illum 2 87 | 88 | newmtl men050SG3 89 | Ns 0.000000 90 | Ka 1.000000 1.000000 1.000000 91 | Kd 0.590000 0.590000 0.590000 92 | Ks 0.000000 0.000000 0.000000 93 | Ke 0.000000 0.000000 0.000000 94 | Ni 1.000000 95 | d 1.000000 96 | illum 2 97 | 98 | newmtl yuan50SG 99 | Ns 0.000000 100 | Ka 1.000000 1.000000 1.000000 101 | Kd 0.870000 0.810000 0.410000 102 | Ks 0.000000 0.000000 0.000000 103 | Ke 0.000000 0.000000 0.000000 104 | Ni 1.000000 105 | d 1.000000 106 | illum 2 107 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/ISCAS_Museum/meshes/zd_011.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_gazebo/models/ISCAS_Museum/meshes/zd_011.jpg -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/ISCAS_Museum/meshes/zd_021.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_gazebo/models/ISCAS_Museum/meshes/zd_021.jpg -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/ISCAS_Museum/meshes/zd_031.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_gazebo/models/ISCAS_Museum/meshes/zd_031.jpg -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/ISCAS_Museum/meshes/zd_041.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_gazebo/models/ISCAS_Museum/meshes/zd_041.jpg -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/ISCAS_Museum/meshes/轮胎贴图.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_gazebo/models/ISCAS_Museum/meshes/轮胎贴图.jpg -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/ISCAS_Museum/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | ISCAS_Museum 4 | 1.0 5 | model.sdf 6 | 7 | Changkun Chai 8 | chaichangkun@163.com 9 | 10 | 11 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/ISCAS_Museum/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 15 7 | 8 | 0.0 9 | 0.0 10 | 0.0 11 | 0.0 12 | 0.0 13 | 0.0 14 | 15 | 16 | 17 | 0 0 0 0 0 0 18 | 19 | 20 | model://ISCAS_Museum/meshes/ISCASmuseum.obj 21 | 22 | 23 | 24 | 25 | 0 0 0 0 0 0 26 | 27 | 28 | model://ISCAS_Museum/meshes/ISCASmuseum.obj 29 | 30 | 31 | 32 | 33 | 1 34 | 35 | 36 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/ISCAS_groundplane/materials/scripts/ISCAS_groundplane.material: -------------------------------------------------------------------------------- 1 | material ISCAS_groundplane/Image 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | ambient .8 .8 .8 1.000000 8 | diffuse .5 .5 .5 1.000000 9 | specular 1 1 1 5.00000 10 | emissive 0.01 0.01 0.01 1.000000 11 | 12 | texture_unit 13 | { 14 | texture ISCAS_groundplane.png 15 | filtering trilinear 16 | scale 0.12 0.12 17 | } 18 | } 19 | } 20 | } -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/ISCAS_groundplane/materials/textures/ISCAS_groundplane.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_gazebo/models/ISCAS_groundplane/materials/textures/ISCAS_groundplane.png -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/ISCAS_groundplane/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | ISCAS_groundplane 4 | 1.0 5 | model.sdf 6 | 7 | Changkun Chai 8 | chaichangkun@163.com 9 | 10 | 11 | The ground texture of ISCAS Museum. 12 | 13 | 14 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/ISCAS_groundplane/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 0 0 1 10 | 30 30 11 | 12 | 13 | 14 | 15 | 16 | 100 17 | 50 18 | 19 | 20 | 21 | 22 | 23 | false 24 | 25 | 26 | 0 0 1 27 | 30 30 28 | 29 | 30 | 31 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/ISCAS_post/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ISCAS_post 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Changkun 10 | chaichangkun@163.com 11 | 12 | 13 | 14 | A model of the post in ISCAS Museum. 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/ISCAS_post/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 0 0 1.375 0 0.02 0 8 | 9 | 10 | 0.9 0.04 1.15 11 | 12 | 13 | 14 | 15 | 16 | 0 0 0.4 0 0 0 17 | 18 | 19 | 0.02 0.02 0.8 20 | 21 | 22 | 23 | 24 | 25 | 0 0 0.02 0 0 0 26 | 27 | 28 | 0.4 0.3 0.04 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | model://ISCAS_post/meshes/post.dae 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/meshes/.mayaSwatches/uvtietu.jpg.swatch: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_gazebo/models/meshes/.mayaSwatches/uvtietu.jpg.swatch -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/meshes/base_link.mtl: -------------------------------------------------------------------------------- 1 | newmtl group_pasted__group33_pasted__nurbsToPoly153SG1 2 | illum 4 3 | Kd 0.00 0.00 0.00 4 | Ka 1.00 1.00 1.00 5 | Tf 1.00 1.00 1.00 6 | map_Kd uvtietu.jpg 7 | Ni 1.00 8 | Ks 0.80 0.80 0.80 9 | Ns 98.00 10 | newmtl group_pasted__group33_pasted__pasted__polySurface25SG1 11 | illum 4 12 | Kd 0.00 0.00 0.00 13 | Ka 1.00 1.00 1.00 14 | Tf 0.00 0.00 0.00 15 | map_Kd uvtietu.jpg 16 | Ni 1.00 17 | Ks 0.50 0.50 0.50 18 | Ns 48.00 19 | newmtl group_pasted__pasted__pasted__nurbsToPoly1_pasted__pasted__grouSG1 20 | illum 4 21 | Kd 0.00 0.00 0.00 22 | Ka 1.00 1.00 1.00 23 | Tf 1.00 1.00 1.00 24 | map_Kd uvtietu.jpg 25 | Ni 1.00 26 | Ks 0.50 0.50 0.50 27 | Ns 48.00 28 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/meshes/pitch_platform.mtl: -------------------------------------------------------------------------------- 1 | newmtl Pitch_PlatformSG 2 | illum 4 3 | Kd 0.16 0.16 0.16 4 | Ka 0.30 0.30 0.30 5 | Tf 1.00 1.00 1.00 6 | Ni 1.00 7 | Ks 0.16 0.16 0.16 8 | Ns 48.00 9 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/meshes/realsense.mtl: -------------------------------------------------------------------------------- 1 | newmtl group_pasted__nurbsToPoly438SG 2 | illum 4 3 | Kd 0.71 0.71 0.71 4 | Ka 0.50 0.50 0.50 5 | Tf 1.00 1.00 1.00 6 | Ni 1.00 7 | Ks 0.06 0.06 0.06 8 | Ns 48.00 9 | newmtl group_pasted__nurbsToPoly443SG 10 | illum 4 11 | Kd 0.06 0.06 0.06 12 | Ka 0.20 0.20 0.20 13 | Tf 1.00 1.00 1.00 14 | Ni 1.00 15 | Ks 0.50 0.50 0.50 16 | Ns 48.00 17 | newmtl group_pasted__nurbsToPoly465SG 18 | illum 4 19 | Kd 0.15 0.15 0.15 20 | Ka 0.50 0.50 0.50 21 | Tf 1.00 1.00 1.00 22 | Ni 1.00 23 | Ks 0.06 0.06 0.06 24 | Ns 48.00 25 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/meshes/rplidar.mtl: -------------------------------------------------------------------------------- 1 | newmtl pasted__polySurface29SG 2 | illum 4 3 | Kd 0.62 0.00 0.00 4 | Ka 0.80 0.80 0.80 5 | Tf 1.00 1.00 1.00 6 | Ni 1.00 7 | Ks 27.98 27.98 27.98 8 | Ns 509.00 9 | newmtl pasted__polySurface33SG 10 | illum 4 11 | Kd 0.62 0.00 0.00 12 | Ka 0.70 0.70 0.70 13 | Tf 1.00 1.00 1.00 14 | Ni 1.00 15 | Ks 27.98 27.98 27.98 16 | Ns 509.00 17 | newmtl pasted__polySurface34SG 18 | illum 4 19 | Kd 0.04 0.05 0.10 20 | Ka 0.70 0.70 0.70 21 | Tf 1.00 1.00 1.00 22 | Ni 1.00 23 | Ks 0.50 0.50 0.50 24 | Ns 48.00 25 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/meshes/uvtietu.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_gazebo/models/meshes/uvtietu.jpg -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/meshes/wheel.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_gazebo/models/meshes/wheel.jpg -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/meshes/wheel.mtl: -------------------------------------------------------------------------------- 1 | newmtl wheel_wheel_pasted__polySurface1SG 2 | illum 4 3 | Kd 0.00 0.00 0.00 4 | Ka 1.00 1.00 1.00 5 | Tf 1.00 1.00 1.00 6 | map_Kd wheel.jpg 7 | Ni 1.00 8 | Ks 0.16 0.16 0.16 9 | Ns 48.00 10 | newmtl wheel_wheel_pasted__polySurface5SG 11 | illum 4 12 | Kd 0.05 0.05 0.05 13 | Ka 0.40 0.40 0.40 14 | Tf 1.00 1.00 1.00 15 | Ni 1.00 16 | Ks 0.22 0.22 0.22 17 | Ns -1.00 18 | newmtl wheel_wheel_pasted__polySurface7SG 19 | illum 4 20 | Kd 0.60 0.60 0.60 21 | Ka 0.80 0.80 0.80 22 | Tf 1.00 1.00 1.00 23 | Ni 1.00 24 | Ks 0.66 0.66 0.66 25 | Ns 48.00 26 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/meshes/xtion_pro_camera.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_gazebo/models/meshes/xtion_pro_camera.jpg -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/meshes/xtion_pro_camera.mtl: -------------------------------------------------------------------------------- 1 | newmtl xtionFBXASC045proFBXASC045cameraSG1 2 | illum 4 3 | Kd 0.00 0.00 0.00 4 | Ka 0.89 0.89 0.89 5 | Tf 1.00 1.00 1.00 6 | map_Kd xtion_pro_camera.jpg 7 | Ni 1.00 8 | Ks 0.90 0.90 0.90 9 | Ns -2.00 10 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/meshes/yaw_platform.mtl: -------------------------------------------------------------------------------- 1 | newmtl pasted__nurbsToPoly383SG 2 | illum 4 3 | Kd 0.16 0.16 0.16 4 | Ka 0.30 0.30 0.30 5 | Tf 1.00 1.00 1.00 6 | Ni 1.00 7 | Ks 0.16 0.16 0.16 8 | Ns 48.00 9 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/sai_di/meshes/tielouti.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_gazebo/models/sai_di/meshes/tielouti.jpg -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/sai_di/meshes/tietu3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_gazebo/models/sai_di/meshes/tietu3.jpg -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/sai_di/meshes/不锈钢.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_gazebo/models/sai_di/meshes/不锈钢.JPG -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/sai_di/meshes/地板底.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_gazebo/models/sai_di/meshes/地板底.jpg -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/sai_di/meshes/贴图1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_gazebo/models/sai_di/meshes/贴图1.jpg -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/sai_di/meshes/贴图2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_gazebo/models/sai_di/meshes/贴图2.jpg -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/sai_di/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | sai_di 4 | 1.0 5 | model.sdf 6 | 7 | Zhou 8 | zhouxiaorui@buaa.edu.cn 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/models/sai_di/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 15 7 | 8 | 0.0 9 | 0.0 10 | 0.0 11 | 0.0 12 | 0.0 13 | 0.0 14 | 15 | 16 | 17 | 0 0 0 0 0 0 18 | 19 | 20 | model://sai_di/meshes/sai_di1.dae 21 | 22 | 23 | 24 | 25 | 26 | 0 0 0 0 0 0 27 | 28 | 29 | model://sai_di/meshes/sai_di1.dae 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 5 39 | 40 | 0.0 41 | 0.0 42 | 0.0 43 | 0.0 44 | 0.0 45 | 0.0 46 | 47 | 48 | 49 | 0 0 0 0 0 0 50 | 51 | 52 | model://sai_di/meshes/sai_di2.dae 53 | 54 | 55 | 56 | 57 | 58 | 0 0 0 0 0 0 59 | 60 | 61 | model://sai_di/meshes/sai_di2.dae 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 5 71 | 72 | 0.0 73 | 0.0 74 | 0.0 75 | 0.0 76 | 0.0 77 | 0.0 78 | 79 | 80 | 81 | 0 0 0 0 0 0 82 | 83 | 84 | model://sai_di/meshes/sai_di3.dae 85 | 86 | 87 | 88 | 89 | 90 | 0 0 0 0 0 0 91 | 92 | 93 | model://sai_di/meshes/sai_di3.dae 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 5 103 | 104 | 0.0 105 | 0.0 106 | 0.0 107 | 0.0 108 | 0.0 109 | 0.0 110 | 111 | 112 | 113 | 0 0 0 0 0 0 114 | 115 | 116 | model://sai_di/meshes/sai_di4.dae 117 | 118 | 119 | 120 | 121 | 122 | 0 0 0 0 0 0 123 | 124 | 125 | model://sai_di/meshes/sai_di4.dae 126 | 127 | 128 | 129 | 130 | 131 | 132 | 1 133 | 134 | 135 | 136 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | robot_gazebo 3 | 0.0.1 4 | The robot_gazebo package 5 | 6 | 7 | 8 | 9 | yuemingbi 10 | 11 | 12 | 13 | 14 | 15 | TODO 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 | catkin 51 | roscpp 52 | rospy 53 | std_msgs 54 | message_generation 55 | gazebo_ros 56 | 57 | roscpp 58 | rospy 59 | std_msgs 60 | controller_manager 61 | urdf 62 | effort_controllers 63 | gazebo_plugins 64 | gazebo_ros 65 | gazebo_ros_control 66 | joint_state_controller 67 | joint_state_publisher 68 | robot_state_publisher 69 | yocs_cmd_vel_mux 70 | message_runtime 71 | xacro 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /ch9/robot_gazebo/worlds/museum.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0.68 0.68 0.68 1.0 6 | false 7 | 8 | 9 | 10 | 12 11 | 12 | 13 | 14 | 15 | 16 | 0.001 17 | 1 18 | 1000 19 | 0 0 -9.8 20 | 21 | 22 | 23 | model://sun 24 | 25 | 26 | 27 | model://ISCAS_groundplane 28 | 29 | 30 | 31 | model://ISCAS_Museum 32 | 0 0 0 0 0 -1.57 33 | 34 | 35 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /ch9/robot_navigation/config/lidar.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 | include "map_builder.lua" 16 | include "trajectory_builder.lua" 17 | 18 | options = { 19 | map_builder = MAP_BUILDER, 20 | trajectory_builder = TRAJECTORY_BUILDER, 21 | map_frame = "map", 22 | tracking_frame = "base_footprint", 23 | published_frame = "base_footprint", 24 | odom_frame = "odom", 25 | provide_odom_frame = true, 26 | publish_frame_projected_to_2d = false, 27 | use_odometry = true, 28 | use_nav_sat = false, 29 | use_landmarks = false, 30 | num_laser_scans = 1, 31 | num_multi_echo_laser_scans = 0, 32 | num_subdivisions_per_laser_scan = 1, 33 | num_point_clouds = 0, 34 | lookup_transform_timeout_sec = 0.2, 35 | submap_publish_period_sec = 0.3, 36 | pose_publish_period_sec = 5e-3, 37 | trajectory_publish_period_sec = 30e-3, 38 | rangefinder_sampling_ratio = 1., 39 | odometry_sampling_ratio = 1., 40 | fixed_frame_pose_sampling_ratio = 1., 41 | imu_sampling_ratio = 1., 42 | landmarks_sampling_ratio = 1., 43 | } 44 | 45 | MAP_BUILDER.use_trajectory_builder_2d = true 46 | 47 | TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35 48 | TRAJECTORY_BUILDER_2D.min_range = 0.3 49 | TRAJECTORY_BUILDER_2D.max_range = 8. 50 | TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1. 51 | TRAJECTORY_BUILDER_2D.use_imu_data = false 52 | TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 53 | TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1 54 | TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10. 55 | TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1 56 | 57 | POSE_GRAPH.optimization_problem.huber_scale = 1e2 58 | POSE_GRAPH.optimize_every_n_nodes = 35 59 | POSE_GRAPH.constraint_builder.min_score = 0.65 60 | 61 | return options 62 | -------------------------------------------------------------------------------- /ch9/robot_navigation/config/robot/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | TrajectoryPlannerROS: 2 | 3 | # Robot Configuration Parameters 4 | max_vel_x: 0.18 5 | min_vel_x: 0.08 6 | 7 | max_vel_theta: 1.0 8 | min_vel_theta: -1.0 9 | min_in_place_vel_theta: 1.0 10 | 11 | acc_lim_x: 1.0 12 | acc_lim_y: 0.0 13 | acc_lim_theta: 0.6 14 | 15 | meter_scoring: true 16 | 17 | # Goal Tolerance Parameters 18 | xy_goal_tolerance: 0.10 19 | yaw_goal_tolerance: 0.05 20 | 21 | # Differential-drive robot configuration 22 | holonomic_robot: false 23 | 24 | # Forward Simulation Parameters 25 | sim_time: 0.8 26 | vx_samples: 18 27 | vtheta_samples: 20 28 | sim_granularity: 0.05 29 | -------------------------------------------------------------------------------- /ch9/robot_navigation/config/robot/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: 1.0 7 | cost_scaling_factor: 3.0 8 | map_type: costmap 9 | #transform_tolerance: 1 10 | observation_sources: scan 11 | scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true} 12 | -------------------------------------------------------------------------------- /ch9/robot_navigation/config/robot/dwa_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | DWAPlannerROS: 2 | 3 | # Robot Configuration Parameters 4 | max_vel_x: 0.22 5 | min_vel_x: -0.22 6 | 7 | max_vel_y: 0.0 8 | min_vel_y: 0.0 9 | 10 | # The velocity when robot is moving in a straight line 11 | max_trans_vel: 0.22 12 | min_trans_vel: 0.11 13 | 14 | max_rot_vel: 2.75 15 | min_rot_vel: 1.37 16 | 17 | acc_lim_x: 2.5 18 | acc_lim_y: 0.0 19 | acc_lim_theta: 3.2 20 | 21 | # Goal Tolerance Parametes 22 | xy_goal_tolerance: 0.05 23 | yaw_goal_tolerance: 0.17 24 | latch_xy_goal_tolerance: false 25 | 26 | # Forward Simulation Parameters 27 | sim_time: 1.5 28 | vx_samples: 20 29 | vy_samples: 0 30 | vth_samples: 40 31 | controller_frequency: 10.0 32 | 33 | # Trajectory Scoring Parameters 34 | path_distance_bias: 32.0 35 | goal_distance_bias: 20.0 36 | occdist_scale: 0.02 37 | forward_point_distance: 0.325 38 | stop_time_buffer: 0.2 39 | scaling_speed: 0.25 40 | max_scaling_factor: 0.2 41 | 42 | # Oscillation Prevention Parameters 43 | oscillation_reset_dist: 0.05 44 | 45 | # Debugging 46 | publish_traj_pc : true 47 | publish_cost_grid_pc: true 48 | -------------------------------------------------------------------------------- /ch9/robot_navigation/config/robot/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | robot_base_frame: base_footprint 4 | 5 | update_frequency: 10.0 6 | publish_frequency: 10.0 7 | transform_tolerance: 0.5 8 | 9 | static_map: true 10 | 11 | -------------------------------------------------------------------------------- /ch9/robot_navigation/config/robot/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: base_footprint 4 | 5 | update_frequency: 10.0 6 | publish_frequency: 10.0 7 | transform_tolerance: 0.5 8 | 9 | static_map: false 10 | rolling_window: true 11 | width: 3 12 | height: 3 13 | resolution: 0.05 14 | -------------------------------------------------------------------------------- /ch9/robot_navigation/config/robot/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | shutdown_costmaps: false 2 | controller_frequency: 10.0 3 | planner_patience: 5.0 4 | controller_patience: 15.0 5 | conservative_reset_dist: 3.0 6 | planner_frequency: 5.0 7 | oscillation_timeout: 10.0 8 | oscillation_distance: 0.2 9 | 10 | -------------------------------------------------------------------------------- /ch9/robot_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 | 40 | 41 | -------------------------------------------------------------------------------- /ch9/robot_navigation/launch/gmapping_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /ch9/robot_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 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /ch9/robot_navigation/launch/hector_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /ch9/robot_navigation/launch/include/move_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /ch9/robot_navigation/maps/museum_gmapping.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_navigation/maps/museum_gmapping.pgm -------------------------------------------------------------------------------- /ch9/robot_navigation/maps/museum_gmapping.yaml: -------------------------------------------------------------------------------- 1 | image: map_gmapping.pgm 2 | resolution: 0.050000 3 | origin: [-25.000000, -25.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /ch9/robot_navigation/maps/museum_hector.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_navigation/maps/museum_hector.pgm -------------------------------------------------------------------------------- /ch9/robot_navigation/maps/museum_hector.yaml: -------------------------------------------------------------------------------- 1 | image: map_hector.pgm 2 | resolution: 0.050000 3 | origin: [-25.000000, -25.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /ch9/robot_navigation/maps/room_gmapping.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/ch9/robot_navigation/maps/room_gmapping.pgm -------------------------------------------------------------------------------- /ch9/robot_navigation/maps/room_gmapping.yaml: -------------------------------------------------------------------------------- 1 | image: room_gmapping.pgm 2 | resolution: 0.050000 3 | origin: [-12.200000, -1.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /ch9/robot_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_navigation 4 | 0.0.0 5 | The robot_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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | move_base_msgs 54 | roscpp 55 | rospy 56 | geometry_msgs 57 | move_base_msgs 58 | roscpp 59 | rospy 60 | geometry_msgs 61 | move_base_msgs 62 | roscpp 63 | rospy 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /ch9/robot_navigation/param/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | max_obstacle_height: 2.40 # assume something like an arm is mounted on top of the robot 2 | 3 | # Obstacle Cost Shaping (http://wiki.ros.org/costmap_2d/hydro/inflation) 4 | robot_radius: 0.25 # distance a circular robot should be clear of the obstacle (kobuki: 0.18) 5 | # footprint: [[x0, y0], [x1, y1], ... [xn, yn]] # if the robot is not circular 6 | 7 | map_type: voxel 8 | 9 | voxel_layer: 10 | enabled: true 11 | max_obstacle_height: 2.2 12 | origin_z: 0.0 13 | z_resolution: 0.1 14 | z_voxels: 22 15 | unknown_threshold: 15 16 | mark_threshold: 0 17 | combination_method: 1 18 | track_unknown_space: true #true needed for disabling global path planning through unknown space 19 | obstacle_range: 2.5 20 | raytrace_range: 3.0 21 | publish_voxel_map: true 22 | observation_sources: scan camera #bump 23 | scan: 24 | data_type: LaserScan 25 | topic: scan 26 | marking: true 27 | clearing: true 28 | min_obstacle_height: 0.1 29 | max_obstacle_height: 0.3 30 | camera: 31 | data_type: PointCloud2 32 | topic: camera/depth/points 33 | marking: true 34 | clearing: true 35 | min_obstacle_height: 0.0 36 | max_obstacle_height: 2.0 37 | #bump: 38 | # data_type: PointCloud2 39 | # topic: mobile_base/sensors/bumper_pointcloud 40 | # marking: true 41 | # clearing: false 42 | # min_obstacle_height: 0.0 43 | # max_obstacle_height: 0.15 44 | # for debugging only, let's you see the entire voxel grid 45 | 46 | #cost_scaling_factor and inflation_radius were now moved to the inflation_layer ns 47 | inflation_layer: 48 | enabled: true 49 | cost_scaling_factor: 2.58 # exponential rate at which the obstacle cost drops off (default: 10) 50 | inflation_radius: 1.0 # max. distance from an obstacle at which costs are incurred for planning paths. 51 | 52 | static_layer: 53 | enabled: true 54 | 55 | -------------------------------------------------------------------------------- /ch9/robot_navigation/param/dwa_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | DWAPlannerROS: 2 | 3 | # Robot Configuration Parameters 4 | max_vel_x: 0.5 5 | min_vel_x: 0.0 6 | 7 | max_vel_y: 0.0 8 | min_vel_y: 0.0 9 | 10 | # The velocity when robot is moving in a straight line 11 | max_trans_vel: 0.55 12 | min_trans_vel: 0.1 13 | trans_stopped_vel: 0.1 14 | 15 | max_rot_vel: 5.0 16 | min_rot_vel: 0.8 17 | rot_stopped_vel: 0.8 18 | 19 | acc_lim_x: 1.0 20 | acc_lim_theta: 2.0 21 | acc_lim_y: 0.0 22 | 23 | # Goal Tolerance Parametes 24 | yaw_goal_tolerance: 0.3 25 | xy_goal_tolerance: 0.15 26 | # latch_xy_goal_tolerance: false 27 | 28 | # Forward Simulation Parameters 29 | sim_time: 4.0 30 | vx_samples: 20 31 | vy_samples: 0 32 | vtheta_samples: 40 33 | 34 | # Trajectory Scoring Parameters 35 | path_distance_bias: 32.0 36 | goal_distance_bias: 24.0 37 | occdist_scale: 0.1 38 | forward_point_distance: 0.325 39 | stop_time_buffer: 0.2 40 | scaling_speed: 0.25 41 | max_scaling_factor: 0.2 42 | 43 | # Oscillation Prevention Parameters 44 | oscillation_reset_dist: 0.05 45 | 46 | # Debugging 47 | publish_traj_pc : true 48 | publish_cost_grid_pc: true 49 | global_frame_id: odom -------------------------------------------------------------------------------- /ch9/robot_navigation/param/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_footprint 4 | update_frequency: 2.0 5 | publish_frequency: 0.5 6 | static_map: false 7 | rolling_window: false 8 | transform_tolerance: 0.5 9 | plugins: 10 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 11 | - {name: voxel_layer, type: "costmap_2d::VoxelLayer"} 12 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 13 | -------------------------------------------------------------------------------- /ch9/robot_navigation/param/global_planner_params.yaml: -------------------------------------------------------------------------------- 1 | GlobalPlanner: # Also see: http://wiki.ros.org/global_planner 2 | old_navfn_behavior: false # Exactly mirror behavior of navfn, use defaults for other boolean parameters, default false 3 | use_quadratic: true # Use the quadratic approximation of the potential. Otherwise, use a simpler calculation, default true 4 | use_dijkstra: false # Use dijkstra's algorithm. Otherwise, A*, default true 5 | use_grid_path: false # Create a path that follows the grid boundaries. Otherwise, use a gradient descent method, default false 6 | 7 | allow_unknown: true # Allow planner to plan through unknown space, default true 8 | #Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work 9 | planner_window_x: 0.0 # default 0.0 10 | planner_window_y: 0.0 # default 0.0 11 | default_tolerance: 0.0 # If goal in obstacle, plan to the closest point in radius default_tolerance, default 0.0 12 | 13 | publish_scale: 100 # Scale by which the published potential gets multiplied, default 100 14 | planner_costmap_publish_frequency: 0.0 # default 0.0 15 | 16 | lethal_cost: 253 # default 253 17 | neutral_cost: 50 # default 50 18 | cost_factor: 3.0 # Factor to multiply each cost from costmap by, default 3.0 19 | publish_potential: true # Publish Potential Costmap (this is not like the navfn pointcloud2 potential), default true -------------------------------------------------------------------------------- /ch9/robot_navigation/param/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_footprint 4 | update_frequency: 5.0 5 | publish_frequency: 2.0 6 | static_map: false 7 | rolling_window: true 8 | width: 4.0 9 | height: 4.0 10 | resolution: 0.05 11 | origin_x: 5.0 12 | origin_y: 0 13 | transform_tolerance: 0.5 14 | plugins: 15 | - {name: voxel_layer, type: "costmap_2d::VoxelLayer"} 16 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} -------------------------------------------------------------------------------- /ch9/robot_navigation/param/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | # Move base node parameters. For full documentation of the parameters in this file, please see 2 | # 3 | # http://www.ros.org/wiki/move_base 4 | # 5 | shutdown_costmaps: false 6 | 7 | controller_frequency: 5.0 8 | controller_patience: 3.0 9 | 10 | 11 | planner_frequency: 1.0 12 | planner_patience: 5.0 13 | 14 | oscillation_timeout: 10.0 15 | oscillation_distance: 0.2 16 | 17 | # local planner - default is trajectory rollout 18 | base_local_planner: "dwa_local_planner/DWAPlannerROS" 19 | 20 | base_global_planner: "navfn/NavfnROS" #alternatives: global_planner/GlobalPlanner, carrot_planner/CarrotPlanner 21 | 22 | 23 | #We plan to integrate recovery behaviors for turtlebot but currently those belong to gopher and still have to be adapted. 24 | ## recovery behaviors; we avoid spinning, but we need a fall-back replanning 25 | #recovery_behavior_enabled: true 26 | 27 | #recovery_behaviors: 28 | #- name: 'super_conservative_reset1' 29 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 30 | #- name: 'conservative_reset1' 31 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 32 | #- name: 'aggressive_reset1' 33 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 34 | #- name: 'clearing_rotation1' 35 | #type: 'rotate_recovery/RotateRecovery' 36 | #- name: 'super_conservative_reset2' 37 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 38 | #- name: 'conservative_reset2' 39 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 40 | #- name: 'aggressive_reset2' 41 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 42 | #- name: 'clearing_rotation2' 43 | #type: 'rotate_recovery/RotateRecovery' 44 | 45 | #super_conservative_reset1: 46 | #reset_distance: 3.0 47 | #conservative_reset1: 48 | #reset_distance: 1.5 49 | #aggressive_reset1: 50 | #reset_distance: 0.0 51 | #super_conservative_reset2: 52 | #reset_distance: 3.0 53 | #conservative_reset2: 54 | #reset_distance: 1.5 55 | #aggressive_reset2: 56 | #reset_distance: 0.0 -------------------------------------------------------------------------------- /ch9/robot_navigation/param/navfn_global_planner_params.yaml: -------------------------------------------------------------------------------- 1 | 2 | NavfnROS: 3 | visualize_potential: false #Publish potential for rviz as pointcloud2, not really helpful, default false 4 | allow_unknown: true #Specifies whether or not to allow navfn to create plans that traverse unknown space, default true 5 | #Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work 6 | planner_window_x: 0.0 #Specifies the x size of an optional window to restrict the planner to, default 0.0 7 | planner_window_y: 0.0 #Specifies the y size of an optional window to restrict the planner to, default 0.0 8 | 9 | default_tolerance: 0.0 #If the goal is in an obstacle, the planer will plan to the nearest point in the radius of default_tolerance, default 0.0 10 | #The area is always searched, so could be slow for big values 11 | -------------------------------------------------------------------------------- /ch9/robot_teleop/launch/robot_teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /ch9/robot_teleop/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_teleop 4 | 0.0.0 5 | The robot_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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | roscpp 54 | rospy 55 | geometry_msgs 56 | roscpp 57 | rospy 58 | geometry_msgs 59 | roscpp 60 | rospy 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /ch9/robot_teleop/scripts/robot_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('robot_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 | -------------------------------------------------------------------------------- /linux_raisimCheckMyMachine: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YuemingBi/ros_practice/a4ef1b8b79c0c74d1e0587a6799b3ac7c5a4a394/linux_raisimCheckMyMachine --------------------------------------------------------------------------------