├── param ├── kp.json ├── greet.json ├── dummy.yaml ├── amcl_config.yaml ├── global_costmap_params.yaml ├── local_costmap_params.yaml ├── smoother.yaml ├── trajectory_local_planner_params.yaml ├── navfn_global_planner_params.yaml ├── global_planner_params.yaml ├── costmap_common_params.yaml ├── move_base_params.yaml └── dwa_local_planner_params.yaml ├── debian ├── compat ├── source │ ├── format │ └── options ├── changelog ├── control └── rules ├── map ├── compete.pgm ├── 807compete.pgm ├── compete.yaml └── 807compete.yaml ├── launch ├── rviz_build_map.launch ├── rviz_input_keypoint.launch ├── include │ ├── map_server.launch │ ├── velocity_smoother.launch.xml │ ├── move_base.launch.xml │ └── amcl.launch.xml ├── input_keypoints.launch ├── navi_carto.launch ├── build_map_carto.launch ├── build_map.launch └── demo.launch ├── src ├── test.cpp ├── auto_launch.cpp ├── autoCycle.cpp ├── greet_one.cpp ├── environment.cpp ├── pub_3d_model.cpp └── auto_dock.cpp ├── README.md ├── include └── auto_dock.h ├── script ├── input_keypoint.py └── demo.py ├── package.xml ├── CMakeLists.txt └── rviz ├── input_keypoints.rviz ├── build_map.rviz └── xbot_input_goal.rviz /param/kp.json: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /debian/compat: -------------------------------------------------------------------------------- 1 | 9 -------------------------------------------------------------------------------- /param/greet.json: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /debian/source/format: -------------------------------------------------------------------------------- 1 | 3.0 (native) 2 | -------------------------------------------------------------------------------- /param/dummy.yaml: -------------------------------------------------------------------------------- 1 | #A dummy file loaded when no custom param file is given 2 | -------------------------------------------------------------------------------- /map/compete.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DroidAITech/xbot_navi/HEAD/map/compete.pgm -------------------------------------------------------------------------------- /map/807compete.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DroidAITech/xbot_navi/HEAD/map/807compete.pgm -------------------------------------------------------------------------------- /map/compete.yaml: -------------------------------------------------------------------------------- 1 | image: compete.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 | -------------------------------------------------------------------------------- /map/807compete.yaml: -------------------------------------------------------------------------------- 1 | image: 807compete.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 | -------------------------------------------------------------------------------- /launch/rviz_build_map.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /debian/changelog: -------------------------------------------------------------------------------- 1 | ros-kinetic-xbot-navi (0.0.1) stable; urgency=high 2 | 3 | * Autogenerated, no changelog for this version found in CHANGELOG.rst. 4 | 5 | -- lee Thu, 16 Aug 2018 08:08:11 -0000 6 | 7 | 8 | -------------------------------------------------------------------------------- /launch/rviz_input_keypoint.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /debian/source/options: -------------------------------------------------------------------------------- 1 | # Automatically add upstream changes to the quilt overlay. 2 | # http://manpages.ubuntu.com/manpages/trusty/man1/dpkg-source.1.html 3 | # This supports reusing the orig.tar.gz for debian increments. 4 | auto-commit 5 | 6 | -------------------------------------------------------------------------------- /launch/include/map_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /param/amcl_config.yaml: -------------------------------------------------------------------------------- 1 | scan_topic: scan 2 | use_map_topic: true 3 | initial_pose_x: 0.0 4 | initial_pose_y: 0.0 5 | initial_pose_a: 0 6 | pose: 7 | pose: 8 | position: 9 | x: -10.5727825165 10 | y: -3.69539213181 11 | z: 0.0 12 | orientation: 13 | x: 0.0 14 | y: 0.0 15 | z: 0.813573610538 16 | w: 0.581461933608 -------------------------------------------------------------------------------- /param/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_footprint 4 | update_frequency: 5.0 5 | publish_frequency: 5.0 6 | static_map: true 7 | transform_tolerance: 0.5 8 | plugins: 9 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 10 | - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} 11 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 12 | 13 | -------------------------------------------------------------------------------- /param/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 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: 8.0 9 | height: 8.0 10 | resolution: 0.05 11 | transform_tolerance: 0.5 12 | plugins: 13 | - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} 14 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 15 | -------------------------------------------------------------------------------- /launch/input_keypoints.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /param/smoother.yaml: -------------------------------------------------------------------------------- 1 | # Default parameters used by the yocs_velocity_smoother module. 2 | # This isn't used by minimal.launch per se, rather by everything 3 | # which runs on top. 4 | 5 | # Mandatory parameters 6 | speed_lim_v: 0.8 7 | speed_lim_w: 5.4 8 | 9 | accel_lim_v: 1.0 # maximum is actually 2.0, but we push it down to be smooth 10 | accel_lim_w: 2.0 11 | 12 | # Optional parameters 13 | frequency: 20.0 14 | decel_factor: 1.5 15 | 16 | # Robot velocity feedback type: 17 | # 0 - none (default) 18 | # 1 - odometry 19 | # 2 - end robot commands 20 | robot_feedback: 1 21 | -------------------------------------------------------------------------------- /launch/navi_carto.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/test.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | 4 | int main(int argc, char **argv) 5 | { 6 | ros::init(argc, argv, "test"); 7 | ros::NodeHandle nh; 8 | 9 | ros::Publisher chatter_pub = nh.advertise("chatter", 1000); 10 | 11 | ros::Rate loop_rate(10); 12 | // while (ros::ok()) 13 | // { 14 | std_msgs::String msg; 15 | msg.data = "hello world"; 16 | 17 | chatter_pub.publish(msg); 18 | 19 | ros::spinOnce(); 20 | 21 | loop_rate.sleep(); 22 | // } 23 | 24 | return 0; 25 | } 26 | -------------------------------------------------------------------------------- /launch/build_map_carto.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/auto_launch.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/UInt32.h" 3 | 4 | int main(int argc, char **argv) 5 | { 6 | ros::init(argc, argv, "auto_launch"); 7 | ros::NodeHandle nh; 8 | 9 | ros::Publisher auto_launch_pub = nh.advertise("/office/next_loop", 1000); 10 | 11 | ros::Rate loop_rate(1); 12 | int wait_time = 255+60; 13 | while (wait_time >254) 14 | { 15 | std_msgs::UInt32 msg; 16 | msg.data = wait_time; 17 | auto_launch_pub.publish(msg); 18 | ros::spinOnce(); 19 | wait_time--; 20 | 21 | loop_rate.sleep(); 22 | } 23 | 24 | return 0; 25 | } 26 | -------------------------------------------------------------------------------- /debian/control: -------------------------------------------------------------------------------- 1 | Source: ros-kinetic-xbot-navi 2 | Section: misc 3 | Priority: extra 4 | Maintainer: lee 5 | Build-Depends: debhelper (>= 9.0.0), ros-kinetic-catkin, ros-kinetic-roscpp, ros-kinetic-rospy, ros-kinetic-std-msgs, ros-kinetic-sensor-msgs, ros-kinetic-visualization-msgs, ros-kinetic-xbot-talker, ros-kinetic-xbot-face 6 | Homepage: 7 | Standards-Version: 3.9.2 8 | 9 | Package: ros-kinetic-xbot-navi 10 | Architecture: any 11 | Depends: ${shlibs:Depends}, ${misc:Depends}, ros-kinetic-catkin, ros-kinetic-roscpp, ros-kinetic-rospy, ros-kinetic-std-msgs, ros-kinetic-sensor-msgs, ros-kinetic-visualization-msgs, ros-kinetic-xbot-talker, ros-kinetic-xbot-face 12 | Description: The xbot_navi package 13 | -------------------------------------------------------------------------------- /launch/include/velocity_smoother.launch.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/autoCycle.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "ros/ros.h" 3 | #include "std_msgs/String.h" 4 | 5 | int main(int argc, char **argv) 6 | { 7 | ros::init(argc, argv, "auto_launch"); 8 | ros::NodeHandle nh; 9 | 10 | ros::Publisher auto_cycle_pub = nh.advertise("/office/goal_name", 1000); 11 | 12 | ros::Rate loop_rate(1); 13 | int wait_time = 10; 14 | while (wait_time) 15 | { 16 | std_msgs::String msg; 17 | if(wait_time > 1) 18 | { 19 | msg.data = "nothing"; 20 | } 21 | else{ 22 | msg.data = "A"; 23 | } 24 | auto_cycle_pub.publish(msg); 25 | wait_time--; 26 | 27 | loop_rate.sleep(); 28 | } 29 | 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /launch/build_map.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 | -------------------------------------------------------------------------------- /param/trajectory_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | TrajectoryPlannerROS: 2 | 3 | # Robot Configuration Parameters - Kobuki 4 | max_vel_x: 0.3 # 0.55 5 | min_vel_x: 0.0 6 | 7 | max_vel_y: 0.0 # diff drive robot 8 | min_vel_y: 0.0 # diff drive robot 9 | 10 | 11 | 12 | # Warning! 13 | # do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities 14 | # are non-negligible and small in place rotational velocities will be created. 15 | 16 | max_vel_theta: 0.5 # choose slightly less than the base's capability 17 | min_vel_theta: -0.5 # this is the min angular velocity when there is negligible translational velocity 18 | 19 | 20 | acc_lim_x: 0.3 # maximum is theoretically 2.0, but we 21 | acc_lim_theta: 1.0 22 | acc_lim_y: 0.0 # diff drive robot 23 | 24 | 25 | -------------------------------------------------------------------------------- /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: false #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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # xbot_navi 2 | 3 | ## 简介 4 | 5 | xbot_navi是用于重德智能XBot-U科研教学平台机器人的**导航定位**ROS程序包。 6 | 7 | 该程序包具有建图、定位、导航、规划以及服务功能实现等多种功能。 8 | 9 | 10 | 11 | ## 使用方法 12 | ### 建立地图 13 | 14 | 在机器人上运行 15 | 16 | ``` 17 | roslaunch xbot_navi build_map.launch 18 | ``` 19 | 20 | 然后在作为ROS从机的PC上运行可视化程序： 21 | 22 | ``` 23 | roslaunch xbot_navi rviz_build_map.launch 24 | ``` 25 | 26 | 或者也可以选择使用google开源的cartographer来建图： 27 | 28 | ``` 29 | roslaunch xbot_navi build_map_carto.launch 30 | ``` 31 | 32 | 33 | 34 | ### SLAM导航程序 35 | 36 | 在机器人上运行 37 | 38 | ``` 39 | roslaunch xbot_navi demo.launch 40 | ``` 41 | 42 | 或者使用cartographer: 43 | 44 | ``` 45 | roslaunch xbot_navi navi_carto.launch 46 | ``` 47 | 48 | 49 | 50 | ### 服务功能实现 51 | 52 | 该服务功能将导航、人脸识别、语音对话集合成一个整体的服务。 53 | 54 | 用户需要修改配置文件param/kp.json和param/greet.json来管理关键点和人脸问候语。 55 | 56 | 运行可参考 57 | 58 | ``` 59 | roslaunch xbot_navi demo.launch 60 | ``` 61 | 62 | 63 | 64 | ## 参考链接 65 | 66 | - [ROSwiki xbot tutorials]() 67 | - [ROSwiki xbot_navi软件说明](http://wiki.ros.org/xbot_navi) 68 | - [重德智能](https://www.droid.ac.cn/) 69 | - [XBot-U机器人网站介绍](https://www.droid.ac.cn/xbot_u.html) 70 | - [中国大学慕课-机器人操作系统入门](https://www.icourse163.org/course/0802ISCAS001-1002580008) 71 | 72 | ## 联系我们 73 | 74 | **商务合作**：bd@droid.ac.cn 75 | 76 | **技术咨询**：wangpeng@droid.ac.cn或添加微信:18046501051（注明XBot-U咨询） 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /param/global_planner_params.yaml: -------------------------------------------------------------------------------- 1 | 2 | GlobalPlanner: # Also see: http://wiki.ros.org/global_planner 3 | old_navfn_behavior: false # Exactly mirror behavior of navfn, use defaults for other boolean parameters, default false 4 | use_quadratic: true # Use the quadratic approximation of the potential. Otherwise, use a simpler calculation, default true 5 | use_dijkstra: true # Use dijkstra's algorithm. Otherwise, A*, default true 6 | use_grid_path: false # Create a path that follows the grid boundaries. Otherwise, use a gradient descent method, default false 7 | 8 | visualize_potential: true 9 | allow_unknown: true # Allow planner to plan through unknown space, default true 10 | #Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work 11 | planner_window_x: 0.0 # default 0.0 12 | planner_window_y: 0.0 # default 0.0 13 | default_tolerance: 0.0 # If goal in obstacle, plan to the closest point in radius default_tolerance, default 0.0 14 | 15 | publish_scale: 100 # Scale by which the published potential gets multiplied, default 100 16 | planner_costmap_publish_frequency: 0.0 # default 0.0 17 | 18 | lethal_cost: 253 # default 253 19 | neutral_cost: 50 #50 # default 50 20 | cost_factor: 3.0 #3.0 # Factor to multiply each cost from costmap by, default 3.0 21 | publish_potential: true # Publish Potential Costmap (this is not like the navfn pointcloud2 potential), default true 22 | -------------------------------------------------------------------------------- /include/auto_dock.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | > File Name: auto_dock.h 3 | > Author:Rocwang 4 | > Mail: yowlings@gmail.com; Github:https://github.com/yowlings 5 | > Created Time: 2018年06月04日 星期一 14时41分20秒 6 | ************************************************************************/ 7 | 8 | #ifndef _AUTO_DOCK_H 9 | #define _AUTO_DOCK_H 10 | #include "ros/ros.h" 11 | #include 12 | #include 13 | #include 14 | #include "sensor_msgs/LaserScan.h" 15 | #include "xbot_navi/AutodockState.h" 16 | #include "std_msgs/Bool.h" 17 | #include "geometry_msgs/TwistStamped.h" 18 | 19 | #include "boost/thread.hpp" 20 | using namespace std; 21 | namespace xbot { 22 | 23 | class AutoDock { 24 | public: 25 | AutoDock(); 26 | ~AutoDock(); 27 | 28 | 29 | void init(); 30 | void laser_scanCB(const sensor_msgs::LaserScan); 31 | void launch_autodockCB(const std_msgs::Bool); 32 | void pub_autodock_state(); 33 | void cal_docker_pose(); 34 | void search_docker(); 35 | void heading_docker(); 36 | bool is_vertical(); 37 | void go_direct(); 38 | void go_middlepose(); 39 | 40 | 41 | 42 | private: 43 | float theta[3]; 44 | float distance[3]; 45 | int p[3]; 46 | float theta_diff; 47 | float vertical_theta_thresh; 48 | bool found_docker; 49 | bool get_middlepose; 50 | bool reached_docker; 51 | ros::Subscriber laser_scan_sub; 52 | ros::Subscriber launch_autodock_sub; 53 | ros::Publisher xbot_cmd_vel_pub; 54 | ros::Publisher autodock_state_pub; 55 | xbot_navi::AutodockState autodock_state_msg; 56 | geometry_msgs::Twist cmd_vel_msg; 57 | 58 | 59 | boost::thread* state_pub_thread; 60 | boost::thread* search_dock_thread; 61 | boost::thread* heading_docker_thread; 62 | 63 | }; 64 | 65 | } 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /param/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | max_obstacle_height: 1.0 # 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 | obstacle_layer: 10 | enabled: true 11 | max_obstacle_height: 1.0 12 | origin_z: 0.0 13 | z_resolution: 0.2 14 | z_voxels: 2 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 | origin_z: 0.0 22 | z_resolution: 0.2 23 | z_voxels: 2 24 | publish_voxel_map: false 25 | observation_sources: scan bump 26 | scan: 27 | data_type: LaserScan 28 | topic: scan 29 | marking: true 30 | clearing: true 31 | min_obstacle_height: 0.0 32 | max_obstacle_height: 0.5 33 | bump: 34 | data_type: PointCloud2 35 | topic: mobile_base/sensors/bumper_pointcloud 36 | marking: true 37 | clearing: false 38 | min_obstacle_height: 0.0 39 | max_obstacle_height: 0.15 40 | # for debugging only, let's you see the entire voxel grid 41 | 42 | #cost_scaling_factor and inflation_radius were now moved to the inflation_layer ns 43 | inflation_layer: 44 | enabled: true 45 | cost_scaling_factor: 2.58 #5.0 # exponential rate at which the obstacle cost drops off (default: 10) 46 | inflation_radius: 0.5 #0.5 # max. distance from an obstacle at which costs are incurred for planning paths. 47 | 48 | static_layer: 49 | enabled: true 50 | 51 | 52 | -------------------------------------------------------------------------------- /src/greet_one.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "xbot_talker/play.h" 3 | #include "xbot_talker/chat.h" 4 | #include "xbot_face/FaceResult.h" 5 | #include 6 | 7 | 8 | using namespace std; 9 | int detect_times = 0; 10 | bool meet = false; 11 | 12 | ros::ServiceClient playclient; 13 | ros::ServiceClient chatclient; 14 | 15 | string kehu = "lyh"; 16 | string wenhou = "你好，刘银河"; 17 | 18 | void faceCB(xbot_face::FaceResult msg){ 19 | if(meet) 20 | { 21 | return; 22 | } 23 | 24 | if(msg.face_exist == false) 25 | { 26 | detect_times = 0; 27 | return; 28 | } 29 | 30 | if((msg.name == kehu)&&(msg.confidence>0.6)) 31 | { 32 | detect_times+=1; 33 | 34 | } 35 | else 36 | { 37 | detect_times = 0; 38 | } 39 | 40 | if (detect_times>=5) 41 | { 42 | meet = true; 43 | xbot_talker::play srv; 44 | srv.request.mode = 1; 45 | srv.request.tts_text = wenhou; 46 | 47 | if (playclient.call(srv)) 48 | { 49 | //ROS_INFO("Result: %s", srv.response.result); 50 | } 51 | else 52 | { 53 | ROS_ERROR("Failed to call service play"); 54 | return; 55 | } 56 | xbot_talker::chat srv1; 57 | srv1.request.start_chat = true; 58 | 59 | if(chatclient.call(srv1)) 60 | { 61 | ROS_INFO("Result: %b", srv1.response.chat_success); 62 | exit(0); 63 | } 64 | else 65 | { 66 | ROS_ERROR("Failed to call service play"); 67 | return; 68 | } 69 | 70 | } 71 | 72 | } 73 | 74 | int main(int argc, char **argv) 75 | { 76 | ros::init(argc, argv, "play_client"); 77 | ros::NodeHandle n; 78 | 79 | 80 | ros::Subscriber sub = n.subscribe("/xbot/face_result",1000, faceCB); 81 | playclient = n.serviceClient("/play"); 82 | chatclient = n.serviceClient("/chat"); 83 | ros::spin(); 84 | 85 | 86 | 87 | return 0; 88 | } 89 | 90 | -------------------------------------------------------------------------------- /launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 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 | -------------------------------------------------------------------------------- /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: 2.0 12 | planner_patience: 5.0 13 | 14 | oscillation_timeout: 10.0 #10 15 | oscillation_distance: 0.2 #0.2 16 | 17 | # local planner - default is trajectory rollout 18 | base_local_planner: "dwa_local_planner/DWAPlannerROS" #"base_local_planner/TrajectoryPlannerROS" # 19 | 20 | base_global_planner: "navfn/NavfnROS" #alternatives: global_planner/GlobalPlanner, carrot_planner/CarrotPlanner 21 | 22 | 23 | 24 | #We plan to integrate recovery behaviors for turtlebot but currently those belong to gopher and still have to be adapted. 25 | ## recovery behaviors; we avoid spinning, but we need a fall-back replanning 26 | #recovery_behavior_enabled: true 27 | 28 | #recovery_behaviors: 29 | #- name: 'super_conservative_reset1' 30 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 31 | #- name: 'conservative_reset1' 32 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 33 | #- name: 'aggressive_reset1' 34 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 35 | #- name: 'clearing_rotation1' 36 | #type: 'rotate_recovery/RotateRecovery' 37 | #- name: 'super_conservative_reset2' 38 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 39 | #- name: 'conservative_reset2' 40 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 41 | #- name: 'aggressive_reset2' 42 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 43 | #- name: 'clearing_rotation2' 44 | #type: 'rotate_recovery/RotateRecovery' 45 | 46 | #super_conservative_reset1: 47 | #reset_distance: 3.0 48 | #conservative_reset1: 49 | #reset_distance: 1.5 50 | #aggressive_reset1: 51 | #reset_distance: 0.0 52 | #super_conservative_reset2: 53 | #reset_distance: 3.0 54 | #conservative_reset2: 55 | #reset_distance: 1.5 56 | #aggressive_reset2: 57 | #reset_distance: 0.0 58 | -------------------------------------------------------------------------------- /launch/include/move_base.launch.xml: -------------------------------------------------------------------------------- 1 | 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 | -------------------------------------------------------------------------------- /param/dwa_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | DWAPlannerROS: 2 | 3 | # Robot Configuration Parameters - Kobuki 4 | max_vel_x: 0.7 # 0.55 5 | min_vel_x: 0 6 | 7 | max_vel_y: 0.0 # diff drive robot 8 | min_vel_y: 0.0 # diff drive robot 9 | 10 | max_trans_vel: 0.5 # choose slightly less than the base's capability 11 | min_trans_vel: 0.0 # this is the min trans velocity when there is negligible rotational velocity 12 | trans_stopped_vel: 0.1 13 | 14 | # Warning! 15 | # do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities 16 | # are non-negligible and small in place rotational velocities will be created. 17 | 18 | max_rot_vel: 0.8 # choose slightly less than the base's capability 19 | min_rot_vel: -0.8 # this is the min angular velocity when there is negligible translational velocity 20 | rot_stopped_vel: 0.2 21 | 22 | acc_lim_x: 0.4 # maximum is theoretically 2.0, but we 23 | acc_lim_theta: 2.0 #不能超过3! 24 | acc_lim_y: 0.0 # diff drive robot 25 | 26 | # Goal Tolerance Parameters 27 | yaw_goal_tolerance: 0.5 # 0.05 28 | xy_goal_tolerance: 0.2 # 0.10 29 | # latch_xy_goal_tolerance: false 30 | 31 | # Forward Simulation Parameters 32 | sim_time: 2.0 # 1.7 33 | vx_samples: 10 # 3 34 | vy_samples: 0 # diff drive robot, there is only one sample 35 | vtheta_samples: 20 # 20 36 | 37 | # Trajectory Scoring Parameters 38 | path_distance_bias: 32 # 32.0 - weighting for how much it should stick to the global path plan 39 | goal_distance_bias: 24.0 # 24.0 - wighting for how much it should attempt to reach its goal 40 | occdist_scale: 0.01 # 0.01 - weighting for how much the controller should avoid obstacles 41 | forward_point_distance: 0.325 # 0.325 - how far along to place an additional scoring point 42 | stop_time_buffer: 0.2 # 0.2 - amount of time a robot must stop in before colliding for a valid traj. 43 | scaling_speed: 0.25 # 0.25 - absolute velocity at which to start scaling the robot's footprint 44 | max_scaling_factor: 0.2 # 0.2 - how much to scale the robot's footprint when at speed. 45 | 46 | # Oscillation Prevention Parameters 47 | oscillation_reset_dist: 0.05 # 0.05 - how far to travel before resetting oscillation flags 48 | 49 | # Debugging 50 | publish_traj_pc : true 51 | publish_cost_grid_pc: true 52 | global_frame_id: odom 53 | 54 | 55 | # Differential-drive robot configuration - necessary? 56 | # holonomic_robot: false 57 | -------------------------------------------------------------------------------- /src/environment.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace std; 7 | 8 | ros::Publisher marker_pub; 9 | visualization_msgs::Marker marker; 10 | void goalCB(const geometry_msgs::PoseStamped msg) 11 | { 12 | marker.pose = msg.pose; 13 | marker.pose.position.z = -0.2; 14 | cout<("visualization_marker", 1); 37 | 38 | ros::Subscriber goal_sub = n.subscribe("/goal",100, &goalCB); 39 | // Set the frame ID and timestamp. See the TF tutorials for information on these. 40 | marker.header.frame_id = "/map"; 41 | marker.header.stamp = ros::Time::now(); 42 | 43 | // Set the namespace and id for this marker. This serves to create a unique ID 44 | // Any marker sent with the same namespace and id will overwrite the old one 45 | marker.ns = "basic_shapes"; 46 | marker.id = 0; 47 | 48 | // Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER 49 | marker.type = 10; 50 | marker.mesh_resource = "package://xbot_s/model/sdzn_full/saidi.DAE"; 51 | marker.mesh_use_embedded_materials = true; 52 | 53 | // Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) 54 | marker.action = visualization_msgs::Marker::ADD; 55 | 56 | // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header 57 | 58 | // Set the scale of the marker -- 1x1x1 here means 1m on a side 59 | marker.scale.x = 4.5/3.2; 60 | marker.scale.y = 4.5/3.2; 61 | marker.scale.z = 4.5/3.2; 62 | 63 | // Set the color -- be sure to set alpha to something non-zero! 64 | // marker.color.r = 0.0f; 65 | // marker.color.g = 1.0f; 66 | marker.color.b = .5f; 67 | marker.color.a = 0.5; 68 | 69 | marker.lifetime = ros::Duration(); 70 | 71 | // Publish the marker 72 | ros::spin(); 73 | 74 | // while (ros::ok()) 75 | // { 76 | 77 | // r.sleep(); 78 | // } 79 | } 80 | -------------------------------------------------------------------------------- /src/pub_3d_model.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | int main( int argc, char** argv ) 5 | { 6 | ros::init(argc, argv, "basic_shapes"); 7 | ros::NodeHandle n; 8 | ros::Rate r(1); 9 | ros::Publisher marker_pub = n.advertise("visualization_marker", 1); 10 | 11 | // Set our initial shape type to be a cube 12 | uint32_t shape = visualization_msgs::Marker::MESH_RESOURCE; 13 | 14 | while (ros::ok()) 15 | { 16 | visualization_msgs::Marker marker; 17 | // Set the frame ID and timestamp. See the TF tutorials for information on these. 18 | marker.header.frame_id = "/map"; 19 | marker.header.stamp = ros::Time::now(); 20 | 21 | // Set the namespace and id for this marker. This serves to create a unique ID 22 | // Any marker sent with the same namespace and id will overwrite the old one 23 | marker.ns = "basic_shapes"; 24 | marker.id = 0; 25 | 26 | // Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER 27 | marker.type = shape; 28 | marker.mesh_resource = "package://xbot_s/model/sdzn_full/saidi.DAE"; 29 | marker.mesh_use_embedded_materials = true; 30 | 31 | // Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) 32 | marker.action = visualization_msgs::Marker::ADD; 33 | 34 | // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header 35 | marker.pose.position.x = -2.16865; 36 | marker.pose.position.y = 10.2442; 37 | marker.pose.position.z = 0; 38 | marker.pose.orientation.x = 0.0; 39 | marker.pose.orientation.y = 0.0; 40 | marker.pose.orientation.z = -0.503664; 41 | marker.pose.orientation.w = 0.8639; 42 | 43 | // Set the scale of the marker -- 1x1x1 here means 1m on a side 44 | marker.scale.x = 4.5/3.2; 45 | marker.scale.y = 4.5/3.2; 46 | marker.scale.z = 4.5/3.2; 47 | 48 | // Set the color -- be sure to set alpha to something non-zero! 49 | // marker.color.r = 1.0f; 50 | marker.color.r = 1.0f; 51 | marker.color.g = 1.0f; 52 | marker.color.b = 1.0f; 53 | 54 | marker.color.a = 1.0f; 55 | 56 | marker.lifetime = ros::Duration(); 57 | 58 | // Publish the marker 59 | while (marker_pub.getNumSubscribers() < 1) 60 | { 61 | if (!ros::ok()) 62 | { 63 | return 0; 64 | } 65 | ROS_WARN_ONCE("Please create a subscriber to the marker"); 66 | sleep(1); 67 | } 68 | marker_pub.publish(marker); 69 | 70 | 71 | 72 | r.sleep(); 73 | } 74 | } 75 | -------------------------------------------------------------------------------- /script/input_keypoint.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #coding=utf-8 3 | 4 | import rospy, sys, termios, tty 5 | import yaml,json 6 | 7 | from geometry_msgs.msg import Pose, PoseStamped 8 | from visualization_msgs.msg import Marker, MarkerArray 9 | from move_base_msgs.msg import MoveBaseActionResult 10 | 11 | 12 | 13 | 14 | 15 | class input_kp(): 16 | """docstring for input_kp""" 17 | def __init__(self): 18 | self.total_kp = input('请输入所有关键点的个数:\n') 19 | # print type(self.total_kp) 20 | # print self.total_kp 21 | self.num_kp = 1 22 | self.kp = [] 23 | self.marker=Marker() 24 | self.marker.color.r=1.0 25 | self.marker.color.g=0.0 26 | self.marker.color.b=0.0 27 | self.marker.color.a=1.0 28 | self.marker.ns='input_kp' 29 | self.marker.scale.x=1 30 | self.marker.scale.y=0.1 31 | self.marker.scale.z=0.1 32 | self.marker.header.frame_id='map' 33 | self.marker.type=Marker.ARROW 34 | self.marker.action=Marker.ADD 35 | self.arraymarker = MarkerArray() 36 | self.markers_pub = rospy.Publisher('/kp',MarkerArray,queue_size=1) 37 | self.goal_sub = rospy.Subscriber('/goal',PoseStamped, self.mark_kpCB) 38 | 39 | 40 | 41 | tip = '请在rviz当中使用鼠标点击第 ' + str(self.num_kp) +' 个目标点的位置。' 42 | print tip 43 | rospy.spin() 44 | 45 | def mark_kpCB(self, pos): 46 | if self.num_kp <= self.total_kp: 47 | kptmp = {"recog": False, "play_words": "大家好,这是安徽省机器人比赛服务机器人项讲解机器人子项的比赛现场,我是您的机器人讲解员小德.我们本场比赛总共设置了两个讲解点,目前我们所在的位置比赛的是第一个讲解点.", "name": "", "pose": [[pos.pose.position.x,pos.pose.position.y,pos.pose.position.z],[pos.pose.orientation.x,pos.pose.orientation.y,pos.pose.orientation.z,pos.pose.orientation.w]], "play": True, "chat": False} 48 | 49 | self.kp.append(kptmp) 50 | print self.kp 51 | 52 | 53 | print 'added '+str(self.num_kp)+' keypoints' 54 | self.marker.header.stamp =rospy.Time.now() 55 | self.marker.pose = pos.pose 56 | self.marker.id = self.num_kp 57 | self.arraymarker.markers.append(self.marker) 58 | self.markers_pub.publish(self.arraymarker) 59 | tip = '请在rviz当中使用鼠标点击第' + str(self.num_kp+1) +' 个目标点的位置。' 60 | print tip 61 | 62 | if self.num_kp == self.total_kp: 63 | with open('kp.json', 'w') as f: 64 | json.dump(self.kp,f,ensure_ascii=False) 65 | 66 | print '您已完成所有关键点的录入，关键点文件已成功存入运行目录下的keypoint.yaml文件，请使用ctrl+c退出程序即可。' 67 | 68 | self.num_kp+=1 69 | 70 | def goal_resultCB(self, result): 71 | pass 72 | 73 | 74 | 75 | 76 | 77 | 78 | if __name__ == '__main__': 79 | rospy.init_node('input_kp_serve') 80 | try: 81 | rospy.loginfo('office lady initialized...') 82 | input_kp() 83 | except rospy.ROSInterruptException: 84 | rospy.loginfo('office lady initialize failed, please retry...') -------------------------------------------------------------------------------- /debian/rules: -------------------------------------------------------------------------------- 1 | #!/usr/bin/make -f 2 | # -*- makefile -*- 3 | # Sample debian/rules that uses debhelper. 4 | # This file was originally written by Joey Hess and Craig Small. 5 | # As a special exception, when this file is copied by dh-make into a 6 | # dh-make output file, you may use that output file without restriction. 7 | # This special exception was added by Craig Small in version 0.37 of dh-make. 8 | 9 | # Uncomment this to turn on verbose mode. 10 | export DH_VERBOSE=1 11 | # TODO: remove the LDFLAGS override. It's here to avoid esoteric problems 12 | # of this sort: 13 | # https://code.ros.org/trac/ros/ticket/2977 14 | # https://code.ros.org/trac/ros/ticket/3842 15 | export LDFLAGS= 16 | export PKG_CONFIG_PATH=/opt/ros/kinetic/lib/pkgconfig 17 | # Explicitly enable -DNDEBUG, see: 18 | # https://github.com/ros-infrastructure/bloom/issues/327 19 | export DEB_CXXFLAGS_MAINT_APPEND=-DNDEBUG 20 | 21 | %: 22 | dh $@ -v --buildsystem=cmake 23 | 24 | override_dh_auto_configure: 25 | # In case we're installing to a non-standard location, look for a setup.sh 26 | # in the install tree that was dropped by catkin, and source it. It will 27 | # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. 28 | if [ -f "/opt/ros/kinetic/setup.sh" ]; then . "/opt/ros/kinetic/setup.sh"; fi && \ 29 | dh_auto_configure -- \ 30 | -DCATKIN_BUILD_BINARY_PACKAGE="1" \ 31 | -DCMAKE_INSTALL_PREFIX="/opt/ros/kinetic" \ 32 | -DCMAKE_PREFIX_PATH="/opt/ros/kinetic" 33 | 34 | override_dh_auto_build: 35 | # In case we're installing to a non-standard location, look for a setup.sh 36 | # in the install tree that was dropped by catkin, and source it. It will 37 | # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. 38 | if [ -f "/opt/ros/kinetic/setup.sh" ]; then . "/opt/ros/kinetic/setup.sh"; fi && \ 39 | dh_auto_build 40 | 41 | override_dh_auto_test: 42 | # In case we're installing to a non-standard location, look for a setup.sh 43 | # in the install tree that was dropped by catkin, and source it. It will 44 | # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. 45 | echo -- Running tests. Even if one of them fails the build is not canceled. 46 | if [ -f "/opt/ros/kinetic/setup.sh" ]; then . "/opt/ros/kinetic/setup.sh"; fi && \ 47 | dh_auto_test || true 48 | 49 | override_dh_shlibdeps: 50 | # In case we're installing to a non-standard location, look for a setup.sh 51 | # in the install tree that was dropped by catkin, and source it. It will 52 | # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. 53 | if [ -f "/opt/ros/kinetic/setup.sh" ]; then . "/opt/ros/kinetic/setup.sh"; fi && \ 54 | dh_shlibdeps -l$(CURDIR)/debian/ros-kinetic-xbot-navi//opt/ros/kinetic/lib/ 55 | 56 | override_dh_auto_install: 57 | # In case we're installing to a non-standard location, look for a setup.sh 58 | # in the install tree that was dropped by catkin, and source it. It will 59 | # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. 60 | if [ -f "/opt/ros/kinetic/setup.sh" ]; then . "/opt/ros/kinetic/setup.sh"; fi && \ 61 | dh_auto_install 62 | -------------------------------------------------------------------------------- /launch/include/amcl.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | xbot_navi 4 | 0.0.0 5 | The xbot_navi package 6 | 7 | 8 | 9 | 10 | Roc Wang 11 | Roc Wang 12 | wangxiaoyun 13 | 14 | 15 | 16 | 17 | BSD 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | catkin 53 | roscpp 54 | rospy 55 | std_msgs 56 | roscpp 57 | rospy 58 | std_msgs 59 | roscpp 60 | rospy 61 | std_msgs 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /script/demo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #coding=utf-8 3 | 4 | import rospy, yaml, os, json,time 5 | 6 | from xbot_face.msg import FaceResult 7 | from xbot_talker.srv import chat, play 8 | from std_msgs.msg import String, UInt32, UInt8, Bool 9 | from geometry_msgs.msg import Pose, PoseStamped 10 | from actionlib_msgs.msg import GoalStatusArray 11 | from move_base_msgs.msg import MoveBaseActionResult 12 | from std_srvs.srv import Empty 13 | 14 | class demo(): 15 | """docstring for welcome""" 16 | def __init__(self): 17 | # 声明节点订阅与发布的消息 18 | 19 | # 发布目标点信息 20 | self.move_base_goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=1) 21 | # 订阅人脸识别结果 22 | self.face_result_sub = rospy.Subscriber('/xbot/face_result', FaceResult, self.faceCB) 23 | # 订阅是否到达目标点结果 24 | self.move_base_result_sub = rospy.Subscriber('/move_base/result', MoveBaseActionResult, self.move_base_resultCB) 25 | # 请求清除costmap服务 26 | self.clear_costmaps_srv = rospy.ServiceProxy('/move_base/clear_costmaps',Empty) 27 | # 订阅是否收到visit信息 28 | self.visit_sub = rospy.Subscriber('/demo/visit', Bool, self.visitCB) 29 | 30 | # 请求chat服务 31 | self.chat_srv = rospy.ServiceProxy('/xbot/chat',chat) 32 | # 请求播放服务 33 | self.play_srv = rospy.ServiceProxy('/xbot/play',play) 34 | 35 | 36 | # 记录机器人当前的目标点 37 | self.current_goal = 0 38 | 39 | # self.VIP_name = 'wp' 40 | 41 | self.greet_last_time = time.time() 42 | 43 | self.getVIP = False 44 | self.known_face_times = 0 45 | self.unknown_face_times = 0 46 | 47 | # 读取一存储的讲解点字典文件,默认位于xbot_s/param/position_dic.yaml文件 48 | self.kp_path = rospy.get_param('/demo/kp_path','/home/xbot/catkin_ws/src/xbot_navi/script/demo/standard/kp.json') 49 | self.greet_path = rospy.get_param('/demo/greet_path','/home/xbot/catkin_ws/src/xbot_navi/script/demo/standard/greet.json') 50 | # yaml_path = yaml_path + '/scripts/position_dic.yaml' 51 | with open(self.kp_path, 'r') as json_file: 52 | self.kp_list = json.load(json_file) 53 | json_file.close() 54 | 55 | with open(self.greet_path,'r') as f: 56 | self.greet_dict = json.load(f) 57 | f.close() 58 | rospy.spin() 59 | 60 | def greeting(self,name): 61 | if name == 'unknown': 62 | self.play_srv(False,1,'','您好，我还不认识您，我正在等待一位重要客人，请自行参观，如有需要请注册。') 63 | else: 64 | resp1 = self.play_srv(False,1,'',self.greet_dict[name]['greet_words']) 65 | if resp1 and self.greet_dict[name]['isVIP']: 66 | self.getVIP = True 67 | self.chat_srv(True) 68 | # self.pub_kp() 69 | 70 | 71 | 72 | def faceCB(self,msg): 73 | if not self.getVIP: 74 | if not msg.face_exist: 75 | self.known_face_times = 0 76 | self.unknown_face_times = 0 77 | else: 78 | if msg.confidence >0.55: 79 | self.known_face_times+=1 80 | self.unknown_face_times = 0 81 | else: 82 | self.unknown_face_times +=1 83 | self.known_face_times = 0 84 | 85 | 86 | if time.time()-self.greet_last_time>5: 87 | if self.known_face_times >=5: 88 | self.greeting(msg.name) 89 | self.known_face_times = 0 90 | 91 | 92 | if self.unknown_face_times >=10: 93 | self.greeting('unknown') 94 | self.unknown_face_times = 0 95 | 96 | self.greet_last_time = time.time() 97 | else: 98 | pass 99 | 100 | def pub_kp(self): 101 | if self.current_goal < len(self.kp_list): 102 | pos = self.kp_list[self.current_goal]['pose'] 103 | goal = PoseStamped() 104 | goal.header.frame_id = 'map' 105 | goal.pose.position.x = pos[0][0] 106 | goal.pose.position.y = pos[0][1] 107 | goal.pose.position.z = pos[0][2] 108 | goal.pose.orientation.x = pos[1][0] 109 | goal.pose.orientation.y = pos[1][1] 110 | goal.pose.orientation.z = pos[1][2] 111 | goal.pose.orientation.w = pos[1][3] 112 | print goal 113 | self.move_base_goal_pub.publish(goal) 114 | 115 | def visitCB(self, msg): 116 | if msg.data: 117 | end_talk = self.play_srv(False, 1, '', '好的，您请跟我来！') 118 | if end_talk: 119 | self.pub_kp() 120 | 121 | 122 | # 导航程序对前往目标点的执行结果 123 | def move_base_resultCB(self, result): 124 | if result.status.status == 3: 125 | # 成功到达目标点 126 | kp = self.kp_list[self.current_goal] 127 | if kp['play']: 128 | resp = self.play_srv(False, 1, '', kp['play_words']) 129 | if resp: 130 | self.current_goal+=1 131 | self.clear_costmaps_srv() 132 | self.pub_kp() 133 | else: 134 | self.current_goal += 1 135 | self.clear_costmaps_srv() 136 | self.pub_kp() 137 | elif result.status.status == 4: 138 | # 到达目标点失败,slam发布abort信号给talker,talker会请求前方人员让一下,然后重新规划路径尝试去往目标点 139 | # TODO:此时启用胸前深度摄像头确认前方障碍物情况再决定是否请求人员让一下 140 | self.pub_kp() 141 | 142 | 143 | 144 | 145 | if __name__ == '__main__': 146 | rospy.init_node('demo_node') 147 | try: 148 | rospy.loginfo('demo node initialized...') 149 | demo() 150 | except rospy.ROSInterruptException: 151 | rospy.loginfo('demo node initialize failed, please retry...') 152 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(xbot_navi) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | std_msgs 14 | sensor_msgs 15 | message_generation 16 | ) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | # find_package(Boost REQUIRED COMPONENTS system) 20 | 21 | 22 | ## Uncomment this if the package has a setup.py. This macro ensures 23 | ## modules and global scripts declared therein get installed 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 25 | # catkin_python_setup() 26 | 27 | ################################################ 28 | ## Declare ROS messages, services and actions ## 29 | ################################################ 30 | 31 | ## To declare and build messages, services or actions from within this 32 | ## package, follow these steps: 33 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 34 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 35 | ## * In the file package.xml: 36 | ## * add a build_depend tag for "message_generation" 37 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 38 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 39 | ## but can be declared for certainty nonetheless: 40 | ## * add a run_depend tag for "message_runtime" 41 | ## * In this file (CMakeLists.txt): 42 | ## * add "message_generation" and every package in MSG_DEP_SET to 43 | ## find_package(catkin REQUIRED COMPONENTS ...) 44 | ## * add "message_runtime" and every package in MSG_DEP_SET to 45 | ## catkin_package(CATKIN_DEPENDS ...) 46 | ## * uncomment the add_*_files sections below as needed 47 | ## and list every .msg/.srv/.action file to be processed 48 | ## * uncomment the generate_messages entry below 49 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 50 | 51 | ## Generate messages in the 'msg' folder 52 | 53 | 54 | ## Generate services in the 'srv' folder 55 | # add_service_files( 56 | # FILES 57 | # Service1.srv 58 | # Service2.srv 59 | # ) 60 | 61 | ## Generate actions in the 'action' folder 62 | # add_action_files( 63 | # FILES 64 | # Action1.action 65 | # Action2.action 66 | # ) 67 | 68 | ## Generate added messages and services with any dependencies listed here 69 | generate_messages( 70 | DEPENDENCIES 71 | std_msgs 72 | ) 73 | 74 | ################################################ 75 | ## Declare ROS dynamic reconfigure parameters ## 76 | ################################################ 77 | 78 | ## To declare and build dynamic reconfigure parameters within this 79 | ## package, follow these steps: 80 | ## * In the file package.xml: 81 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 82 | ## * In this file (CMakeLists.txt): 83 | ## * add "dynamic_reconfigure" to 84 | ## find_package(catkin REQUIRED COMPONENTS ...) 85 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 86 | ## and list every .cfg file to be processed 87 | 88 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 89 | # generate_dynamic_reconfigure_options( 90 | # cfg/DynReconf1.cfg 91 | # cfg/DynReconf2.cfg 92 | # ) 93 | 94 | ################################### 95 | ## catkin specific configuration ## 96 | ################################### 97 | ## The catkin_package macro generates cmake config files for your package 98 | ## Declare things to be passed to dependent projects 99 | ## INCLUDE_DIRS: uncomment this if your package contains header files 100 | ## LIBRARIES: libraries you create in this project that dependent projects also need 101 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 102 | ## DEPENDS: system dependencies of this project that dependent projects also need 103 | catkin_package( 104 | # INCLUDE_DIRS include 105 | # LIBRARIES xbot_s 106 | CATKIN_DEPENDS roscpp rospy std_msgs 107 | # DEPENDS system_lib 108 | ) 109 | 110 | ########### 111 | ## Build ## 112 | ########### 113 | 114 | ## Specify additional locations of header files 115 | ## Your package locations should be listed before other locations 116 | include_directories( 117 | # include 118 | ${catkin_INCLUDE_DIRS} 119 | ) 120 | 121 | ## Declare a C++ library 122 | # add_library(${PROJECT_NAME} 123 | # src/${PROJECT_NAME}/xbot_s.cpp 124 | # ) 125 | 126 | ## Add cmake target dependencies of the library 127 | ## as an example, code may need to be generated before libraries 128 | ## either from message generation or dynamic reconfigure 129 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 130 | 131 | ## Declare a C++ executable 132 | ## With catkin_make all packages are built within a single CMake context 133 | ## The recommended prefix ensures that target names across packages don't collide 134 | # add_executable(${PROJECT_NAME}_node src/xbot_s_node.cpp) 135 | 136 | ## Rename C++ executable without prefix 137 | ## The above recommended prefix causes long target names, the following renames the 138 | ## target back to the shorter version for ease of user use 139 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 140 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 141 | 142 | ## Add cmake target dependencies of the executable 143 | ## same as for the library above 144 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 145 | 146 | ## Specify libraries to link a library or executable target against 147 | # target_link_libraries(${PROJECT_NAME}_node 148 | # ${catkin_LIBRARIES} 149 | # ) 150 | 151 | ############# 152 | ## Install ## 153 | ############# 154 | 155 | # all install targets should use catkin DESTINATION variables 156 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 157 | 158 | ## Mark executable scripts (Python etc.) for installation 159 | ## in contrast to setup.py, you can choose the destination 160 | # install(PROGRAMS 161 | # scripts/my_python_script 162 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 163 | # ) 164 | 165 | ## Mark executables and/or libraries for installation 166 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 167 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 168 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 169 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 170 | # ) 171 | 172 | ## Mark cpp header files for installation 173 | # install(DIRECTORY include/${PROJECT_NAME}/ 174 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 175 | # FILES_MATCHING PATTERN "*.h" 176 | # PATTERN ".svn" EXCLUDE 177 | # ) 178 | 179 | ## Mark other files for installation (e.g. launch and bag files, etc.) 180 | # install(FILES 181 | # # myfile1 182 | # # myfile2 183 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 184 | # ) 185 | 186 | ############# 187 | ## Testing ## 188 | ############# 189 | 190 | ## Add gtest based cpp test target and link libraries 191 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_xbot_s.cpp) 192 | # if(TARGET ${PROJECT_NAME}-test) 193 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 194 | # endif() 195 | 196 | ## Add folders to be run by python nosetests 197 | # catkin_add_nosetests(test) 198 | 199 | -------------------------------------------------------------------------------- /rviz/input_keypoints.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | - /TF1/Tree1 9 | - /Map2 10 | - /Pose1 11 | - /MarkerArray1 12 | Splitter Ratio: 0.5 13 | Tree Height: 1203 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /Publish Point1 20 | - /2D Nav Goal1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.588679016 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 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 | - Alpha: 1 50 | Class: rviz/RobotModel 51 | Collision Enabled: false 52 | Enabled: true 53 | Links: 54 | All Links Enabled: true 55 | Expand Joint Details: false 56 | Expand Link Details: false 57 | Expand Tree: false 58 | Link Tree Style: Links in Alphabetic Order 59 | Name: RobotModel 60 | Robot Description: robot_description 61 | TF Prefix: "" 62 | Update Interval: 0 63 | Value: true 64 | Visual Enabled: true 65 | - Class: rviz/TF 66 | Enabled: false 67 | Frame Timeout: 15 68 | Frames: 69 | All Enabled: false 70 | Marker Scale: 1 71 | Name: TF 72 | Show Arrows: true 73 | Show Axes: true 74 | Show Names: true 75 | Tree: 76 | {} 77 | Update Interval: 0 78 | Value: false 79 | - Alpha: 0.699999988 80 | Class: rviz/Map 81 | Color Scheme: map 82 | Draw Behind: false 83 | Enabled: true 84 | Name: Map 85 | Topic: /map 86 | Unreliable: false 87 | Use Timestamp: false 88 | Value: true 89 | - Class: rviz/Marker 90 | Enabled: true 91 | Marker Topic: /visualization_marker 92 | Name: Marker 93 | Namespaces: 94 | {} 95 | Queue Size: 100 96 | Value: true 97 | - Alpha: 0.699999988 98 | Class: rviz/Map 99 | Color Scheme: costmap 100 | Draw Behind: false 101 | Enabled: true 102 | Name: Map 103 | Topic: /move_base/global_costmap/costmap 104 | Unreliable: false 105 | Use Timestamp: false 106 | Value: true 107 | - Alpha: 1 108 | Buffer Length: 1 109 | Class: rviz/Path 110 | Color: 25; 255; 0 111 | Enabled: true 112 | Head Diameter: 0.300000012 113 | Head Length: 0.200000003 114 | Length: 0.300000012 115 | Line Style: Lines 116 | Line Width: 0.0299999993 117 | Name: Path 118 | Offset: 119 | X: 0 120 | Y: 0 121 | Z: 0 122 | Pose Color: 255; 85; 255 123 | Pose Style: None 124 | Radius: 0.0299999993 125 | Shaft Diameter: 0.100000001 126 | Shaft Length: 0.100000001 127 | Topic: /move_base/DWAPlannerROS/global_plan 128 | Unreliable: false 129 | Value: true 130 | - Alpha: 1 131 | Autocompute Intensity Bounds: true 132 | Autocompute Value Bounds: 133 | Max Value: 10 134 | Min Value: -10 135 | Value: true 136 | Axis: Z 137 | Channel Name: intensity 138 | Class: rviz/LaserScan 139 | Color: 255; 255; 255 140 | Color Transformer: Intensity 141 | Decay Time: 0 142 | Enabled: true 143 | Invert Rainbow: false 144 | Max Color: 255; 255; 255 145 | Max Intensity: 47 146 | Min Color: 0; 0; 0 147 | Min Intensity: 47 148 | Name: LaserScan 149 | Position Transformer: XYZ 150 | Queue Size: 10 151 | Selectable: true 152 | Size (Pixels): 3 153 | Size (m): 0.100000001 154 | Style: Flat Squares 155 | Topic: /scan 156 | Unreliable: false 157 | Use Fixed Frame: true 158 | Use rainbow: true 159 | Value: true 160 | - Alpha: 1 161 | Axes Length: 1 162 | Axes Radius: 0.100000001 163 | Class: rviz/Pose 164 | Color: 255; 25; 0 165 | Enabled: true 166 | Head Length: 0.300000012 167 | Head Radius: 0.100000001 168 | Name: Pose 169 | Shaft Length: 1 170 | Shaft Radius: 0.0500000007 171 | Shape: Arrow 172 | Topic: /move_base/current_goal 173 | Unreliable: false 174 | Value: true 175 | - Class: rviz/MarkerArray 176 | Enabled: true 177 | Marker Topic: /kp 178 | Name: MarkerArray 179 | Namespaces: 180 | {} 181 | Queue Size: 100 182 | Value: true 183 | Enabled: true 184 | Global Options: 185 | Background Color: 48; 48; 48 186 | Default Light: true 187 | Fixed Frame: map 188 | Frame Rate: 30 189 | Name: root 190 | Tools: 191 | - Class: rviz/MoveCamera 192 | - Class: rviz/Interact 193 | Hide Inactive Objects: true 194 | - Class: rviz/Select 195 | - Class: rviz/SetInitialPose 196 | Topic: /initialpose 197 | - Class: rviz/Measure 198 | - Class: rviz/PublishPoint 199 | Single click: true 200 | Topic: /clicked_point 201 | - Class: rviz/SetGoal 202 | Topic: /goal 203 | Value: true 204 | Views: 205 | Current: 206 | Angle: -1.66999829 207 | Class: rviz/TopDownOrtho 208 | Enable Stereo Rendering: 209 | Stereo Eye Separation: 0.0599999987 210 | Stereo Focal Distance: 1 211 | Swap Stereo Eyes: false 212 | Value: false 213 | Invert Z Axis: false 214 | Name: Current View 215 | Near Clip Distance: 0.00999999978 216 | Scale: 124.951599 217 | Target Frame: map 218 | Value: TopDownOrtho (rviz) 219 | X: 0.770656526 220 | Y: -0.385142893 221 | Saved: ~ 222 | Window Geometry: 223 | Displays: 224 | collapsed: false 225 | Height: 1416 226 | Hide Left Dock: false 227 | Hide Right Dock: false 228 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000542fc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000002a400000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000542000000d700ffffff000000010000010f00000542fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000542000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000000000000000fb0000000800540069006d006501000000000000045000000000000000000000073a0000054200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 229 | Selection: 230 | collapsed: false 231 | Tool Properties: 232 | collapsed: false 233 | Views: 234 | collapsed: false 235 | Width: 2495 236 | X: 65 237 | Y: 24 238 | -------------------------------------------------------------------------------- /src/auto_dock.cpp: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | > File Name: src/auto_dock.cpp 3 | > Author: 4 | > Mail: 5 | > Created Time: 2018年05月31日 星期四 17时08分21秒 6 | ************************************************************************/ 7 | 8 | #include 9 | #include "../include/auto_dock.h" 10 | #define MAX(x,y) (x>y)?y:x 11 | 12 | 13 | 14 | namespace xbot{ 15 | 16 | AutoDock::AutoDock():theta_diff(100.0),vertical_theta_thresh(0.0),found_docker(false),get_middlepose(false), 17 | reached_docker(false){ 18 | 19 | } 20 | 21 | AutoDock::~AutoDock(){ 22 | if(state_pub_thread){ 23 | state_pub_thread->join(); 24 | delete state_pub_thread; 25 | } 26 | if(search_dock_thread){ 27 | search_dock_thread->join(); 28 | delete search_dock_thread; 29 | } 30 | 31 | } 32 | void AutoDock::init(){ 33 | ros::NodeHandle nh("~"); 34 | laser_scan_sub = nh.subscribe("/scan",10,&AutoDock::laser_scanCB,this); 35 | p[0]=-1; 36 | p[1]=-1; 37 | p[2]=-1; 38 | launch_autodock_sub = nh.subscribe("/auto_dock/launch",10,&AutoDock::launch_autodockCB,this); 39 | autodock_state_pub = nh.advertise ("state",100); 40 | xbot_cmd_vel_pub = nh.advertise ("/cmd_vel_mux/input/auto_dock",100); 41 | autodock_state_msg.header.stamp = ros::Time::now(); 42 | autodock_state_msg.state = xbot_navi::AutodockState::NOT_LAUNCHED; 43 | 44 | 45 | 46 | state_pub_thread = new boost::thread(boost::bind(&AutoDock::pub_autodock_state,this)); 47 | search_dock_thread = new boost::thread(boost::bind(&AutoDock::search_docker,this)); 48 | heading_docker_thread = new boost::thread(boost::bind(&AutoDock::heading_docker,this)); 49 | 50 | 51 | 52 | 53 | } 54 | void AutoDock::pub_autodock_state() 55 | { 56 | ros::Rate autodock_state_rate(10); 57 | while(ros::ok()) 58 | { 59 | autodock_state_pub.publish(autodock_state_msg); 60 | autodock_state_rate.sleep(); 61 | 62 | 63 | } 64 | } 65 | void AutoDock::cal_docker_pose(){ 66 | 67 | } 68 | 69 | bool AutoDock::is_vertical(){ 70 | // 机器人正对中心点,并且左右两边距离相等,形成等腰三角形,即垂直正对中心点 71 | return (abs(theta[1])<2*vertical_theta_thresh)&&(abs(distance[0]-distance[2])<0.05); 72 | } 73 | 74 | void AutoDock::search_docker(){ 75 | ros::Rate search_rate(20); 76 | int search_times=0; 77 | ROS_INFO("pose1:%d;%d",p[0],p[2]); 78 | 79 | while((p[0]==-1||p[2]==-1)&&search_times<60*20/*&&!is_vertical()||abs(theta_diff)>0.004*/) 80 | { 81 | // ROS_INFO("pose1:%d;%d",p[0],p[2]); 82 | 83 | cmd_vel_msg.angular.z = 0.157; 84 | xbot_cmd_vel_pub.publish(cmd_vel_msg); 85 | search_rate.sleep(); 86 | search_times++; 87 | // ROS_INFO("p[1]=%d;vertical:%d;theta_diff=%f",p[1],is_vertical(),theta_diff); 88 | 89 | } 90 | if(search_times>=60*20){ 91 | ROS_INFO("searched for 30 seconds for turn 3 loops,but not found docker,please recheck!"); 92 | found_docker=false; 93 | autodock_state_msg.state=xbot_navi::AutodockState::NOT_FOUND_DOCKER; 94 | 95 | } 96 | else{ 97 | found_docker=true; 98 | 99 | // if(!is_vertical()){ 100 | // get_middlepose=false; 101 | // search_times=0; 102 | // while(abs(theta_diff)>0.004&&search_times<60){ 103 | // cmd_vel_msg.angular.z = 0.314; 104 | // xbot_cmd_vel_pub.publish(cmd_vel_msg); 105 | // search_rate.sleep(); 106 | // search_times++; 107 | // } 108 | // if(search_times>=60){ 109 | // ROS_INFO("searched for 60 seconds for turn 3 loops,but not found horizonal pose,please recheck!"); 110 | // found_docker=false; 111 | // autodock_state_msg.state=xbot_navi::AutodockState::NOT_FOUND_DOCKER; 112 | 113 | // } 114 | // } 115 | // else{ 116 | // get_middlepose=true; 117 | // } 118 | 119 | } 120 | 121 | } 122 | void AutoDock::go_direct(){ 123 | ros::Rate forward_rate(20); 124 | while(distance[1]>0.2){ 125 | cmd_vel_msg.linear.x=0.1; 126 | xbot_cmd_vel_pub.publish(cmd_vel_msg); 127 | forward_rate.sleep(); 128 | } 129 | 130 | 131 | 132 | reached_docker=true; 133 | } 134 | void AutoDock::go_middlepose(){ 135 | ros::Rate forward_rate(20); 136 | while(p[0]>5&&abs(p[2]-524)<=5){ 137 | cmd_vel_msg.linear.x=0.1; 138 | xbot_cmd_vel_pub.publish(cmd_vel_msg); 139 | forward_rate.sleep(); 140 | } 141 | int times=0; 142 | while(!is_vertical()&×<60){ 143 | cmd_vel_msg.angular.z=0.314; 144 | xbot_cmd_vel_pub.publish(cmd_vel_msg); 145 | forward_rate.sleep(); 146 | } 147 | if(times<60) 148 | { 149 | get_middlepose=true; 150 | } 151 | 152 | } 153 | void AutoDock::heading_docker(){ 154 | ros::Rate cmd_pub_rate(20); 155 | while(ros::ok()&&!reached_docker){ 156 | if(found_docker){ 157 | autodock_state_msg.state=xbot_navi::AutodockState::HEADING; 158 | cmd_vel_msg.angular.z=MAX(0.2*(theta[0]+theta[2])/2-(distance[0]-distance[2]),1.0); 159 | cmd_vel_msg.linear.x=MAX(0.1*(distance[0]+distance[2])/2,0.5); 160 | xbot_cmd_vel_pub.publish(cmd_vel_msg); 161 | cmd_pub_rate.sleep(); 162 | 163 | // if(get_middlepose=true){ 164 | // go_direct(); 165 | 166 | // } 167 | // else{ 168 | // go_middlepose(); 169 | // } 170 | 171 | } 172 | } 173 | 174 | } 175 | 176 | void AutoDock::launch_autodockCB(const std_msgs::Bool msg){ 177 | if(msg.data == true){ 178 | if(autodock_state_msg.state%2==0){ 179 | ROS_INFO("launch autodock success, start searching docker around robot."); 180 | autodock_state_msg.state = xbot_navi::AutodockState::SEARCHING; 181 | 182 | 183 | } 184 | else{ 185 | ROS_INFO("xbot autodock has already launched, please check /auto_dock/state for autodock state and try later!"); 186 | } 187 | } 188 | 189 | } 190 | void AutoDock::laser_scanCB(const sensor_msgs::LaserScan msg){ 191 | vertical_theta_thresh = msg.angle_increment; 192 | int length = msg.ranges.size(); 193 | vector a = msg.intensities; 194 | int i=0,j=0; 195 | p[0]=-1; 196 | for(;i200) 199 | { 200 | for(;j<50;j++) 201 | { 202 | if(a[i+j]<200) break; 203 | } 204 | if(j>=20) 205 | { 206 | p[0]=i; 207 | break; 208 | } 209 | 210 | } 211 | 212 | } 213 | i=i+j; 214 | j=0; 215 | p[2]=-1; 216 | for(;i200) 219 | { 220 | for(;j<50;j++) 221 | { 222 | if(a[i+j]<200) break; 223 | } 224 | if(j>=20) 225 | { 226 | p[2]=i+j; 227 | break; 228 | } 229 | 230 | } 231 | 232 | } 233 | 234 | 235 | 236 | p[1]=-1; 237 | if(p[0]!=-1&&p[2]!=-1) 238 | { 239 | p[1] = (p[0]+p[2])/2; 240 | } 241 | 242 | if(p[1]!=-1){ 243 | for(int pii=0;pii<3;pii++){ 244 | theta[pii]=msg.angle_min+p[pii]*msg.angle_increment; 245 | distance[pii]=msg.ranges[p[pii]]; 246 | } 247 | theta_diff = abs(tan(theta[1]))-(abs(tan(theta[0]))+abs(tan(theta[2])))/2; 248 | 249 | } 250 | else{ 251 | theta_diff = 100; 252 | } 253 | 254 | 255 | 256 | // ROS_INFO("p[1]=%d;vertical:%d;theta_diff=%f",p[1],is_vertical(),theta_diff); 257 | if(abs(theta_diff)<0.004) 258 | { 259 | ROS_INFO("found right direction!"); 260 | } 261 | 262 | 263 | ROS_INFO("pose0:%d; pose2: %d",p[0],p[2]); 264 | // ROS_INFO("docker pose:%d/%d",p[0],length); 265 | 266 | } 267 | 268 | 269 | 270 | } 271 | 272 | /***************************************************************************** 273 | ** Main 274 | *****************************************************************************/ 275 | 276 | 277 | 278 | int main(int argc, char **argv) 279 | { 280 | ros::init(argc, argv, "auto_dock"); 281 | xbot::AutoDock autodock; 282 | autodock.init(); 283 | ros::spin(); 284 | return 0; 285 | } 286 | 287 | -------------------------------------------------------------------------------- /rviz/build_map.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | - /TF1/Tree1 9 | - /Map2 10 | - /LaserScan1 11 | - /Pose1 12 | Splitter Ratio: 0.5 13 | Tree Height: 843 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /Publish Point1 20 | - /2D Nav Goal1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.588679016 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 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: false 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: false 49 | - Alpha: 1 50 | Class: rviz/RobotModel 51 | Collision Enabled: false 52 | Enabled: true 53 | Links: 54 | All Links Enabled: true 55 | Expand Joint Details: false 56 | Expand Link Details: false 57 | Expand Tree: false 58 | Link Tree Style: Links in Alphabetic Order 59 | base_footprint: 60 | Alpha: 1 61 | Show Axes: false 62 | Show Trail: false 63 | base_link: 64 | Alpha: 1 65 | Show Axes: false 66 | Show Trail: false 67 | Value: true 68 | camera_link: 69 | Alpha: 1 70 | Show Axes: false 71 | Show Trail: false 72 | Value: true 73 | imu_link: 74 | Alpha: 1 75 | Show Axes: false 76 | Show Trail: false 77 | laser: 78 | Alpha: 1 79 | Show Axes: false 80 | Show Trail: false 81 | Value: true 82 | laser_mount_link: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | left_wheel: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | Value: true 91 | pitch_platform: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | Value: true 96 | right_wheel: 97 | Alpha: 1 98 | Show Axes: false 99 | Show Trail: false 100 | Value: true 101 | yaw_platform: 102 | Alpha: 1 103 | Show Axes: false 104 | Show Trail: false 105 | Value: true 106 | Name: RobotModel 107 | Robot Description: robot_description 108 | TF Prefix: "" 109 | Update Interval: 0 110 | Value: true 111 | Visual Enabled: true 112 | - Class: rviz/TF 113 | Enabled: false 114 | Frame Timeout: 15 115 | Frames: 116 | All Enabled: false 117 | Marker Scale: 1 118 | Name: TF 119 | Show Arrows: true 120 | Show Axes: true 121 | Show Names: true 122 | Tree: 123 | {} 124 | Update Interval: 0 125 | Value: false 126 | - Alpha: 0.699999988 127 | Class: rviz/Map 128 | Color Scheme: map 129 | Draw Behind: false 130 | Enabled: true 131 | Name: Map 132 | Topic: /map 133 | Unreliable: false 134 | Use Timestamp: false 135 | Value: true 136 | - Class: rviz/Marker 137 | Enabled: true 138 | Marker Topic: /visualization_marker 139 | Name: Marker 140 | Namespaces: 141 | {} 142 | Queue Size: 100 143 | Value: true 144 | - Alpha: 0.699999988 145 | Class: rviz/Map 146 | Color Scheme: costmap 147 | Draw Behind: false 148 | Enabled: true 149 | Name: Map 150 | Topic: /move_base/global_costmap/costmap 151 | Unreliable: false 152 | Use Timestamp: false 153 | Value: true 154 | - Alpha: 1 155 | Buffer Length: 1 156 | Class: rviz/Path 157 | Color: 25; 255; 0 158 | Enabled: true 159 | Head Diameter: 0.300000012 160 | Head Length: 0.200000003 161 | Length: 0.300000012 162 | Line Style: Lines 163 | Line Width: 0.0299999993 164 | Name: Path 165 | Offset: 166 | X: 0 167 | Y: 0 168 | Z: 0 169 | Pose Color: 255; 85; 255 170 | Pose Style: None 171 | Radius: 0.0299999993 172 | Shaft Diameter: 0.100000001 173 | Shaft Length: 0.100000001 174 | Topic: /move_base/DWAPlannerROS/global_plan 175 | Unreliable: false 176 | Value: true 177 | - Alpha: 1 178 | Autocompute Intensity Bounds: true 179 | Autocompute Value Bounds: 180 | Max Value: 10 181 | Min Value: -10 182 | Value: true 183 | Axis: Z 184 | Channel Name: intensity 185 | Class: rviz/LaserScan 186 | Color: 255; 255; 255 187 | Color Transformer: Intensity 188 | Decay Time: 0 189 | Enabled: true 190 | Invert Rainbow: false 191 | Max Color: 255; 255; 255 192 | Max Intensity: 47 193 | Min Color: 0; 0; 0 194 | Min Intensity: 47 195 | Name: LaserScan 196 | Position Transformer: XYZ 197 | Queue Size: 10 198 | Selectable: true 199 | Size (Pixels): 3 200 | Size (m): 0.0500000007 201 | Style: Flat Squares 202 | Topic: /scan 203 | Unreliable: false 204 | Use Fixed Frame: true 205 | Use rainbow: true 206 | Value: true 207 | - Alpha: 1 208 | Axes Length: 1 209 | Axes Radius: 0.100000001 210 | Class: rviz/Pose 211 | Color: 255; 25; 0 212 | Enabled: true 213 | Head Length: 0.300000012 214 | Head Radius: 0.100000001 215 | Name: Pose 216 | Shaft Length: 1 217 | Shaft Radius: 0.0500000007 218 | Shape: Arrow 219 | Topic: /move_base/current_goal 220 | Unreliable: false 221 | Value: true 222 | Enabled: true 223 | Global Options: 224 | Background Color: 48; 48; 48 225 | Default Light: true 226 | Fixed Frame: map 227 | Frame Rate: 30 228 | Name: root 229 | Tools: 230 | - Class: rviz/MoveCamera 231 | - Class: rviz/Interact 232 | Hide Inactive Objects: true 233 | - Class: rviz/Select 234 | - Class: rviz/SetInitialPose 235 | Topic: /initialpose 236 | - Class: rviz/Measure 237 | - Class: rviz/PublishPoint 238 | Single click: true 239 | Topic: /clicked_point 240 | - Class: rviz/SetGoal 241 | Topic: /move_base_simple/goal 242 | Value: true 243 | Views: 244 | Current: 245 | Angle: 0.0700000823 246 | Class: rviz/TopDownOrtho 247 | Enable Stereo Rendering: 248 | Stereo Eye Separation: 0.0599999987 249 | Stereo Focal Distance: 1 250 | Swap Stereo Eyes: false 251 | Value: false 252 | Invert Z Axis: false 253 | Name: Current View 254 | Near Clip Distance: 0.00999999978 255 | Scale: 46.9354706 256 | Target Frame: map 257 | Value: TopDownOrtho (rviz) 258 | X: 1.49365008 259 | Y: 0.921626866 260 | Saved: ~ 261 | Window Geometry: 262 | Displays: 263 | collapsed: false 264 | Height: 1056 265 | Hide Left Dock: false 266 | Hide Right Dock: false 267 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000003dafc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000002a400000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000003da000000dd00ffffff000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000003da000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004ba000003da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 268 | Selection: 269 | collapsed: false 270 | Tool Properties: 271 | collapsed: false 272 | Views: 273 | collapsed: false 274 | Width: 1855 275 | X: 65 276 | Y: 24 277 | -------------------------------------------------------------------------------- /rviz/xbot_input_goal.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1 8 | - /TF1/Frames1 9 | - /TF1/Tree1 10 | - /Local Planning1 11 | - /Global Planning1 12 | - /Global Planning1/Planner1 13 | - /Pose (move_base)1 14 | - /LaserScan1 15 | - /Marker1 16 | - /MarkerArray1 17 | - /Path1 18 | Splitter Ratio: 0.5 19 | Tree Height: 935 20 | - Class: rviz/Selection 21 | Name: Selection 22 | - Class: rviz/Tool Properties 23 | Expanded: 24 | - /2D Pose Estimate1 25 | - /Publish Point1 26 | - /2D Nav Goal1 27 | Name: Tool Properties 28 | Splitter Ratio: 0.588679016 29 | - Class: rviz/Views 30 | Expanded: 31 | - /Current View1 32 | Name: Views 33 | Splitter Ratio: 0.5 34 | - Class: rviz/Time 35 | Experimental: false 36 | Name: Time 37 | SyncMode: 0 38 | SyncSource: LaserScan 39 | Visualization Manager: 40 | Class: "" 41 | Displays: 42 | - Alpha: 0.5 43 | Cell Size: 1 44 | Class: rviz/Grid 45 | Color: 160; 160; 164 46 | Enabled: true 47 | Line Style: 48 | Line Width: 0.0299999993 49 | Value: Lines 50 | Name: Grid 51 | Normal Cell Count: 0 52 | Offset: 53 | X: 0 54 | Y: 0 55 | Z: 0 56 | Plane: XY 57 | Plane Cell Count: 10 58 | Reference Frame: 59 | Value: true 60 | - Alpha: 1 61 | Class: rviz/RobotModel 62 | Collision Enabled: false 63 | Enabled: true 64 | Links: 65 | All Links Enabled: true 66 | Expand Joint Details: false 67 | Expand Link Details: false 68 | Expand Tree: false 69 | Link Tree Style: Links in Alphabetic Order 70 | base_footprint: 71 | Alpha: 1 72 | Show Axes: false 73 | Show Trail: false 74 | base_link: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | camera_link: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | Value: true 84 | camera_link_optical: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | imu_link: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | laser: 93 | Alpha: 1 94 | Show Axes: false 95 | Show Trail: false 96 | Value: true 97 | laser_mount_link: 98 | Alpha: 1 99 | Show Axes: false 100 | Show Trail: false 101 | left_wheel: 102 | Alpha: 1 103 | Show Axes: false 104 | Show Trail: false 105 | Value: true 106 | pitch_platform: 107 | Alpha: 1 108 | Show Axes: false 109 | Show Trail: false 110 | Value: true 111 | right_wheel: 112 | Alpha: 1 113 | Show Axes: false 114 | Show Trail: false 115 | Value: true 116 | yaw_platform: 117 | Alpha: 1 118 | Show Axes: false 119 | Show Trail: false 120 | Value: true 121 | Name: RobotModel 122 | Robot Description: robot_description 123 | TF Prefix: "" 124 | Update Interval: 0 125 | Value: true 126 | Visual Enabled: true 127 | - Class: rviz/TF 128 | Enabled: false 129 | Frame Timeout: 15 130 | Frames: 131 | All Enabled: false 132 | Marker Scale: 1 133 | Name: TF 134 | Show Arrows: true 135 | Show Axes: true 136 | Show Names: true 137 | Tree: 138 | {} 139 | Update Interval: 0 140 | Value: false 141 | - Alpha: 1 142 | Autocompute Intensity Bounds: true 143 | Autocompute Value Bounds: 144 | Max Value: 10 145 | Min Value: -10 146 | Value: true 147 | Axis: Z 148 | Channel Name: intensity 149 | Class: rviz/PointCloud2 150 | Color: 255; 255; 255 151 | Color Transformer: Intensity 152 | Decay Time: 0 153 | Enabled: false 154 | Invert Rainbow: false 155 | Max Color: 255; 255; 255 156 | Max Intensity: 4096 157 | Min Color: 0; 0; 0 158 | Min Intensity: 0 159 | Name: PointCloud (bumpers) 160 | Position Transformer: XYZ 161 | Queue Size: 10 162 | Selectable: true 163 | Size (Pixels): 3 164 | Size (m): 0.0399999991 165 | Style: Flat Squares 166 | Topic: /mobile_base/sensors/bumper_pointcloud 167 | Unreliable: false 168 | Use Fixed Frame: true 169 | Use rainbow: true 170 | Value: false 171 | - Alpha: 0.699999988 172 | Class: rviz/Map 173 | Color Scheme: map 174 | Draw Behind: false 175 | Enabled: true 176 | Name: Map 177 | Topic: /map 178 | Unreliable: false 179 | Use Timestamp: false 180 | Value: true 181 | - Class: rviz/Group 182 | Displays: 183 | - Alpha: 0.699999988 184 | Class: rviz/Map 185 | Color Scheme: costmap 186 | Draw Behind: false 187 | Enabled: false 188 | Name: Costmap 189 | Topic: /move_base/local_costmap/costmap 190 | Unreliable: false 191 | Use Timestamp: false 192 | Value: false 193 | - Alpha: 1 194 | Buffer Length: 1 195 | Class: rviz/Path 196 | Color: 0; 12; 255 197 | Enabled: true 198 | Head Diameter: 0.300000012 199 | Head Length: 0.200000003 200 | Length: 0.300000012 201 | Line Style: Lines 202 | Line Width: 0.0299999993 203 | Name: Planner 204 | Offset: 205 | X: 0 206 | Y: 0 207 | Z: 0 208 | Pose Color: 255; 85; 255 209 | Pose Style: None 210 | Radius: 0.0299999993 211 | Shaft Diameter: 0.100000001 212 | Shaft Length: 0.100000001 213 | Topic: /move_base/DWAPlannerROS/local_plan 214 | Unreliable: false 215 | Value: true 216 | - Alpha: 0.5 217 | Autocompute Intensity Bounds: true 218 | Autocompute Value Bounds: 219 | Max Value: 10 220 | Min Value: -10 221 | Value: true 222 | Axis: Z 223 | Channel Name: total_cost 224 | Class: rviz/PointCloud2 225 | Color: 255; 255; 255 226 | Color Transformer: Intensity 227 | Decay Time: 0 228 | Enabled: false 229 | Invert Rainbow: false 230 | Max Color: 255; 255; 255 231 | Max Intensity: 339.399994 232 | Min Color: 0; 0; 0 233 | Min Intensity: 0 234 | Name: Cost Cloud 235 | Position Transformer: XYZ 236 | Queue Size: 10 237 | Selectable: true 238 | Size (Pixels): 3 239 | Size (m): 0.0399999991 240 | Style: Flat Squares 241 | Topic: /move_base/DWAPlannerROS/cost_cloud 242 | Unreliable: false 243 | Use Fixed Frame: true 244 | Use rainbow: true 245 | Value: false 246 | - Alpha: 1 247 | Autocompute Intensity Bounds: true 248 | Autocompute Value Bounds: 249 | Max Value: 10 250 | Min Value: -10 251 | Value: true 252 | Axis: Z 253 | Channel Name: total_cost 254 | Class: rviz/PointCloud2 255 | Color: 255; 255; 255 256 | Color Transformer: Intensity 257 | Decay Time: 0 258 | Enabled: false 259 | Invert Rainbow: false 260 | Max Color: 255; 255; 255 261 | Max Intensity: 128.274994 262 | Min Color: 0; 0; 0 263 | Min Intensity: 43.2000008 264 | Name: Trajectory Cloud 265 | Position Transformer: XYZ 266 | Queue Size: 10 267 | Selectable: true 268 | Size (Pixels): 3 269 | Size (m): 0.0399999991 270 | Style: Flat Squares 271 | Topic: /move_base/DWAPlannerROS/trajectory_cloud 272 | Unreliable: false 273 | Use Fixed Frame: true 274 | Use rainbow: false 275 | Value: false 276 | Enabled: true 277 | Name: Local Planning 278 | - Class: rviz/Group 279 | Displays: 280 | - Alpha: 0.400000006 281 | Class: rviz/Map 282 | Color Scheme: costmap 283 | Draw Behind: true 284 | Enabled: true 285 | Name: Costmap 286 | Topic: /move_base/global_costmap/costmap 287 | Unreliable: false 288 | Use Timestamp: false 289 | Value: true 290 | - Alpha: 1 291 | Buffer Length: 1 292 | Class: rviz/Path 293 | Color: 255; 0; 0 294 | Enabled: true 295 | Head Diameter: 0.300000012 296 | Head Length: 0.200000003 297 | Length: 0.300000012 298 | Line Style: Lines 299 | Line Width: 0.0299999993 300 | Name: Planner 301 | Offset: 302 | X: 0 303 | Y: 0 304 | Z: 0 305 | Pose Color: 255; 85; 255 306 | Pose Style: None 307 | Radius: 0.0299999993 308 | Shaft Diameter: 0.100000001 309 | Shaft Length: 0.100000001 310 | Topic: /move_base/DWAPlannerROS/global_plan 311 | Unreliable: false 312 | Value: true 313 | Enabled: true 314 | Name: Global Planning 315 | - Alpha: 1 316 | Axes Length: 1 317 | Axes Radius: 0.100000001 318 | Class: rviz/Pose 319 | Color: 255; 25; 0 320 | Enabled: true 321 | Head Length: 0.200000003 322 | Head Radius: 0.100000001 323 | Name: Pose (move_base) 324 | Shaft Length: 1 325 | Shaft Radius: 0.0500000007 326 | Shape: Arrow 327 | Topic: /move_base/current_goal 328 | Unreliable: false 329 | Value: true 330 | - Alpha: 1 331 | Buffer Length: 1 332 | Class: rviz/Path 333 | Color: 25; 255; 0 334 | Enabled: true 335 | Head Diameter: 0.300000012 336 | Head Length: 0.200000003 337 | Length: 0.300000012 338 | Line Style: Lines 339 | Line Width: 0.0299999993 340 | Name: Path (global) 341 | Offset: 342 | X: 0 343 | Y: 0 344 | Z: 0 345 | Pose Color: 255; 85; 255 346 | Pose Style: None 347 | Radius: 0.0299999993 348 | Shaft Diameter: 0.100000001 349 | Shaft Length: 0.100000001 350 | Topic: /move_base/NavfnROS/plan 351 | Unreliable: false 352 | Value: true 353 | - Angle Tolerance: 0.100000001 354 | Class: rviz/Odometry 355 | Covariance: 356 | Orientation: 357 | Alpha: 0.5 358 | Color: 255; 255; 127 359 | Color Style: Unique 360 | Frame: Local 361 | Offset: 1 362 | Scale: 1 363 | Value: true 364 | Position: 365 | Alpha: 0.300000012 366 | Color: 204; 51; 204 367 | Scale: 1 368 | Value: true 369 | Value: true 370 | Enabled: false 371 | Keep: 10 372 | Name: Odometry 373 | Position Tolerance: 0.100000001 374 | Shape: 375 | Alpha: 1 376 | Axes Length: 1 377 | Axes Radius: 0.100000001 378 | Color: 255; 25; 0 379 | Head Length: 0.300000012 380 | Head Radius: 0.100000001 381 | Shaft Length: 1 382 | Shaft Radius: 0.0500000007 383 | Value: Arrow 384 | Topic: /odom 385 | Unreliable: false 386 | Value: false 387 | - Class: rviz/MarkerArray 388 | Enabled: false 389 | Marker Topic: /move_base/EBandPlannerROS/eband_visualization_array 390 | Name: EBand boubles 391 | Namespaces: 392 | {} 393 | Queue Size: 100 394 | Value: false 395 | - Alpha: 1 396 | Autocompute Intensity Bounds: true 397 | Autocompute Value Bounds: 398 | Max Value: 10 399 | Min Value: -10 400 | Value: true 401 | Axis: Z 402 | Channel Name: intensity 403 | Class: rviz/LaserScan 404 | Color: 255; 255; 255 405 | Color Transformer: Intensity 406 | Decay Time: 0 407 | Enabled: true 408 | Invert Rainbow: false 409 | Max Color: 255; 255; 255 410 | Max Intensity: 999999 411 | Min Color: 0; 0; 0 412 | Min Intensity: 2.06214214e-19 413 | Name: LaserScan 414 | Position Transformer: XYZ 415 | Queue Size: 10 416 | Selectable: true 417 | Size (Pixels): 3 418 | Size (m): 0.00999999978 419 | Style: Flat Squares 420 | Topic: /scan 421 | Unreliable: false 422 | Use Fixed Frame: true 423 | Use rainbow: true 424 | Value: true 425 | - Class: rviz/Marker 426 | Enabled: true 427 | Marker Topic: /exploration_polygon_marker 428 | Name: Marker 429 | Namespaces: 430 | {} 431 | Queue Size: 100 432 | Value: true 433 | - Class: rviz/MarkerArray 434 | Enabled: true 435 | Marker Topic: /xbot_navigoals/marker_goals 436 | Name: MarkerArray 437 | Namespaces: 438 | my_namespace: true 439 | Queue Size: 100 440 | Value: true 441 | - Alpha: 1 442 | Buffer Length: 1 443 | Class: rviz/Path 444 | Color: 25; 255; 0 445 | Enabled: true 446 | Head Diameter: 0.300000012 447 | Head Length: 0.200000003 448 | Length: 0.300000012 449 | Line Style: Lines 450 | Line Width: 0.0299999993 451 | Name: Path 452 | Offset: 453 | X: 0 454 | Y: 0 455 | Z: 0 456 | Pose Color: 255; 85; 255 457 | Pose Style: None 458 | Radius: 0.0299999993 459 | Shaft Diameter: 0.100000001 460 | Shaft Length: 0.100000001 461 | Topic: /navi_plans 462 | Unreliable: false 463 | Value: true 464 | Enabled: true 465 | Global Options: 466 | Background Color: 48; 48; 48 467 | Default Light: true 468 | Fixed Frame: map 469 | Frame Rate: 30 470 | Name: root 471 | Tools: 472 | - Class: rviz/MoveCamera 473 | - Class: rviz/Interact 474 | Hide Inactive Objects: true 475 | - Class: rviz/Select 476 | - Class: rviz/SetInitialPose 477 | Topic: /initialpose 478 | - Class: rviz/Measure 479 | - Class: rviz/PublishPoint 480 | Single click: true 481 | Topic: /clicked_point 482 | - Class: rviz/SetGoal 483 | Topic: /mark_coll_position 484 | Value: true 485 | Views: 486 | Current: 487 | Angle: -3.13499951 488 | Class: rviz/TopDownOrtho 489 | Enable Stereo Rendering: 490 | Stereo Eye Separation: 0.0599999987 491 | Stereo Focal Distance: 1 492 | Swap Stereo Eyes: false 493 | Value: false 494 | Invert Z Axis: false 495 | Name: Current View 496 | Near Clip Distance: 0.00999999978 497 | Scale: 44.2460747 498 | Target Frame: map 499 | Value: TopDownOrtho (rviz) 500 | X: 0.0362191424 501 | Y: -2.35373878 502 | Saved: ~ 503 | Window Geometry: 504 | Displays: 505 | collapsed: true 506 | Height: 1148 507 | Hide Left Dock: true 508 | Hide Right Dock: true 509 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000436fc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000002a400000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000002800000436000000dd00ffffff000000010000010f00000452fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000452000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003a00000043600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 510 | Selection: 511 | collapsed: false 512 | Time: 513 | collapsed: false 514 | Tool Properties: 515 | collapsed: false 516 | Views: 517 | collapsed: true 518 | Width: 928 519 | X: 65 520 | Y: 24 521 | --------------------------------------------------------------------------------