├── map ├── map.pgm └── map.yaml ├── launch ├── simulation │ ├── includes │ │ ├── maze.png │ │ ├── maze.yaml │ │ ├── turtlebot.inc │ │ ├── maze.world │ │ ├── local_costmap_params.yaml │ │ ├── dwa_local_planner_params.yaml │ │ ├── global_costmap_params.yaml │ │ └── amcl_node.xml │ └── NeuronBot_Stage_Swarm.launch ├── client │ ├── NeuronBot_Demo_Client_AIO.launch │ ├── params │ │ ├── global_planner_params.yaml │ │ ├── local_costmap_params.yaml │ │ ├── global_costmap_params.yaml │ │ ├── dwa_local_planner_params.yaml │ │ └── base_local_planner_params.yaml │ ├── NeuronBot_Demo_Client_AllDrivers.launch │ └── NeuronBot_Demo_Client_Nav_Swarm.launch ├── host │ ├── NeuronBot_Demo_Host_Teleop.launch │ ├── NeuronBot_Demo_Host_Localization.launch │ ├── include │ │ ├── params │ │ │ ├── astra_pro_640_480.yaml │ │ │ └── template_config.inp │ │ ├── ncs_people_detection_mobilenetssd.launch.xml │ │ ├── D435_rgbd_driver.launch.xml │ │ ├── Astra_Pro_rgbd_driver.launch.xml │ │ └── Astra_rgbd_driver.launch.xml │ ├── NeuronBot_Demo_Host_Joy.launch │ ├── NeuronBot_Demo_Host_AllDrivers.launch │ ├── NeuronBot_Demo_Host_ROS2_Bridge.launch │ ├── NeuronBot_Demo_Host_Following.launch │ ├── NeuronBot_Demo_Host_Gmapping.launch │ ├── NeuronBot_Demo_Host_AIO.launch │ └── NeuronBot_Demo_Host_Detection.launch ├── include │ ├── ydlidar_driver.launch.xml │ ├── neuronbot_tf.launch.xml │ ├── neuronbot_kobuki.launch.xml │ ├── neuronbot_amcl_improved.launch.xml │ └── neuronbot_amcl.launch.xml └── params │ └── neuronbot_mux.yaml ├── document ├── ADLINK_NeuronBot_20180313.pdf ├── Useful_cmds └── autostart │ ├── NeuronBot_Demo_Host_AutoStart.sh │ └── NeuronBot_Demo_Client_AutoStart.sh ├── msg ├── MultiRobots.msg └── Robot.msg ├── costmap_plugins.xml ├── cfg ├── SwarmLayer.cfg └── Following.cfg ├── package.xml ├── CMakeLists.txt ├── script ├── neuronbot_goal_filter.py ├── neuronbot_tf_to_transform.py ├── leg_tracker_to_detected_persons.py ├── neuronbot_robot_id_filter.py ├── neuronbot_multi_goals.py ├── neuronbot_following_fused.py └── neuronbot_following_legs.py ├── include └── adlink_neuronbot │ └── swarm_layer.h ├── readme.md ├── rviz ├── neuronbot_sim_swarm.rviz ├── neuronbot_demo_client.rviz └── neuronbot_demo_host.rviz ├── src └── swarm_layer.cpp └── LICENSE /map/map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Adlink-ROS/adlink_neuronbot/HEAD/map/map.pgm -------------------------------------------------------------------------------- /launch/simulation/includes/maze.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Adlink-ROS/adlink_neuronbot/HEAD/launch/simulation/includes/maze.png -------------------------------------------------------------------------------- /document/ADLINK_NeuronBot_20180313.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Adlink-ROS/adlink_neuronbot/HEAD/document/ADLINK_NeuronBot_20180313.pdf -------------------------------------------------------------------------------- /launch/simulation/includes/maze.yaml: -------------------------------------------------------------------------------- 1 | image: maze.png 2 | resolution: 0.05 3 | origin: [0.0, 0.0, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /map/map.yaml: -------------------------------------------------------------------------------- 1 | image: map.pgm 2 | resolution: 0.050000 3 | origin: [-50.000000, -50.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /msg/MultiRobots.msg: -------------------------------------------------------------------------------- 1 | # header: the frame_id inside is for the final target map frame. 2 | # In current adlink_neuronbot ver. it is unfunctional. 3 | # robots: the container of robots 4 | 5 | std_msgs/Header header 6 | adlink_neuronbot/Robot[] robots 7 | 8 | -------------------------------------------------------------------------------- /costmap_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | the costmap layer for NeuronBot Swarm application 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /msg/Robot.msg: -------------------------------------------------------------------------------- 1 | # transform: To store translation, rotation, global_id, 2 | # body_id, timestamp info of a robot 3 | # radius: the radius of robot size (meter) 4 | # reliability: The accuracy of pose estimation (0~1) 5 | 6 | geometry_msgs/TransformStamped transform 7 | float64 radius 8 | float64 reliability 9 | 10 | -------------------------------------------------------------------------------- /launch/client/NeuronBot_Demo_Client_AIO.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /document/Useful_cmds: -------------------------------------------------------------------------------- 1 | # Create pkg 2 | $ catkin_create_pkg adlink_neuronbot rospy roscpp move_base tf nav_msgs std_msgs geometry_msgs message_generation dynamic_reconfigure costmap_2d visualization_msgs people_msgs 3 | 4 | # Calibration 5 | $ rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.0246 image:=/astra_pro/rgb/image_raw camera:=/astra_pro/rgb 6 | -------------------------------------------------------------------------------- /cfg/SwarmLayer.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE='adlink_neuronbot' 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | # Name Type Level Description Default Min Max 9 | gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not ", True) 10 | 11 | exit(gen.generate(PACKAGE, "adlink_neuronbot", "SwarmLayer")) 12 | -------------------------------------------------------------------------------- /launch/client/params/global_planner_params.yaml: -------------------------------------------------------------------------------- 1 | #For full documentation of the parameters in this file, and a list of all the 2 | #parameters available for GlobalPlanner, please see 3 | #http://www.ros.org/wiki/global_planner 4 | GlobalPlanner: 5 | allow_unknown: true 6 | default_tolerance: 0.5 # meter 7 | visualize_potential: false 8 | use_dijkstra: true 9 | use_quadratic: true 10 | use_grid_path: false 11 | old_navfn_behavior: false 12 | 13 | 14 | -------------------------------------------------------------------------------- /launch/host/NeuronBot_Demo_Host_Teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /launch/simulation/includes/turtlebot.inc: -------------------------------------------------------------------------------- 1 | define kinect ranger 2 | ( 3 | sensor 4 | ( 5 | range_max 10 6 | fov 270.0 7 | samples 1280 8 | ) 9 | # generic model properties 10 | color "black" 11 | size [ 0.06 0.15 0.03 ] 12 | ) 13 | 14 | define turtlebot position 15 | ( 16 | pose [ 0.0 0.0 0.0 0.0 ] 17 | 18 | #odom_error [0.000 0.000 999999 999999 999999 0.000] 19 | 20 | size [ 0.2552 0.2552 0.40 ] 21 | origin [ 0.0 0.0 0.0 0.0 ] 22 | gui_nose 1 23 | drive "diff" 24 | color "gray" 25 | 26 | kinect(pose [ -0.1 0.0 -0.11 0.0 ]) 27 | ) 28 | -------------------------------------------------------------------------------- /launch/host/NeuronBot_Demo_Host_Localization.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /launch/host/include/params/astra_pro_640_480.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: camera 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [608.288824, 0.000000, 322.786703, 0.000000, 613.098399, 234.650707, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [0.163435, -0.241903, -0.010445, 0.004125, 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: [629.144165, 0.000000, 325.137233, 0.000000, 0.000000, 632.298279, 230.512607, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /launch/client/NeuronBot_Demo_Client_AllDrivers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /launch/host/NeuronBot_Demo_Host_Joy.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /launch/include/ydlidar_driver.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/simulation/includes/maze.world: -------------------------------------------------------------------------------- 1 | include "turtlebot.inc" 2 | 3 | define floorplan model 4 | ( 5 | # sombre, sensible, artistic 6 | color "gray30" 7 | 8 | # most maps will need a bounding box 9 | boundary 1 10 | 11 | gui_nose 0 12 | gui_grid 0 13 | gui_outline 0 14 | gripper_return 0 15 | fiducial_return 0 16 | laser_return 1 17 | ) 18 | 19 | resolution 0.05 20 | interval_sim 100 # simulation timestep in milliseconds 21 | 22 | window 23 | ( 24 | size [ 600 700 ] 25 | center [ 0.0 0.0 ] 26 | rotate [ 0.000 0.000 ] 27 | scale 30.789 28 | ) 29 | 30 | floorplan 31 | ( 32 | name "maze" 33 | bitmap "maze.png" 34 | size [ 10.000 10.000 2.000 ] 35 | pose [ 5.000 5.000 0.000 0.000 ] 36 | ) 37 | 38 | # throw in a robot 39 | turtlebot 40 | ( 41 | pose [ 2.000 2.000 0.000 0.000 ] 42 | name "turtlebot" 43 | color "black" 44 | ) 45 | -------------------------------------------------------------------------------- /launch/include/neuronbot_tf.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /launch/params/neuronbot_mux.yaml: -------------------------------------------------------------------------------- 1 | # Individual subscriber configuration: 2 | # name: Source name 3 | # topic: The topic that provides cmd_vel messages 4 | # timeout: Time in seconds without incoming messages to consider this topic inactive 5 | # priority: Priority: an UNIQUE unsigned integer from 0 (lowest) to MAX_INT 6 | # short_desc: Short description (optional) 7 | 8 | subscribers: 9 | - name: "Safe reactive controller" 10 | topic: "input/safety_controller" 11 | timeout: 0.2 12 | priority: 10 13 | - name: "Teleoperation" 14 | topic: "input/teleop" 15 | timeout: 1.0 16 | priority: 7 17 | - name: "Following" 18 | topic: "input/follow" 19 | timeout: 1.0 20 | priority: 6 21 | - name: "Navigation" 22 | topic: "input/navi" 23 | timeout: 1.0 24 | priority: 5 25 | -------------------------------------------------------------------------------- /launch/client/params/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | #footprint: [[-0.1, -0.1], [-0.1, 0.1], [0.1, 0.1], [0.1, -0.1]] 3 | robot_radius: 0.15 4 | footprint_padding: 0.01 5 | transform_tolerance: 1.0 6 | update_frequency: 10.0 7 | publish_frequency: 10.0 8 | 9 | global_frame: /odom 10 | robot_base_frame: /base_footprint 11 | resolution: 0.05 12 | static_map: false 13 | rolling_window: true 14 | width: 0.5 15 | height: 0.5 16 | resolution: 0.1 17 | 18 | plugins: 19 | #- {name: sensor, type: "costmap_2d::ObstacleLayer"} 20 | - {name: inflation, type: "costmap_2d::InflationLayer"} 21 | 22 | #sensor: 23 | #observation_sources: base_scan 24 | #base_scan: {sensor_frame: laser_frame, data_type: LaserScan, topic: scan, marking: true, clearing: true} 25 | 26 | inflation: 27 | inflation_radius: 0.1 28 | cost_scaling_factor: 10.0 29 | -------------------------------------------------------------------------------- /document/autostart/NeuronBot_Demo_Host_AutoStart.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "===== ADLINK Neuronbot AutoStart (Host) =====" 4 | echo "Loading env parameters ..." 5 | # ROS 2.0 (ardent) 6 | RMW_IMPLEMENTATION=rmw_opensplice_cpp 7 | source /opt/ros/ardent/setup.bash 8 | 9 | # ROS 1.0 (Kinetic) 10 | source /opt/ros/kinetic/setup.bash 11 | source /home/chester/catkin_ws/devel/setup.bash 12 | 13 | # ROS 1 multi-machine 14 | export ROS_MASTER_URI=http://192.168.50.211:11311 15 | export ROS_IP=192.168.50.211 16 | 17 | export HOSTNAME 18 | #source /home/chester/.bashrc 19 | 20 | echo "Launch AIO for host robot (person following) ..." 21 | xterm -title "ROS 1 Nodes" -hold -e "source /home/chester/.bashrc && roslaunch adlink_neuronbot NeuronBot_Demo_Host_AIO.launch" & 22 | 23 | echo "Waiting 5 sec before executing ros2 nodes (for swarm) ..." 24 | sleep 5 25 | xterm -title "ROS 2 Nodes" -hold -e "ros2 run ros1_bridge parameter_bridge" 26 | #roslaunch adlink_neuronbot ADLink_NeuronBot_Demo_AIO.launch --pid ros.pid 27 | -------------------------------------------------------------------------------- /launch/simulation/includes/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.25, 0.0], [0.12, -0.12]] 3 | footprint_padding: 0.01 4 | transform_tolerance: 0.5 5 | update_frequency: 10.0 6 | publish_frequency: 10.0 7 | 8 | global_frame: odom 9 | robot_base_frame: base_link 10 | resolution: 0.05 11 | static_map: false 12 | rolling_window: true 13 | width: 1.0 14 | height: 1.0 15 | 16 | plugins: 17 | - {name: sensor, type: "costmap_2d::ObstacleLayer"} 18 | - {name: inflation, type: "costmap_2d::InflationLayer"} 19 | 20 | sensor: 21 | observation_sources: base_scan 22 | base_scan: {data_type: LaserScan, expected_update_rate: 0.4, observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08} 23 | 24 | inflation: 25 | inflation_radius: 1 26 | cost_scaling_factor: 10.0 27 | 28 | -------------------------------------------------------------------------------- /launch/host/NeuronBot_Demo_Host_AllDrivers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /document/autostart/NeuronBot_Demo_Client_AutoStart.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "===== ADLINK Neuronbot AutoStart (Client) =====" 4 | echo "Loading env parameters ..." 5 | # ROS 2.0 (ardent) 6 | RMW_IMPLEMENTATION=rmw_opensplice_cpp 7 | source /opt/ros/ardent/setup.bash 8 | 9 | # ROS 1.0 (Kinetic) 10 | source /opt/ros/kinetic/setup.bash 11 | source /home/chester/catkin_ws/devel/setup.bash 12 | 13 | # ROS 1 multi-machine 14 | export ROS_MASTER_URI=http://192.168.50.211:11311 15 | export ROS_IP=192.168.50.211 16 | 17 | export HOSTNAME 18 | #source /home/chester/.bashrc 19 | 20 | echo "Waiting 5 sec before time synchronizing ..." 21 | sleep 5 22 | echo adlinkros | sudo -S date --set="$(sshpass -p adlinkros ssh chester@192.168.50.211 date -R)" 23 | 24 | echo "Waiting 5 sec before launching AIO for client robot ..." 25 | sleep 5 26 | xterm -title "ROS 1 Nodes" -hold -e "source /home/chester/.bashrc && roslaunch adlink_neuronbot NeuronBot_Demo_Client_AIO.launch" & 27 | 28 | echo "Waiting 5 sec before executing ros2 nodes (for swarm) ..." 29 | sleep 5 30 | xterm -title "ROS 2 Nodes" -hold -e "ros2 run ros1_bridge parameter_bridge" 31 | -------------------------------------------------------------------------------- /launch/client/params/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | #footprint: [[-0.1, -0.1], [-0.1, 0.1], [0.1, 0.1], [0.1, -0.1]] 3 | robot_radius: 0.15 4 | footprint_padding: 0.01 5 | transform_tolerance: 0.5 6 | update_frequency: 20.0 7 | publish_frequency: 20.0 8 | 9 | global_frame: map 10 | robot_base_frame: base_footprint 11 | resolution: 0.05 12 | 13 | rolling_window: false 14 | track_unknown_space: true 15 | 16 | plugins: 17 | - {name: static, type: "costmap_2d::StaticLayer"} 18 | #- {name: sensor, type: "costmap_2d::ObstacleLayer"} 19 | - {name: swarm_layer, type: "neuronbot_swarm_layer::SwarmLayer"} 20 | - {name: inflation, type: "costmap_2d::InflationLayer"} 21 | 22 | static: 23 | map_topic: /map 24 | subscribe_to_updates: true 25 | 26 | #sensor: 27 | #observation_sources: base_scan 28 | #base_scan: {sensor_frame: laser_frame, data_type: LaserScan, topic: scan, marking: true, clearing: true} 29 | 30 | swarm_layer: 31 | enabled: true 32 | 33 | inflation: 34 | inflation_radius: 0.3 35 | cost_scaling_factor: 5.0 36 | -------------------------------------------------------------------------------- /launch/client/params/dwa_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | #For full documentation of the parameters in this file, and a list of all the 2 | #parameters available for DWAPlannerROS, please see 3 | #http://www.ros.org/wiki/dwa_local_planner 4 | DWAPlannerROS: 5 | max_trans_vel: 0.12 6 | min_trans_vel: 0.07 7 | 8 | max_vel_x: 0.12 9 | min_vel_x: -0.03 10 | max_vel_y: 0.0 11 | min_vel_y: 0.0 12 | max_rot_vel: 1.5 13 | min_rot_vel: 0.3 14 | 15 | acc_lim_x: 0.7 16 | acc_lim_y: 0.0 17 | acc_lim_theta: 3.2 18 | acc_limit_trans: 0.5 19 | 20 | prune_plan: false 21 | 22 | xy_goal_tolerance: 0.15 23 | yaw_goal_tolerance: 1.0 24 | latch_xy_goal_tolerance: true 25 | 26 | trans_stopped_vel: 0.01 27 | rot_stopped_vel: 0.01 28 | 29 | sim_time: 2.5 30 | sim_granularity: 0.01 31 | 32 | path_distance_bias: 40.0 33 | goal_distance_bias: 30.0 34 | occdist_scale: 0.01 35 | 36 | stop_time_buffer: 0.2 37 | oscillation_reset_dist: 0.05 38 | 39 | forward_point_distance: 0.1 40 | 41 | scaling_speed: 0.25 42 | max_scaling_factor: 0.2 43 | 44 | vx_samples: 30 45 | vy_samples: 10 46 | vtheta_samples: 30 47 | 48 | use_dwa: false 49 | 50 | 51 | -------------------------------------------------------------------------------- /launch/simulation/includes/dwa_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | #For full documentation of the parameters in this file, and a list of all the 2 | #parameters available for DWAPlannerROS, please see 3 | #http://www.ros.org/wiki/dwa_local_planner 4 | DWAPlannerROS: 5 | max_trans_vel: 0.12 6 | min_trans_vel: 0.07 7 | 8 | max_vel_x: 0.12 9 | min_vel_x: -0.03 10 | max_vel_y: 0.0 11 | min_vel_y: 0.0 12 | max_rot_vel: 1.5 13 | min_rot_vel: 0.3 14 | 15 | acc_lim_x: 0.7 16 | acc_lim_y: 0.0 17 | acc_lim_theta: 3.2 18 | acc_limit_trans: 0.5 19 | 20 | prune_plan: false 21 | 22 | xy_goal_tolerance: 0.15 23 | yaw_goal_tolerance: 1.0 24 | latch_xy_goal_tolerance: true 25 | 26 | trans_stopped_vel: 0.01 27 | rot_stopped_vel: 0.01 28 | 29 | sim_time: 2.5 30 | sim_granularity: 0.01 31 | 32 | path_distance_bias: 40.0 33 | goal_distance_bias: 30.0 34 | occdist_scale: 0.01 35 | 36 | stop_time_buffer: 0.2 37 | oscillation_reset_dist: 0.05 38 | 39 | forward_point_distance: 0.1 40 | 41 | scaling_speed: 0.25 42 | max_scaling_factor: 0.2 43 | 44 | vx_samples: 30 45 | vy_samples: 10 46 | vtheta_samples: 30 47 | 48 | use_dwa: false 49 | 50 | 51 | -------------------------------------------------------------------------------- /launch/simulation/includes/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.25, 0.0], [0.12, -0.12]] 3 | footprint_padding: 0.01 4 | transform_tolerance: 0.5 5 | update_frequency: 20.0 6 | publish_frequency: 20.0 7 | 8 | global_frame: map 9 | robot_base_frame: base_link 10 | resolution: 0.05 11 | 12 | rolling_window: false 13 | track_unknown_space: true 14 | 15 | plugins: 16 | - {name: static, type: "costmap_2d::StaticLayer"} 17 | #- {name: sensor, type: "costmap_2d::ObstacleLayer"} 18 | - {name: swarm_layer, type: "neuronbot_swarm_layer::SwarmLayer"} 19 | - {name: inflation, type: "costmap_2d::InflationLayer"} 20 | 21 | static: 22 | map_topic: /map 23 | subscribe_to_updates: false 24 | 25 | #sensor: 26 | #observation_sources: base_scan 27 | #base_scan: {data_type: LaserScan, expected_update_rate: 0.4, observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08} 28 | 29 | swarm_layer: 30 | enabled: true 31 | 32 | inflation: 33 | inflation_radius: 1 34 | cost_scaling_factor: 10.0 35 | -------------------------------------------------------------------------------- /cfg/Following.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE='adlink_neuronbot' 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | # Name Type Level Description Default Min Max 9 | gen.add("Status", bool_t, 0, "The status for tracking mode.", True) 10 | gen.add("Distance", double_t, 0, "Following distance.", 0.9, 0.4, 2.0) 11 | gen.add("Search_radius",double_t, 0, "Radius of target searching area.", 1.5, 0.4, 2.0) 12 | gen.add("Search_theta", double_t, 0, "Theta of target searching area.", 0.3, 0.1, 1.0) 13 | gen.add("Kp_linear", double_t, 0, "The Kp gain for distant error.", 0.8, 0.0, 2.0) 14 | gen.add("Ki_linear", double_t, 0, "The Ki gain for distant error.", 0.0, -2.0, 2.0) 15 | gen.add("Kp_angular", double_t, 0, "The Kp gain for angular error.", 1.5, 0.0, 3.0) 16 | gen.add("Ki_angular", double_t, 0, "The Ki gain for angular error.", 0.0, -3.0, 3.0) 17 | gen.add("DeadZone_radius", double_t, 0, "Radius of Dead Zone Area.", 0.1, 0.0, 1.0) 18 | gen.add("DeadZone_theta", double_t, 0, "Theta of Dead Zone Area.", 0.1, 0.0, 1.0) 19 | 20 | exit(gen.generate(PACKAGE, "adlink_neuronbot", "Following")) 21 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | adlink_neuronbot 4 | 0.0.0 5 | The adlink_neuronbot package 6 | adlinkros 7 | Apache 2.0 8 | 9 | 10 | catkin 11 | geometry_msgs 12 | move_base 13 | nav_msgs 14 | roscpp 15 | rospy 16 | std_msgs 17 | tf 18 | visualization_msgs 19 | costmap_2d 20 | dynamic_reconfigure 21 | message_generation 22 | geometry_msgs 23 | move_base 24 | nav_msgs 25 | roscpp 26 | rospy 27 | std_msgs 28 | tf 29 | visualization_msgs 30 | costmap_2d 31 | dynamic_reconfigure 32 | message_runtime 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(adlink_neuronbot) 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 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | costmap_2d 11 | nav_msgs 12 | std_msgs 13 | geometry_msgs 14 | message_generation 15 | dynamic_reconfigure 16 | ) 17 | 18 | ## dynamic reconfigure config 19 | generate_dynamic_reconfigure_options( 20 | cfg/SwarmLayer.cfg 21 | cfg/Following.cfg 22 | ) 23 | 24 | ## custom message 25 | add_message_files( 26 | FILES 27 | Robot.msg 28 | MultiRobots.msg 29 | ) 30 | 31 | generate_messages( 32 | DEPENDENCIES 33 | nav_msgs 34 | std_msgs 35 | geometry_msgs 36 | ) 37 | 38 | 39 | catkin_package( 40 | CATKIN_DEPENDS std_msgs geometry_msgs nav_msgs message_runtime costmap_2d dynamic_reconfigure 41 | ) 42 | 43 | ## Specify additional locations of header files 44 | include_directories( 45 | include ${catkin_INCLUDE_DIRS} 46 | ) 47 | 48 | ## add cpp library 49 | add_library(swarm_layers src/swarm_layer.cpp) 50 | 51 | ## Add cmake target dependencies of the executable/library 52 | add_dependencies(swarm_layers 53 | ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) 54 | 55 | 56 | ## Install costmap plugin 57 | install(FILES costmap_plugins.xml 58 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 59 | ) 60 | 61 | install(TARGETS swarm_layers 62 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 63 | ) 64 | -------------------------------------------------------------------------------- /launch/client/params/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | TrajectoryPlannerROS: 2 | #Set the acceleration limits of the robot 3 | acc_lim_theta: 1.0 4 | acc_lim_x: 0.05 5 | acc_lim_y: 0.05 6 | 7 | #Set the velocity limits of the robot 8 | max_vel_x: 0.085 9 | min_vel_x: 0.02 10 | max_vel_theta: 0.7 11 | min_vel_theta: -0.7 12 | min_in_place_rotational_vel: 0.6 13 | 14 | #The velocity the robot will command when trying to escape from a stuck situation 15 | escape_vel: -0.1 16 | 17 | #For this example, we'll use a holonomic robot 18 | holonomic_robot: false 19 | 20 | #Set the tolerance on achieving a goal 21 | xy_goal_tolerance: 0.1 22 | yaw_goal_tolerance: 3.14 # final heading is not important 23 | latch_xy_goal_tolerance: true 24 | 25 | #We'll configure how long and with what granularity we'll forward simulate trajectories 26 | sim_time: 4.0 27 | sim_granularity: 0.01 # meter 28 | vx_samples: 50 29 | vtheta_samples: 20 30 | 31 | #Parameters for scoring trajectories 32 | meter_scoring: true # true: unit is meter, false: cell 33 | pdist_scale: 3.0 # max is 5 34 | gdist_scale: 0.5 # max is 5 35 | heading_scoring: false 36 | heading_lookahead: 0.1 # meter 37 | heading_scoring_timestep: 0.5 # secs 38 | 39 | #We'll use the Dynamic Window Approach to control instead of Trajectory Rollout for this example 40 | dwa: false 41 | 42 | #How far the robot must travel before oscillation flags are reset 43 | oscillation_reset_dist: 0.05 44 | 45 | #Eat up the plan as the robot moves along it 46 | prune_plan: true 47 | -------------------------------------------------------------------------------- /launch/include/neuronbot_kobuki.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 | -------------------------------------------------------------------------------- /launch/host/include/ncs_people_detection_mobilenetssd.launch.xml: -------------------------------------------------------------------------------- 1 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /launch/host/NeuronBot_Demo_Host_ROS2_Bridge.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | [{'topic':swarm_poses, 'type':geometry_msgs/TransformStamped}, 11 | {'topic':swarm_goals, 'type':geometry_msgs/TransformStamped}] 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /launch/host/NeuronBot_Demo_Host_Following.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 | -------------------------------------------------------------------------------- /launch/host/NeuronBot_Demo_Host_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 | 42 | 43 | -------------------------------------------------------------------------------- /launch/host/include/D435_rgbd_driver.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 | -------------------------------------------------------------------------------- /script/neuronbot_goal_filter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # Copyright 2017 ADLINK Technology, Inc. 3 | # Developer: HaoChih, LIN (haochih.lin@adlinktech.com) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import rospy 18 | from geometry_msgs.msg import PoseStamped, TransformStamped 19 | 20 | class GoalFilter: 21 | def __init__(self, self_id, sim_mode): 22 | self.sub = rospy.Subscriber('swarm_goals', TransformStamped, self.goalCB, queue_size=10) 23 | self.pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10) 24 | self.selfId = self_id 25 | self.sim_mode = sim_mode 26 | 27 | def goalCB(self, data): 28 | if data.child_frame_id == self.selfId: 29 | goal = PoseStamped() 30 | goal.header = data.header 31 | if sim_mode: 32 | goal.header.stamp = rospy.Time.now() 33 | goal.pose.position = data.transform.translation 34 | goal.pose.orientation = data.transform.rotation 35 | self.pub.publish(goal) 36 | 37 | if __name__ == "__main__": 38 | try: 39 | # ROS Init 40 | rospy.init_node('goal_filter', anonymous=True) 41 | 42 | # Get params 43 | self_id = rospy.get_param('~self_id', 'robot_1') # robot frame id 44 | sim_mode = rospy.get_param('~sim_mode', 'false') # for simulation 45 | 46 | # Constract GoalFilter Obj 47 | gf = GoalFilter(self_id, sim_mode) 48 | rospy.loginfo("Start Goal Filter ...") 49 | rospy.spin() 50 | except KeyboardInterrupt: 51 | print("shutting down") 52 | -------------------------------------------------------------------------------- /launch/simulation/includes/amcl_node.xml: -------------------------------------------------------------------------------- 1 | 2 | 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 | -------------------------------------------------------------------------------- /launch/host/NeuronBot_Demo_Host_AIO.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 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /launch/host/include/params/template_config.inp: -------------------------------------------------------------------------------- 1 | #===================================== 2 | # Distance Range Accepted Detections 3 | #===================================== 4 | distance_range_accepted_detections = 7 # up to what distance detections are kept. All detections after that distance are rejected 5 | 6 | #===================================== 7 | # ROI 8 | #===================================== 9 | inc_width_ratio = 0.2 10 | inc_height_ratio = 0.0 11 | region_size_threshold = 10 12 | 13 | #====================== 14 | # Freespace 15 | #====================== 16 | # parameteters for occupancy Map computation 17 | 18 | # these parameter define the number of bins for the occupancy histogram 19 | freespace_scaleZ = 20; 20 | freespace_scaleX = 20; 21 | 22 | freespace_minX = -3; 23 | freespace_minZ = 0; 24 | freespace_maxX = 3; 25 | freespace_maxZ = 7; 26 | freespace_threshold = 500 27 | freespace_max_depth_to_cons = 7 28 | 29 | #==================================== 30 | # Detector 31 | #==================================== 32 | # parameters upper body detector 33 | evaluation_NMS_threshold_LM = 0.25 # lowering this parameter will reduce the number of false positives! Keep in mind this is distance! 34 | evaluation_inc_height_ratio = 0.2 # parameter for increasing the size of ROI which is then scanned by the depth template 35 | 36 | # <<< these parameters are only relevant if you want to perform multi-scaling, evaluation_nr_scales > 1 37 | evaluation_stride = 3 38 | evaluation_scale_stride = 1.03 39 | evaluation_nr_scales = 1 40 | evaluation_inc_cropped_height = 20 41 | evaluation_greedy_NMS_overlap_threshold = 0.3 42 | evaluation_greedy_NMS_threshold = 0.25 43 | # >>> 44 | 45 | # scanning corridor, which means for each ROI we scan it with the depth template starting from min_height (in meters) for up to max_height 46 | max_height = 2.1 47 | min_height = 1.0 48 | 49 | #====================== 50 | # World scale 51 | #====================== 52 | WORLD_SCALE = 1.0 53 | 54 | #=========================== 55 | # height and width of images 56 | #=========================== 57 | dImWidth = 640 58 | dImHeight = 480 59 | 60 | #==================================== 61 | # Number of Frames / offset 62 | #==================================== 63 | numberFrames = 1000000 64 | nOffset = 0 65 | 66 | #==================================== 67 | # Size of Template 68 | #==================================== 69 | template_size = 30 70 | -------------------------------------------------------------------------------- /launch/include/neuronbot_amcl_improved.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 | 56 | -------------------------------------------------------------------------------- /launch/include/neuronbot_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 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /include/adlink_neuronbot/swarm_layer.h: -------------------------------------------------------------------------------- 1 | /* 2 | # Copyright 2017 ADLINK Technology, Inc. 3 | # Developer: HaoChih, LIN (haochih.lin@adlinktech.com) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | */ 17 | 18 | #ifndef SWARM_LAYER_H_ 19 | #define SWARM_LAYER_H_ 20 | #include 21 | #include 22 | #include 23 | #include //custom msg 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | namespace neuronbot_swarm_layer 30 | { 31 | class SwarmLayer : public costmap_2d::Layer 32 | { 33 | public: 34 | SwarmLayer() { layered_costmap_ = NULL; } 35 | 36 | virtual void onInitialize(); 37 | virtual void updateBounds(double origin_x, double origin_y, double origin_yaw, double* min_x, double* min_y, double* max_x, double* max_y); 38 | virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); 39 | 40 | protected: 41 | void robotsCallback(const adlink_neuronbot::MultiRobots& robots); 42 | void updateBoundsFromRobots(double* min_x, double* min_y, double* max_x, double* max_y); 43 | double gaussian(double x, double y, double x0, double y0, double A, double varx, double vary, double skew); 44 | void configure(adlink_neuronbot::SwarmLayerConfig &config, uint32_t level); 45 | dynamic_reconfigure::Server* server_; 46 | dynamic_reconfigure::Server::CallbackType f_; 47 | bool enabled_; 48 | 49 | ros::Subscriber robots_sub_; 50 | adlink_neuronbot::MultiRobots robots_list_; 51 | std::list transformed_robots_; 52 | ros::Duration robots_keep_time_; 53 | boost::recursive_mutex lock_; 54 | tf::TransformListener tf_; 55 | bool first_time_; 56 | double last_min_x_, last_min_y_, last_max_x_, last_max_y_; 57 | }; 58 | }; 59 | 60 | 61 | #endif 62 | 63 | -------------------------------------------------------------------------------- /script/neuronbot_tf_to_transform.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # Copyright 2017 ADLINK Technology, Inc. 3 | # Developer: HaoChih, LIN (haochih.lin@adlinktech.com) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import rospy 18 | import time 19 | import tf 20 | from geometry_msgs.msg import TransformStamped 21 | 22 | def timerCB(event): 23 | try: 24 | (trans,quat) = listener.lookupTransform(map_frame ,base_frame , rospy.Time(0)) 25 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 26 | return 27 | 28 | msg_out = TransformStamped() 29 | msg_out.header.frame_id = map_frame 30 | if sim_mode: 31 | msg_out.header.stamp = rospy.Time.from_sec(time.time()) 32 | else: 33 | msg_out.header.stamp = rospy.Time.now() 34 | msg_out.child_frame_id = robot_id 35 | msg_out.transform.translation.x = trans[0] 36 | msg_out.transform.translation.y = trans[1] 37 | msg_out.transform.translation.z = trans[2] 38 | msg_out.transform.rotation.x = quat[0] 39 | msg_out.transform.rotation.y = quat[1] 40 | msg_out.transform.rotation.z = quat[2] 41 | msg_out.transform.rotation.w = quat[3] 42 | pub.publish(msg_out) 43 | 44 | if __name__ == '__main__': 45 | try: 46 | # ROS Init 47 | rospy.init_node('pose_to_transform', anonymous=True) 48 | 49 | # Get params 50 | robot_id = rospy.get_param('~self_id', 'robot_1') # hostname (should be unique in the network) 51 | sim_mode = rospy.get_param('~sim_mode', 'false') # for simulation 52 | map_frame = rospy.get_param('~map_frame', 'map') # tf link for map 53 | base_frame = rospy.get_param('~base_frame', 'base_link') # tf link for robot 54 | 55 | # Pub & Timer 56 | pub = rospy.Publisher('swarm_poses', TransformStamped, queue_size=10) 57 | timer = rospy.Timer(rospy.Duration(0.1), timerCB) # 10Hz 58 | 59 | # TF Listener 60 | listener = tf.TransformListener() 61 | 62 | # spin() simply keeps python from exiting until this node is stopped 63 | rospy.loginfo("Converting pose to transform ...") 64 | rospy.spin() 65 | except rospy.ROSInterruptException: 66 | pass 67 | -------------------------------------------------------------------------------- /script/leg_tracker_to_detected_persons.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # Copyright 2018 ADLINK Technology, Inc. 3 | # Developer: HaoChih, LIN (haochih.lin@adlinktech.com) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import rospy 18 | from spencer_tracking_msgs.msg import DetectedPersons, DetectedPerson 19 | from geometry_msgs.msg import Pose 20 | from leg_tracker.msg import PersonArray, Person 21 | 22 | class Conversion: 23 | def __init__(self, detectionId, detectionIdIncrement, posVariance, rotVariance, confidence, modality): 24 | self.sub = rospy.Subscriber('/people_tracked', PersonArray, self.CallBack, queue_size=10) 25 | self.pub = rospy.Publisher('/leg_tracker/detected_persons', DetectedPersons, queue_size=10) 26 | self.modality = modality 27 | self.confidence = confidence 28 | self.posVariance = posVariance 29 | self.rotVariance = rotVariance 30 | self.detectionId = detectionId 31 | self.detectionIdIncrement = detectionIdIncrement 32 | 33 | def CallBack(self, data): 34 | detectedPersons = DetectedPersons() 35 | detectedPersons.header = data.header 36 | 37 | for Person in data.people: 38 | detectedPerson = DetectedPerson() 39 | detectedPerson.modality = self.modality 40 | detectedPerson.confidence = self.confidence 41 | detectedPerson.detection_id = self.detectionId 42 | detectedPerson.pose.pose = Person.pose 43 | 44 | for i in xrange(0, 6): 45 | detectedPerson.pose.covariance[i*6 + i] = self.posVariance if i < 3 else self.rotVariance 46 | 47 | detectedPersons.detections.append(detectedPerson) 48 | self.detectionId += self.detectionIdIncrement 49 | 50 | self.pub.publish(detectedPersons) 51 | # end of func 52 | 53 | 54 | if __name__ == "__main__": 55 | try: 56 | # ROS Init 57 | rospy.init_node('leg_tracker_to_detected_persons', anonymous=True) 58 | 59 | # Get params 60 | detectionId = rospy.get_param("~detection_id_offset", 0) 61 | detectionIdIncrement = rospy.get_param("~detection_id_increment", 1) 62 | posVariance = rospy.get_param("~pos_variance", 0.1) 63 | rotVariance = rospy.get_param("~rot_variance", 99999) 64 | confidence = rospy.get_param("~confidence", 1.0) 65 | modality = rospy.get_param("~modality", "leg") 66 | 67 | # Constract Conversion Obj 68 | conv = Conversion(detectionId, detectionIdIncrement, posVariance, rotVariance, confidence, modality) 69 | rospy.loginfo("Start Conversion Node ...") 70 | rospy.spin() 71 | except KeyboardInterrupt: 72 | print("shutting down") 73 | -------------------------------------------------------------------------------- /launch/simulation/NeuronBot_Stage_Swarm.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 | [{'topic':swarm_poses, 'type':geometry_msgs/TransformStamped}, {'topic':swarm_goals, 'type':geometry_msgs/TransformStamped}] 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /launch/host/include/Astra_Pro_rgbd_driver.launch.xml: -------------------------------------------------------------------------------- 1 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /script/neuronbot_robot_id_filter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # Copyright 2017 ADLINK Technology, Inc. 3 | # Developer: HaoChih, LIN (haochih.lin@adlinktech.com) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import rospy 18 | import time 19 | from adlink_neuronbot.msg import MultiRobots, Robot 20 | from geometry_msgs.msg import TransformStamped 21 | 22 | class RobotIdFilter: 23 | def __init__(self, selfId, mapId, id_timeout, sim_mode, fake_reliability, fake_radius): 24 | self.sub = rospy.Subscriber('swarm_poses', TransformStamped, self.poseCB, queue_size=10) 25 | self.pub = rospy.Publisher('multi_robots', MultiRobots, queue_size=10) 26 | self.timer = rospy.Timer(rospy.Duration(0.1), self.timerCB) # 10Hz 27 | self.selfId = selfId 28 | self.mapId = mapId 29 | self.id_timeout = id_timeout 30 | self.sim_mode = sim_mode 31 | self.robotId_Dict = {} 32 | self.fake_reliability = fake_reliability 33 | self.fake_radius = fake_radius 34 | 35 | def poseCB(self, data): 36 | if data.child_frame_id == self.selfId: 37 | return 38 | # Update robot id in dict 39 | self.robotId_Dict[data.child_frame_id] = data 40 | 41 | def timerCB(self, event): 42 | if not self.robotId_Dict: # check if dict is empty or not 43 | return 44 | # Check the timeout of each robot id, then publish the data 45 | curr_time = rospy.get_time() 46 | if sim_mode: 47 | curr_time = (rospy.Time.from_sec(time.time())).to_sec() 48 | msg = MultiRobots() 49 | msg.header.stamp = rospy.Time.now() 50 | for key, value in self.robotId_Dict.items(): 51 | if (curr_time - value.header.stamp.to_sec()) >= self.id_timeout: 52 | del self.robotId_Dict[key] 53 | print value.child_frame_id + ' is too old, removed!' # for debug 54 | else: 55 | robot = Robot() 56 | robot.transform = value 57 | robot.reliability = self.fake_reliability 58 | robot.radius = self.fake_radius 59 | msg.robots.append(robot) 60 | self.pub.publish(msg) 61 | 62 | if __name__ == "__main__": 63 | try: 64 | # ROS Init 65 | rospy.init_node('robot_id_filter', anonymous=True) 66 | 67 | # Get params 68 | self_id = rospy.get_param('~self_id', 'robot_1') # robot frame id 69 | map_id = rospy.get_param('~map_id', 'map') # map frame id (default is map) 70 | id_timeout = float( rospy.get_param('~id_timeout', '2') ) # unit: sec 71 | sim_mode = rospy.get_param('~sim_mode', 'false') # for simulation 72 | fake_reliability = float( rospy.get_param('~fake_reliability', '0.9') ) # 0 ~ 1 (<=0 disable) 73 | fake_radius = float( rospy.get_param('~fake_radius', '0.12') ) # unit: meter (<=0 disable) 74 | if id_timeout <= 0: 75 | id_timeout = 0 76 | if fake_reliability <= 0: 77 | fake_reliability = 0.9 78 | if fake_radius <= 0: 79 | fake_radius = 0.12 80 | 81 | # Constract RobotIdFilter Obj 82 | rif = RobotIdFilter(self_id, map_id, id_timeout, sim_mode, fake_reliability, fake_radius) 83 | rospy.loginfo("Start Robot Id Filter ...") 84 | rospy.spin() 85 | except KeyboardInterrupt: 86 | print("shutting down") 87 | -------------------------------------------------------------------------------- /launch/client/NeuronBot_Demo_Client_Nav_Swarm.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 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | [{'topic':swarm_poses, 'type':geometry_msgs/TransformStamped}, {'topic':swarm_goals, 'type':geometry_msgs/TransformStamped}] 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # ADLINK NeuronBot 2 | 3 | ## Abstract 4 | The purpose of this pkg is to demonstrate two main features of ADLINK Neuron miniITX platform. 5 | 1. Computing Power: 6 | The host robot is able to follow human by integrating all outputs from SPENCER robot framework, intel "object analytics" pkg and laser based leg_tracker pkg. 7 | ADLINK Neuron illustates its computing power to run these detecting/tracking algorithms smoothly. (HOG+SVM, CNN, Template matching, EKF, MobileNetSSD, etc) 8 | 2. ROS2/DDS Capabilities: 9 | The host robot (for following) is publishing its own pose with respect to known map through ROS2/DDS layer while following a human. 10 | On the other hand, the client robot (random wandering), could avoid the host robot by receiving host's pose and replaning its path. 11 | 12 | [Official Slides] https://github.com/Adlink-ROS/adlink_neuronbot/blob/master/document/ADLINK_NeuronBot_20180313.pdf 13 | [Youtube Video] https://youtu.be/RC6XvTvTs9Y 14 | [Youtube Video] https://youtu.be/qA4_Hmnd_tM 15 | [![alt text](http://img.youtube.com/vi/RC6XvTvTs9Y/0.jpg)](https://youtu.be/RC6XvTvTs9Y) 16 | 17 | ## Developers & Team 18 | HaoChih Lin 19 | Alan Chen 20 | Chester Tseng 21 | Bill Wang 22 | Erik Boasson 23 | Ryan Chen 24 | 25 | ADLINK Technology, Inc 26 | Advanced Robotic Platform Group 27 | 28 | ## License 29 | Apache 2.0 30 | Copyright 2018 ADLINK Technology, Inc. 31 | 32 | ## Tutorial 33 | ### System Prerequisite 34 | [Packages] 35 | * Realsense D400 36 | Source: https://github.com/intel-ros/realsense 37 | Notice: About RealSense SDK 2.0, we highly recommed binary version. 38 | Testing: $ roslaunch realsense2_camera demo_pointcloud.launch 39 |
40 | * Astra Pro OR Astra (alternative choose for camera) 41 | Binary: $ sudo apt-get install ros-kinetic-astra* 42 | Source: https://github.com/orbbec/ros_astra_camera 43 | Notice: Remember to create udev. If possible, please buy Astra instead of Astra Pro! 44 |
45 | * YDLidar 46 | Source: https://github.com/EAIBOT/ydlidar 47 | Notice: Remember to laod udev. Could be replaced by any type of lidar. 48 | Testing: $ roslaunch ydlidar x4.launch 49 |
50 | * Navigation 51 | Binary: $ sudo apt-get install ros-kinetic-navigation* 52 | Source: https://github.com/ros-planning/navigation 53 | Notice: if "replan" mode of global planner is malfunctioned, please compile whole pkgs from source. 54 | Testing: $ roslaunch spencer_people_tracking_launch tracking_on_bagfile.launch 55 |
56 | * SPENCER 57 | Binary: https://github.com/spencer-project/spencer_people_tracking#installation-from-l-cas-package-repository 58 | Source: https://github.com/spencer-project/spencer_people_tracking#installation-from-source 59 | Notice: Unless you want to use HOG+SVM [CUDA required], we highly recommend binary version. 60 |
61 | * Intel object analytics 62 | Source: https://github.com/intel/ros_object_analytics 63 | Notice: Tested with NCSDK v1.12, ros_object_analytics should be "devel" branch 64 | Testing: 65 | ** movidius_ncs ** 66 | $ roslaunch realsense2_camera demo_pointcloud.launch 67 | $ roslaunch movidius_ncs_launch ncs_camera.launch cnn_type:=mobilenetssd input_topic:=/camera/color/image_raw 68 | $ roslaunch movidius_ncs_launch ncs_stream_detection_example.launch camera_topic:="/camera/color/image_raw" 69 | ** object_analytics ** 70 | $ roslaunch realsense2_camera demo_pointcloud.launch 71 | $ roslaunch object_analytics_launch analytics_movidius_ncs.launch input_points:=/camera/depth/color/points 72 |
73 | * leg_tracker 74 | Source: https://github.com/angusleigh/leg_tracker 75 | Notice: The kinetic branch only supports OpenCv 3.3 and higher ver. 76 | Testing: $ roslaunch leg_tracker demo_stationary_simple_environment.launch 77 |
78 | * Turtlebot2 79 | Binaty: $ sudo apt-get install ros-kinetic-turtlebot 80 | Source: https://github.com/turtlebot/turtlebot 81 | Notice: We highly recommed you to install binary version. 82 |
83 | 84 | ### Launching Steps 85 | * Mapping & Time Synchronizing 86 | * Host robot (for following) 87 | $ roslaunch adlink_neuronbot NeuronBot_Demo_Host_AIO.launch 88 | OR (script) 89 | $ ./PATH_TO_WORKSPACE/adlink_neuronbot/autostart/NeuronBot_Demo_Host_AutoStart.sh 90 | * Client robot (for avoidance) 91 | $ roslaunch adlink_neuronbot NeuronBot_Demo_Client_AIO.launch 92 | OR (script) 93 | $ ./PATH_TO_WORKSPACE/adlink_neuronbot/autostart/NeuronBot_Demo_Client_AutoStart.sh 94 | 95 | ### Known Issues 96 | * Q: move_base replanning does not work 97 | A: update your move_base pkg to the latest one, or compile it from source 98 | 99 | ### Roadmap 100 | * Fix PCL detector 101 | * ROS2 driver for camera 102 | * Fix followMe node (fused inputs version) 103 | * Create custom ROS2/DDS msg 104 | * Integrate with object_analytics 105 | 106 | -------------------------------------------------------------------------------- /launch/host/include/Astra_rgbd_driver.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 15 | 16 | 17 | 18 | 19 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 118 | 119 | 120 | 121 | 122 | -------------------------------------------------------------------------------- /script/neuronbot_multi_goals.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright 2018 ADLINK Technology, Inc. 3 | # Developer: HaoChih, LIN (haochih.lin@adlinktech.com) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import rospy 18 | import string 19 | import math 20 | import time 21 | import sys 22 | 23 | from std_msgs.msg import String 24 | from move_base_msgs.msg import MoveBaseActionResult 25 | from actionlib_msgs.msg import GoalStatusArray, GoalID 26 | from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped 27 | from kobuki_msgs.msg import BumperEvent, ButtonEvent 28 | 29 | class MultiGoals: 30 | def __init__(self, goalListX, goalListY, goalListZ, goalListW, retry, map_frame): 31 | self.goalPub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=2) 32 | self.stopPub = rospy.Publisher('/move_base/cancel', GoalID, queue_size=2) 33 | self.amclPub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=2) 34 | rospy.Subscriber('/move_base/result', MoveBaseActionResult, self.statusCB, queue_size=10) 35 | rospy.Subscriber('/mobile_base/events/bumper', BumperEvent, self.BumperCB) 36 | rospy.Subscriber('/mobile_base/events/button', ButtonEvent, self.ButtonCB) 37 | # rosparam (for amcl init pose) 38 | self.initMsg = PoseWithCovarianceStamped() 39 | self.initMsg.header.frame_id = map_frame 40 | self.initMsg.pose.pose.position.x = float( rospy.get_param('~initPosX', '0.0') ) # position 41 | self.initMsg.pose.pose.position.y = float( rospy.get_param('~initPosY', '0.0') ) # position 42 | self.initMsg.pose.pose.orientation.z = float( rospy.get_param('~initOriZ', '0.0') ) # orientation 43 | self.initMsg.pose.pose.orientation.w = float( rospy.get_param('~initOriW', '1.0') ) # orientation 44 | self.initMsg.pose.covariance[0] = float( rospy.get_param('~initCovX', '0.25') ) # covariance of x 45 | self.initMsg.pose.covariance[7] = float( rospy.get_param('~initCovY', '0.25') ) # covariance of y 46 | self.initMsg.pose.covariance[35] = float( rospy.get_param('~initCovA', '0.05') ) # covariance of theta 47 | # params & variables 48 | self.goalListX = goalListX 49 | self.goalListY = goalListY 50 | self.goalListZ = goalListZ 51 | self.goalListW = goalListW 52 | self.retry = retry 53 | self.status = False 54 | self.goalId = 0 55 | self.goalMsg = PoseStamped() 56 | self.goalMsg.header.frame_id = map_frame 57 | self.goalMsg.pose.orientation.z = 0.0 58 | self.goalMsg.pose.orientation.w = 1.0 59 | 60 | def BumperCB(self, msg): 61 | if msg.bumper >= 0: #left:0, center:1, right:2 62 | self.status = False 63 | emptyMsg = GoalID() 64 | self.stopPub.publish(emptyMsg) 65 | rospy.loginfo('Bumper: Stop!') 66 | 67 | def ButtonCB(self, msg): 68 | if msg.button == 0 and msg.state == 0: #left:0, center:1, right:2 69 | # Re-initialize the pose of robot w.r.t the map 70 | self.initMsg.header.stamp = rospy.Time.now() 71 | self.amclPub.publish(self.initMsg) 72 | rospy.loginfo("Re-initial the pose of robot, x: %d, y: %d", self.initMsg.pose.pose.position.x, self.initMsg.pose.pose.position.y) 73 | rospy.sleep(1.0) # wait 1 sec 74 | # Publish the first goal 75 | self.status = True 76 | self.goalId = 0 77 | self.goalMsg.header.stamp = rospy.Time.now() 78 | self.goalMsg.pose.position.x = self.goalListX[self.goalId] 79 | self.goalMsg.pose.position.y = self.goalListY[self.goalId] 80 | self.goalMsg.pose.orientation.z = self.goalListZ[self.goalId] 81 | self.goalMsg.pose.orientation.w = self.goalListW[self.goalId] 82 | self.goalPub.publish(self.goalMsg) 83 | rospy.loginfo("Initial goal published! Goal ID is: %d", self.goalId) 84 | self.goalId = self.goalId + 1 85 | 86 | if msg.button == 1 and msg.state == 0: #left:0, center:1, right:2 87 | # Publish the continued goal 88 | if self.goalId == 0: 89 | self.goalId = (len(self.goalListX)-1) 90 | else: 91 | self.goalId = self.goalId - 1 92 | self.status = True 93 | self.goalMsg.header.stamp = rospy.Time.now() 94 | self.goalMsg.pose.position.x = self.goalListX[self.goalId] 95 | self.goalMsg.pose.position.y = self.goalListY[self.goalId] 96 | self.goalMsg.pose.orientation.z = self.goalListZ[self.goalId] 97 | self.goalMsg.pose.orientation.w = self.goalListW[self.goalId] 98 | self.goalPub.publish(self.goalMsg) 99 | rospy.loginfo("Next goal published! Goal ID is: %d", self.goalId) 100 | if self.goalId < (len(self.goalListX)-1): 101 | self.goalId = self.goalId + 1 102 | else: 103 | self.goalId = 0 104 | 105 | def statusCB(self, data): 106 | if data.status.status == 3 and self.status: # reached & status 107 | self.goalMsg.header.stamp = rospy.Time.now() 108 | self.goalMsg.pose.position.x = self.goalListX[self.goalId] 109 | self.goalMsg.pose.position.y = self.goalListY[self.goalId] 110 | self.goalMsg.pose.orientation.z = self.goalListZ[self.goalId] 111 | self.goalMsg.pose.orientation.w = self.goalListW[self.goalId] 112 | self.goalPub.publish(self.goalMsg) 113 | rospy.loginfo("Reached! Next goal published, goal ID is: %d", self.goalId) 114 | if self.goalId < (len(self.goalListX)-1): 115 | self.goalId = self.goalId + 1 116 | else: 117 | self.goalId = 0 118 | 119 | 120 | 121 | if __name__ == "__main__": 122 | try: 123 | # ROS Init 124 | rospy.init_node('multi_goals', anonymous=True) 125 | 126 | # Get params 127 | goalListX = rospy.get_param('~goalListX', '[0.0, 1.0]') # position 128 | goalListY = rospy.get_param('~goalListY', '[0.0, 1.0]') # position 129 | goalListZ = rospy.get_param('~goalListZ', '[0.0, 0.0]') # orientation 130 | goalListW = rospy.get_param('~goalListW', '[1.0, 1.0]') # orientation 131 | map_frame = rospy.get_param('~map_frame', 'map' ) 132 | retry = rospy.get_param('~retry', '1') 133 | 134 | goalListX = goalListX.replace("[","").replace("]","") 135 | goalListY = goalListY.replace("[","").replace("]","") 136 | goalListZ = goalListZ.replace("[","").replace("]","") 137 | goalListW = goalListW.replace("[","").replace("]","") 138 | goalListX = [float(x) for x in goalListX.split(",")] 139 | goalListY = [float(y) for y in goalListY.split(",")] 140 | goalListZ = [float(z) for z in goalListZ.split(",")] 141 | goalListW = [float(w) for w in goalListW.split(",")] 142 | 143 | if len(goalListX) == len(goalListY) == len(goalListZ) == len(goalListW) and len(goalListX) >=2: 144 | # Constract MultiGoals Obj 145 | rospy.loginfo("Multi Goals Executing...") 146 | mg = MultiGoals(goalListX, goalListY, goalListZ, goalListW, retry, map_frame) 147 | rospy.spin() 148 | else: 149 | rospy.errinfo("Lengths of goal lists are not the same") 150 | except KeyboardInterrupt: 151 | print("shutting down") 152 | 153 | 154 | 155 | -------------------------------------------------------------------------------- /rviz/neuronbot_sim_swarm.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.5 9 | Tree Height: 531 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: LaserScan 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.699999988 33 | Class: rviz/Map 34 | Color Scheme: map 35 | Draw Behind: false 36 | Enabled: true 37 | Name: Map 38 | Topic: /map 39 | Unreliable: false 40 | Use Timestamp: false 41 | Value: true 42 | - Alpha: 1 43 | Class: rviz/Polygon 44 | Color: 0; 0; 255 45 | Enabled: true 46 | Name: Robot Footprint 47 | Topic: /move_base_node/local_costmap/footprint 48 | Unreliable: false 49 | Value: true 50 | - Alpha: 1 51 | Buffer Length: 1 52 | Class: rviz/Path 53 | Color: 255; 149; 0 54 | Enabled: true 55 | Head Diameter: 0.300000012 56 | Head Length: 0.200000003 57 | Length: 0.300000012 58 | Line Style: Lines 59 | Line Width: 0.0299999993 60 | Name: Global Plan 61 | Offset: 62 | X: 0 63 | Y: 0 64 | Z: 0 65 | Pose Color: 255; 85; 255 66 | Pose Style: None 67 | Radius: 0.0299999993 68 | Shaft Diameter: 0.100000001 69 | Shaft Length: 0.100000001 70 | Topic: /move_base_node/TrajectoryPlannerROS/global_plan 71 | Unreliable: false 72 | Value: true 73 | - Alpha: 1 74 | Buffer Length: 1 75 | Class: rviz/Path 76 | Color: 0; 0; 255 77 | Enabled: true 78 | Head Diameter: 0.300000012 79 | Head Length: 0.200000003 80 | Length: 0.300000012 81 | Line Style: Lines 82 | Line Width: 0.0299999993 83 | Name: Local Plan 84 | Offset: 85 | X: 0 86 | Y: 0 87 | Z: 0 88 | Pose Color: 255; 85; 255 89 | Pose Style: None 90 | Radius: 0.0299999993 91 | Shaft Diameter: 0.100000001 92 | Shaft Length: 0.100000001 93 | Topic: /move_base_node/TrajectoryPlannerROS/local_plan 94 | Unreliable: false 95 | Value: true 96 | - Alpha: 1 97 | Buffer Length: 1 98 | Class: rviz/Path 99 | Color: 255; 0; 0 100 | Enabled: true 101 | Head Diameter: 0.300000012 102 | Head Length: 0.200000003 103 | Length: 0.300000012 104 | Line Style: Lines 105 | Line Width: 0.0299999993 106 | Name: NavFn Plan 107 | Offset: 108 | X: 0 109 | Y: 0 110 | Z: 0 111 | Pose Color: 255; 85; 255 112 | Pose Style: None 113 | Radius: 0.0299999993 114 | Shaft Diameter: 0.100000001 115 | Shaft Length: 0.100000001 116 | Topic: /move_base_node/NavfnROS/plan 117 | Unreliable: false 118 | Value: true 119 | - Alpha: 1 120 | Arrow Length: 0.300000012 121 | Axes Length: 0.300000012 122 | Axes Radius: 0.00999999978 123 | Class: rviz/PoseArray 124 | Color: 255; 25; 0 125 | Enabled: true 126 | Head Length: 0.0700000003 127 | Head Radius: 0.0299999993 128 | Name: PoseArray 129 | Shaft Length: 0.230000004 130 | Shaft Radius: 0.00999999978 131 | Shape: Arrow (Flat) 132 | Topic: /particlecloud 133 | Unreliable: false 134 | Value: true 135 | - Alpha: 0.699999988 136 | Class: rviz/Map 137 | Color Scheme: costmap 138 | Draw Behind: false 139 | Enabled: true 140 | Name: Global Costmap 141 | Topic: /move_base_node/global_costmap/costmap 142 | Unreliable: false 143 | Use Timestamp: false 144 | Value: true 145 | - Alpha: 0.699999988 146 | Class: rviz/Map 147 | Color Scheme: map 148 | Draw Behind: false 149 | Enabled: true 150 | Name: Local Costmap 151 | Topic: /move_base_node/local_costmap/costmap 152 | Unreliable: false 153 | Use Timestamp: false 154 | Value: true 155 | - Alpha: 1 156 | Autocompute Intensity Bounds: true 157 | Autocompute Value Bounds: 158 | Max Value: 10 159 | Min Value: -10 160 | Value: true 161 | Axis: Z 162 | Channel Name: intensity 163 | Class: rviz/LaserScan 164 | Color: 255; 255; 0 165 | Color Transformer: FlatColor 166 | Decay Time: 0 167 | Enabled: true 168 | Invert Rainbow: false 169 | Max Color: 255; 255; 255 170 | Max Intensity: 1 171 | Min Color: 0; 0; 0 172 | Min Intensity: 1 173 | Name: LaserScan 174 | Position Transformer: XYZ 175 | Queue Size: 10 176 | Selectable: true 177 | Size (Pixels): 3 178 | Size (m): 0.100000001 179 | Style: Flat Squares 180 | Topic: /base_scan 181 | Unreliable: false 182 | Use Fixed Frame: true 183 | Use rainbow: true 184 | Value: true 185 | Enabled: true 186 | Global Options: 187 | Background Color: 48; 48; 48 188 | Fixed Frame: map 189 | Frame Rate: 30 190 | Name: root 191 | Tools: 192 | - Class: rviz/Interact 193 | Hide Inactive Objects: true 194 | - Class: rviz/MoveCamera 195 | - Class: rviz/Select 196 | - Class: rviz/FocusCamera 197 | - Class: rviz/Measure 198 | - Class: rviz/SetInitialPose 199 | Topic: /initialpose 200 | - Class: rviz/SetGoal 201 | Topic: /move_base_simple/goal 202 | - Class: rviz/PublishPoint 203 | Single click: true 204 | Topic: /clicked_point 205 | Value: true 206 | Views: 207 | Current: 208 | Class: rviz/ThirdPersonFollower 209 | Distance: 14.4803343 210 | Enable Stereo Rendering: 211 | Stereo Eye Separation: 0.0599999987 212 | Stereo Focal Distance: 1 213 | Swap Stereo Eyes: false 214 | Value: false 215 | Focal Point: 216 | X: 3.95683193 217 | Y: 2.15113711 218 | Z: -5.2132069e-05 219 | Focal Shape Fixed Size: true 220 | Focal Shape Size: 0.0500000007 221 | Invert Z Axis: false 222 | Name: Current View 223 | Near Clip Distance: 0.00999999978 224 | Pitch: 1.56979632 225 | Target Frame: map 226 | Value: ThirdPersonFollower (rviz) 227 | Yaw: 4.71041393 228 | Saved: ~ 229 | Window Geometry: 230 | Displays: 231 | collapsed: false 232 | Height: 744 233 | Hide Left Dock: false 234 | Hide Right Dock: true 235 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000002a2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000002540000011a00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002a2000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000286fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000286000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fc0000003efc0100000002fb0000000800540069006d00650000000000000004fc0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003a5000002a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 236 | Selection: 237 | collapsed: false 238 | Time: 239 | collapsed: false 240 | Tool Properties: 241 | collapsed: false 242 | Views: 243 | collapsed: true 244 | Width: 1301 245 | X: 65 246 | Y: 24 247 | -------------------------------------------------------------------------------- /launch/host/NeuronBot_Demo_Host_Detection.launch: -------------------------------------------------------------------------------- 1 | 2 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | -------------------------------------------------------------------------------- /src/swarm_layer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | # Copyright 2017 ADLINK Technology, Inc. 3 | # Developer: HaoChih, LIN (haochih.lin@adlinktech.com) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | */ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | PLUGINLIB_EXPORT_CLASS(neuronbot_swarm_layer::SwarmLayer, costmap_2d::Layer) 23 | 24 | using costmap_2d::NO_INFORMATION; 25 | using costmap_2d::LETHAL_OBSTACLE; 26 | using costmap_2d::FREE_SPACE; 27 | 28 | namespace neuronbot_swarm_layer 29 | { 30 | void SwarmLayer::onInitialize() 31 | { 32 | ros::NodeHandle nh("~/" + name_), g_nh; 33 | current_ = true; 34 | first_time_ = true; 35 | robots_sub_ = nh.subscribe("/multi_robots", 1, &SwarmLayer::robotsCallback, this); 36 | // Dynamic recofigure 37 | server_ = new dynamic_reconfigure::Server(nh); 38 | f_ = boost::bind(&SwarmLayer::configure, this, _1, _2); 39 | server_->setCallback(f_); 40 | } 41 | 42 | void SwarmLayer::robotsCallback(const adlink_neuronbot::MultiRobots& robots) 43 | { 44 | boost::recursive_mutex::scoped_lock lock(lock_); 45 | robots_list_ = robots; 46 | } 47 | 48 | double SwarmLayer::gaussian(double x, double y, double x0, double y0, double A, double varx, double vary, double skew) 49 | { 50 | double dx = x-x0, dy = y-y0; 51 | double h = sqrt(dx*dx+dy*dy); 52 | double angle = atan2(dy,dx); 53 | double mx = cos(angle-skew) * h; 54 | double my = sin(angle-skew) * h; 55 | double f1 = pow(mx, 2.0)/(2.0 * varx), 56 | f2 = pow(my, 2.0)/(2.0 * vary); 57 | return A * exp(-(f1 + f2)); 58 | } 59 | 60 | void SwarmLayer::updateBoundsFromRobots(double* min_x, double* min_y, double* max_x, double* max_y) 61 | { 62 | std::list::iterator r_it; 63 | 64 | for(r_it = transformed_robots_.begin(); r_it != transformed_robots_.end(); ++r_it) 65 | { 66 | adlink_neuronbot::Robot robot = *r_it; 67 | double radius = robot.radius; 68 | 69 | *min_x = std::min(*min_x, robot.transform.transform.translation.x - radius); 70 | *min_y = std::min(*min_y, robot.transform.transform.translation.y - radius); 71 | *max_x = std::max(*max_x, robot.transform.transform.translation.x + radius); 72 | *max_y = std::max(*max_y, robot.transform.transform.translation.y + radius); 73 | 74 | } 75 | } 76 | 77 | 78 | void SwarmLayer::updateBounds(double origin_x, double origin_y, double origin_z, double* min_x, double* min_y, double* max_x, double* max_y) 79 | { 80 | boost::recursive_mutex::scoped_lock lock(lock_); 81 | 82 | std::string global_frame = layered_costmap_->getGlobalFrameID(); 83 | transformed_robots_.clear(); 84 | 85 | for(unsigned int i=0; i::iterator r_it; 160 | costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap(); 161 | double res = costmap->getResolution(); 162 | 163 | for(r_it = transformed_robots_.begin(); r_it != transformed_robots_.end(); ++r_it) 164 | { 165 | adlink_neuronbot::Robot robot = *r_it; 166 | double radius = robot.radius; 167 | 168 | unsigned int width = int( (radius*2) / res ), 169 | height = int( (radius*2) / res ); 170 | 171 | double center_Wx = robot.transform.transform.translation.x, 172 | center_Wy = robot.transform.transform.translation.y; 173 | 174 | double origin_Wx, origin_Wy; 175 | origin_Wx = center_Wx - radius; 176 | origin_Wy = center_Wy - radius; 177 | 178 | int dx, dy; 179 | costmap->worldToMapNoBounds(origin_Wx, origin_Wy, dx, dy); 180 | int cx, cy; 181 | costmap->worldToMapNoBounds(center_Wx, center_Wy, cx, cy); 182 | 183 | int start_x=0, start_y=0, end_x=width, end_y=height; 184 | if(dx < 0) 185 | start_x = -dx; 186 | else if(dx + width > costmap->getSizeInCellsX()) 187 | end_x = (int)costmap->getSizeInCellsX() - dx; 188 | 189 | if((int)(start_x+dx) < min_i) 190 | start_x = min_i - dx; 191 | if((int)(end_x+dx) > max_i) 192 | end_x = max_i - dx; 193 | 194 | if(dy < 0) 195 | start_y = -dy; 196 | else if(dy + height > costmap->getSizeInCellsY()) 197 | end_y = (int)costmap->getSizeInCellsY() - dy; 198 | 199 | if((int)(start_y+dy) < min_j) 200 | start_y = min_j - dy; 201 | if((int)(end_y+dy) > max_j) 202 | end_y = max_j - dy; 203 | 204 | // label the footprint of robot on the costmap (lethal) 205 | for(int i=start_x;igetCost(i+dx, j+dy); 210 | if(old_cost == costmap_2d::NO_INFORMATION) 211 | continue; 212 | 213 | if( std::sqrt( (i+dx-cx)*(i+dx-cx) + (j+dy-cy)*(j+dy-cy) ) > (radius/res) ) 214 | continue; 215 | 216 | costmap->setCost(i+dx, j+dy, costmap_2d::LETHAL_OBSTACLE); 217 | } 218 | } 219 | } 220 | } 221 | 222 | 223 | void SwarmLayer::configure(adlink_neuronbot::SwarmLayerConfig &config, uint32_t level) 224 | { 225 | enabled_ = config.enabled; 226 | } 227 | 228 | }; // end of namespace 229 | -------------------------------------------------------------------------------- /rviz/neuronbot_demo_client.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.5 9 | Tree Height: 853 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: LaserScan 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 | - Alpha: 0.699999988 51 | Class: rviz/Map 52 | Color Scheme: map 53 | Draw Behind: false 54 | Enabled: true 55 | Name: Map 56 | Topic: /map 57 | Unreliable: false 58 | Use Timestamp: false 59 | Value: true 60 | - Alpha: 1 61 | Arrow Length: 0.300000012 62 | Axes Length: 0.300000012 63 | Axes Radius: 0.00999999978 64 | Class: rviz/PoseArray 65 | Color: 255; 25; 0 66 | Enabled: true 67 | Head Length: 0.0700000003 68 | Head Radius: 0.0299999993 69 | Name: PoseArray 70 | Shaft Length: 0.230000004 71 | Shaft Radius: 0.00999999978 72 | Shape: Arrow (Flat) 73 | Topic: /particlecloud 74 | Unreliable: false 75 | Value: true 76 | - Class: rviz/TF 77 | Enabled: true 78 | Frame Timeout: 15 79 | Frames: 80 | All Enabled: true 81 | base_footprint: 82 | Value: true 83 | laser_frame: 84 | Value: true 85 | map: 86 | Value: true 87 | odom: 88 | Value: true 89 | Marker Scale: 1 90 | Name: TF 91 | Show Arrows: true 92 | Show Axes: true 93 | Show Names: true 94 | Tree: 95 | map: 96 | odom: 97 | base_footprint: 98 | laser_frame: 99 | {} 100 | Update Interval: 0 101 | Value: true 102 | - Alpha: 1 103 | Axes Length: 1 104 | Axes Radius: 0.100000001 105 | Class: rviz/Pose 106 | Color: 255; 25; 0 107 | Enabled: true 108 | Head Length: 0.300000012 109 | Head Radius: 0.100000001 110 | Name: Pose 111 | Shaft Length: 1 112 | Shaft Radius: 0.0500000007 113 | Shape: Arrow 114 | Topic: /move_base_simple/goal 115 | Unreliable: false 116 | Value: true 117 | - Alpha: 0.5 118 | Class: rviz/Map 119 | Color Scheme: costmap 120 | Draw Behind: false 121 | Enabled: true 122 | Name: global_costmap 123 | Topic: /move_base/global_costmap/costmap 124 | Unreliable: false 125 | Use Timestamp: false 126 | Value: true 127 | - Alpha: 0.600000024 128 | Class: rviz/Map 129 | Color Scheme: raw 130 | Draw Behind: false 131 | Enabled: true 132 | Name: local_costmap 133 | Topic: /move_base/local_costmap/costmap 134 | Unreliable: false 135 | Use Timestamp: false 136 | Value: true 137 | - Alpha: 1 138 | Buffer Length: 1 139 | Class: rviz/Path 140 | Color: 25; 255; 0 141 | Enabled: true 142 | Head Diameter: 0.300000012 143 | Head Length: 0.200000003 144 | Length: 0.300000012 145 | Line Style: Lines 146 | Line Width: 0.0299999993 147 | Name: global_path 148 | Offset: 149 | X: 0 150 | Y: 0 151 | Z: 0 152 | Pose Color: 255; 85; 255 153 | Pose Style: None 154 | Radius: 0.0299999993 155 | Shaft Diameter: 0.100000001 156 | Shaft Length: 0.100000001 157 | Topic: /move_base/NavfnROS/plan 158 | Unreliable: false 159 | Value: true 160 | - Alpha: 1 161 | Buffer Length: 1 162 | Class: rviz/Path 163 | Color: 0; 0; 255 164 | Enabled: true 165 | Head Diameter: 0.300000012 166 | Head Length: 0.200000003 167 | Length: 0.300000012 168 | Line Style: Lines 169 | Line Width: 0.0299999993 170 | Name: local_path 171 | Offset: 172 | X: 0 173 | Y: 0 174 | Z: 0 175 | Pose Color: 255; 85; 255 176 | Pose Style: None 177 | Radius: 0.0299999993 178 | Shaft Diameter: 0.100000001 179 | Shaft Length: 0.100000001 180 | Topic: /move_base/DWAPlannerROS/local_plan 181 | Unreliable: false 182 | Value: true 183 | - Alpha: 1 184 | Class: rviz/Polygon 185 | Color: 25; 255; 0 186 | Enabled: true 187 | Name: Polygon 188 | Topic: /move_base/global_costmap/footprint 189 | Unreliable: false 190 | Value: true 191 | - Alpha: 1 192 | Autocompute Intensity Bounds: true 193 | Autocompute Value Bounds: 194 | Max Value: 10 195 | Min Value: -10 196 | Value: true 197 | Axis: Z 198 | Channel Name: intensity 199 | Class: rviz/LaserScan 200 | Color: 255; 255; 255 201 | Color Transformer: Intensity 202 | Decay Time: 0 203 | Enabled: true 204 | Invert Rainbow: false 205 | Max Color: 255; 255; 255 206 | Max Intensity: 10 207 | Min Color: 0; 0; 0 208 | Min Intensity: 10 209 | Name: LaserScan 210 | Position Transformer: XYZ 211 | Queue Size: 10 212 | Selectable: true 213 | Size (Pixels): 3 214 | Size (m): 0.0299999993 215 | Style: Flat Squares 216 | Topic: /scan 217 | Unreliable: false 218 | Use Fixed Frame: true 219 | Use rainbow: true 220 | Value: true 221 | Enabled: true 222 | Global Options: 223 | Background Color: 48; 48; 48 224 | Default Light: true 225 | Fixed Frame: map 226 | Frame Rate: 30 227 | Name: root 228 | Tools: 229 | - Class: rviz/Interact 230 | Hide Inactive Objects: true 231 | - Class: rviz/MoveCamera 232 | - Class: rviz/Select 233 | - Class: rviz/FocusCamera 234 | - Class: rviz/Measure 235 | - Class: rviz/SetInitialPose 236 | Topic: /initialpose 237 | - Class: rviz/SetGoal 238 | Topic: /move_base_simple/goal 239 | - Class: rviz/PublishPoint 240 | Single click: true 241 | Topic: /clicked_point 242 | Value: true 243 | Views: 244 | Current: 245 | Class: rviz/Orbit 246 | Distance: 11.1697025 247 | Enable Stereo Rendering: 248 | Stereo Eye Separation: 0.0599999987 249 | Stereo Focal Distance: 1 250 | Swap Stereo Eyes: false 251 | Value: false 252 | Focal Point: 253 | X: -0.308891684 254 | Y: -1.08547711 255 | Z: -1.02487135 256 | Focal Shape Fixed Size: true 257 | Focal Shape Size: 0.0500000007 258 | Invert Z Axis: false 259 | Name: Current View 260 | Near Clip Distance: 0.00999999978 261 | Pitch: 1.56979632 262 | Target Frame: 263 | Value: Orbit (rviz) 264 | Yaw: 2.62358475 265 | Saved: ~ 266 | Window Geometry: 267 | Displays: 268 | collapsed: false 269 | Height: 1056 270 | Hide Left Dock: false 271 | Hide Right Dock: true 272 | QMainWindow State: 000000ff00000000fd00000004000000000000026100000396fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000010b000000b10000000000000000fb0000000a0049006d00610067006500000001510000018c0000000000000000fb0000003400550070007000650072002d0042006f0064007900200044006500740065006300740069006f006e00200049006d00610067006501000002f5000000160000000000000000fb0000000a0049006d00610067006500000002440000015e0000000000000000000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004d80000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 273 | Selection: 274 | collapsed: false 275 | Time: 276 | collapsed: false 277 | Tool Properties: 278 | collapsed: false 279 | Views: 280 | collapsed: true 281 | Width: 1855 282 | X: 65 283 | Y: 24 284 | -------------------------------------------------------------------------------- /script/neuronbot_following_fused.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright 2018 ADLINK Technology, Inc. 3 | # Developer: HaoChih, LIN (haochih.lin@adlinktech.com) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import rospy 18 | import sys 19 | import time 20 | import math 21 | import tf 22 | from std_msgs.msg import * 23 | from geometry_msgs.msg import Twist, PoseStamped 24 | from spencer_tracking_msgs.msg import TrackedPersons 25 | from dynamic_reconfigure.server import Server 26 | from adlink_neuronbot.cfg import FollowingConfig 27 | 28 | class FollowMe: 29 | def __init__(self, status): 30 | self.Ki_linear_window = 0.95 31 | self.Ki_angular_window = 0.95 32 | self.linear_bound = 0.3 33 | self.angular_bound = 0.4 34 | self.target_id = -1 35 | self.status = status 36 | self.sum_error_r = 0 37 | self.sum_error_th = 0 38 | self.previous = 0 39 | self.first_dcb = True # To avoid Dynamic Server CB changing params at beginning 40 | self.listener = tf.TransformListener() 41 | self.debug = rospy.get_param('debug_info', True) 42 | self.following_distance = rospy.get_param('~following_distance', 0.9) 43 | self.dead_zone_radius = rospy.get_param('~dead_zone_radius', 0.1) # +/- m 44 | self.dead_zone_theta = rospy.get_param('~dead_zone_theta', 0.1) # +/- rad 45 | self.search_radius = rospy.get_param('~search_radius', 1.5) # +/- m 46 | self.search_theta = rospy.get_param('~search_theta', 0.4) # +/- rad 47 | self.Kp_linear = rospy.get_param('~Kp_linear', 0.8) 48 | self.Kp_angular = rospy.get_param('~Kp_angular', 1.5) 49 | self.Ki_linear = rospy.get_param('~Ki_linear', 0.0) 50 | self.Ki_angular = rospy.get_param('~Ki_angular', 0.0) 51 | self.Ki_factor_linear = rospy.get_param('~Ki_factor_linear', 0.95) 52 | self.Ki_factor_angular = rospy.get_param('~Ki_factor_angular', 0.95) 53 | self.base_frame = rospy.get_param('~base_frame', "/base_footprint") 54 | self.cmd_pub = rospy.Publisher('/following/cmd_vel', Twist, queue_size = 50) 55 | Server(FollowingConfig, self.dynamicCB) 56 | rospy.Subscriber('/following/mode', String, self.followCB) 57 | rospy.Subscriber('/following/persons', TrackedPersons, self.trackerCB) 58 | 59 | def dynamicCB(self, config, level): 60 | # Update the default values of all dynamical params 61 | if self.first_dcb: 62 | config.Status = self.status 63 | config.Distance = self.following_distance 64 | config.Search_radius = self.search_radius 65 | config.Search_theta = self.search_theta 66 | config.Kp_linear = self.Kp_linear 67 | config.Kp_angular = self.Kp_angular 68 | config.Ki_linear = self.Ki_linear 69 | config.Ki_angular = self.Ki_angular 70 | config.DeadZone_radius = self.dead_zone_radius 71 | config.DeadZone_theta = self.dead_zone_theta 72 | self.first_dcb = False 73 | return config 74 | 75 | self.status = config.Status 76 | self.following_distance = config.Distance 77 | self.search_radius = config.Search_radius 78 | self.search_theta = config.Search_theta 79 | self.Kp_linear = config.Kp_linear 80 | self.Kp_angular = config.Kp_angular 81 | self.Ki_linear = config.Ki_linear 82 | self.Ki_angular = config.Ki_angular 83 | self.dead_zone_radius = config.DeadZone_radius 84 | self.dead_zone_theta = config.DeadZone_theta 85 | 86 | if self.status == False: 87 | self.send_cmd(0.0, 0.0) 88 | rospy.loginfo('Stop following') 89 | if self.debug: 90 | rospy.loginfo('Dynamical Parameters Updated!') 91 | return config 92 | 93 | 94 | def followCB(self, String): 95 | if String.data == 'start': 96 | self.status = True 97 | rospy.loginfo('Start following') 98 | elif String.data == 'stop': 99 | self.status = False 100 | self.send_cmd(0.0, 0.0) 101 | rospy.loginfo('Stop following') 102 | else: 103 | rospy.logerr('Wrong type msg') 104 | 105 | def trackerCB(self, msg): 106 | if self.status: 107 | if self.target_id == -1: 108 | self.lock_target_id(msg) 109 | elif msg.tracks == []: 110 | self.target_id = -1 111 | rospy.loginfo('target lost') 112 | self.send_cmd(0.0, 0.0) 113 | else: 114 | r,th = self.get_target_position(msg) 115 | self.following(r,th) 116 | 117 | def xyToPolar(self, Persons, i): 118 | # transform PoseStamped from /odom to /base_link 119 | try: 120 | odomPerson = PoseStamped() 121 | basePerson = PoseStamped() 122 | odomPerson.header = Persons.header 123 | odomPerson.pose = Persons.tracks[i].pose.pose 124 | basePerson = self.listener.transformPose(self.base_frame, odomPerson) 125 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 126 | rospy.loginfo('tf listening failed') 127 | return -1, -1 128 | target_x = basePerson.pose.position.x 129 | target_y = basePerson.pose.position.y 130 | r = math.sqrt( math.pow(target_x, 2) + math.pow(target_y, 2) ) 131 | th = math.atan2(target_y,target_x) 132 | return r,th 133 | 134 | def get_target_position(self, Persons): 135 | target_get = False 136 | for i in range(0,len(Persons.tracks)): 137 | if Persons.tracks[i].track_id == self.target_id and Persons.tracks[i].is_matched == True: 138 | r,th = self.xyToPolar(Persons, i) 139 | target_get = True 140 | return r,th 141 | if not target_get: 142 | self.target_id = -1 143 | rospy.loginfo('target lost') 144 | self.send_cmd(0.0, 0.0) 145 | return -1, -1 146 | 147 | def lock_target_id(self, Persons): 148 | for i in range(0, len(Persons.tracks)): 149 | r,th = self.xyToPolar(Persons, i) 150 | if r <= self.search_radius and -self.search_theta <= th and th <= self.search_theta: 151 | self.target_id = Persons.tracks[i].track_id 152 | rospy.loginfo('target locked') 153 | 154 | def constrain(self, num, limit_upper, limit_lower): 155 | if num >= limit_upper: 156 | num = limit_upper 157 | return num 158 | elif num <= limit_lower: 159 | num = limit_lower 160 | return num 161 | else: 162 | return num 163 | 164 | def send_cmd(self, linear_x, angular_z): 165 | motion_cmd = Twist() 166 | motion_cmd.angular.z = angular_z 167 | motion_cmd.linear.x = linear_x 168 | self.cmd_pub.publish(motion_cmd) 169 | 170 | def following(self, r, th): 171 | if r == -1: 172 | rospy.loginfo('target pose error !') 173 | return -1 174 | current_time = rospy.Time.now().to_sec() 175 | dt = current_time - self.previous 176 | # Calculate PI control 177 | error_r = r - self.following_distance 178 | error_th = th - 0.0; 179 | if abs(error_r) >= self.dead_zone_radius: 180 | self.sum_error_r = self.Ki_factor_linear*self.Ki_linear_window*self.sum_error_r + error_r*dt 181 | follow_speed_x = self.Kp_linear * error_r + self.Ki_linear * (self.sum_error_r) 182 | follow_speed_x = self.constrain(follow_speed_x, self.linear_bound, -self.linear_bound) 183 | else: 184 | follow_speed_x = 0.0 185 | if self.debug: 186 | rospy.loginfo('Linear Dead Zone!') 187 | 188 | if abs(error_th) >= self.dead_zone_theta: 189 | self.sum_error_th = self.Ki_factor_angular*self.Ki_angular_window*self.sum_error_th + error_th*dt 190 | follow_speed_z = self.Kp_angular * (th) + self.Ki_angular * (self.sum_error_th) 191 | follow_speed_z = self.constrain(follow_speed_z, self.angular_bound, -self.angular_bound) 192 | else: 193 | follow_speed_z = 0.0 194 | if self.debug: 195 | rospy.loginfo('Angular Dead Zone!') 196 | 197 | # Pub vel cmd 198 | self.send_cmd(follow_speed_x, follow_speed_z) 199 | self.previous = current_time 200 | if self.debug: 201 | rospy.loginfo('Target ID: ' + str(self.target_id) + ', r: ' + str(r) + ', th: ' + str(th)) 202 | rospy.loginfo('PID linear: ' + str(follow_speed_x) + ', angular: ' + str(follow_speed_z)) 203 | 204 | # Main function. 205 | if __name__ == '__main__': 206 | # Initialize the node and name it. 207 | rospy.init_node("Follow_me", anonymous = False) 208 | rospy.loginfo("===== neuronbot following node (fused) =====") 209 | rospy.loginfo("Waiting for TrackedPersons msg ...") 210 | # Go to class functions that do all the heavy lifting. Do error checking. 211 | try: 212 | fm = FollowMe(True) 213 | rospy.spin() 214 | except rospy.ROSInterruptException: 215 | pass 216 | -------------------------------------------------------------------------------- /script/neuronbot_following_legs.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright 2018 ADLINK Technology, Inc. 3 | # Developer: HaoChih, LIN (haochih.lin@adlinktech.com) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import rospy 18 | import sys 19 | import time 20 | import math 21 | import tf 22 | from geometry_msgs.msg import Twist, Pose2D, PoseStamped, PoseWithCovarianceStamped 23 | from leg_tracker.msg import * 24 | from dynamic_reconfigure.server import Server 25 | from adlink_neuronbot.cfg import FollowingConfig 26 | from kobuki_msgs.msg import BumperEvent, ButtonEvent 27 | 28 | class FollowMe: 29 | def __init__(self, status): 30 | self.Ki_linear_window = 0.95 31 | self.Ki_angular_window = 0.95 32 | self.linear_bound = 0.17 33 | self.angular_bound = 1.2 #0.65 34 | self.target_id = -1 35 | self.status = status 36 | self.sum_error_r = 0 37 | self.sum_error_th = 0 38 | self.previous = 0 39 | self.first_dcb = True # To avoid Dynamic Server CB changing params at beginning 40 | self.listener = tf.TransformListener() 41 | self.bumper_flag = False # False: continue, True: stop 42 | self.debug = rospy.get_param('debug_info', True) 43 | self.following_distance = rospy.get_param('~following_distance', 0.9) 44 | self.dead_zone_radius = rospy.get_param('~dead_zone_radius', 0.1) # +/- m 45 | self.dead_zone_theta = rospy.get_param('~dead_zone_theta', 0.1) # +/- rad 46 | self.search_radius = rospy.get_param('~search_radius', 1.5) # +/- m 47 | self.search_theta = rospy.get_param('~search_theta', 0.4) # +/- rad 48 | self.Kp_linear = rospy.get_param('~Kp_linear', 0.45) 49 | self.Kp_angular = rospy.get_param('~Kp_angular', 1.0) 50 | self.Ki_linear = rospy.get_param('~Ki_linear', 0.0) 51 | self.Ki_angular = rospy.get_param('~Ki_angular', 0.0) 52 | self.Ki_factor_linear = rospy.get_param('~Ki_factor_linear', 0.95) 53 | self.Ki_factor_angular = rospy.get_param('~Ki_factor_angular', 0.95) 54 | self.base_frame = rospy.get_param('~base_frame', "/base_footprint") 55 | self.cmd_pub = rospy.Publisher('/following/cmd_vel', Twist, queue_size = 50) 56 | Server(FollowingConfig, self.dynamicCB) 57 | rospy.Subscriber('/following/legs', PersonArray, self.trackerCB) 58 | rospy.Subscriber('/mobile_base/events/bumper', BumperEvent, self.BumperCB) 59 | rospy.Subscriber('/mobile_base/events/button', ButtonEvent, self.ButtonCB) 60 | 61 | # rosparam (for amcl init pose) 62 | self.amclPub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=2) 63 | self.initMsg = PoseWithCovarianceStamped() 64 | self.initMsg.header.frame_id = rospy.get_param('~map_frame', 'map') 65 | self.initMsg.pose.pose.position.x = rospy.get_param('~initPosX', 0.662) # position 66 | self.initMsg.pose.pose.position.y = rospy.get_param('~initPosY', 0.427) # position 67 | self.quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, rospy.get_param('~initPosA', 2.612)) 68 | self.initMsg.pose.pose.orientation.z = self.quaternion[2] # orientation 69 | self.initMsg.pose.pose.orientation.w = self.quaternion[3] # orientation 70 | self.initMsg.pose.covariance[0] = rospy.get_param('~initCovX', 0.25) # covariance of x 71 | self.initMsg.pose.covariance[7] = rospy.get_param('~initCovY', 0.25) # covariance of y 72 | self.initMsg.pose.covariance[35] = rospy.get_param('~initCovA', 0.05) # covariance of theta 73 | 74 | def BumperCB(self, msg): 75 | if msg.bumper >= 0: #left:0, center:1, right:2 76 | self.target_id = -1 77 | self.send_cmd(0.0, 0.0) 78 | rospy.loginfo('Bumper: Stop!') 79 | self.bumper_flag = True 80 | 81 | def ButtonCB(self, msg): 82 | if msg.button == 0: #left:0, center:1, right:2 83 | # Re-initialize the pose of robot w.r.t the map 84 | self.initMsg.header.stamp = rospy.Time.now() 85 | self.amclPub.publish(self.initMsg) 86 | rospy.loginfo("Re-initial the pose of robot, x: %d, y: %d", self.initMsg.pose.pose.position.x, self.initMsg.pose.pose.position.y) 87 | rospy.sleep(1.0) # wait 1 sec 88 | if msg.button >= 0: #left:0, center:1, right:2 89 | self.bumper_flag = False 90 | rospy.loginfo('Button: Release!') 91 | 92 | def dynamicCB(self, config, level): 93 | # Update the default values of all dynamical params 94 | if self.first_dcb: 95 | config.Status = self.status 96 | config.Distance = self.following_distance 97 | config.Search_radius = self.search_radius 98 | config.Search_theta = self.search_theta 99 | config.Kp_linear = self.Kp_linear 100 | config.Kp_angular = self.Kp_angular 101 | config.Ki_linear = self.Ki_linear 102 | config.Ki_angular = self.Ki_angular 103 | config.DeadZone_radius = self.dead_zone_radius 104 | config.DeadZone_theta = self.dead_zone_theta 105 | self.first_dcb = False 106 | return config 107 | 108 | self.status = config.Status 109 | self.following_distance = config.Distance 110 | self.search_radius = config.Search_radius 111 | self.search_theta = config.Search_theta 112 | self.Kp_linear = config.Kp_linear 113 | self.Kp_angular = config.Kp_angular 114 | self.Ki_linear = config.Ki_linear 115 | self.Ki_angular = config.Ki_angular 116 | self.dead_zone_radius = config.DeadZone_radius 117 | self.dead_zone_theta = config.DeadZone_theta 118 | 119 | if self.status == False: 120 | self.send_cmd(0.0, 0.0) 121 | rospy.loginfo('Stop following') 122 | if self.debug: 123 | rospy.loginfo('Dynamical Parameters Updated!') 124 | return config 125 | 126 | def trackerCB(self, msg): 127 | if self.status and self.bumper_flag == False: 128 | if self.target_id == -1: 129 | self.lock_target_id(msg) 130 | elif msg.people == []: 131 | self.target_id = -1 132 | rospy.loginfo('target lost') 133 | self.send_cmd(0.05, 0.0) 134 | else: 135 | r,th = self.get_target_position(msg) 136 | self.following(r,th) 137 | 138 | def xyToPolar(self, PersonArray, i): 139 | # transform PoseStamped from /odom to /base_link 140 | try: 141 | odomPerson = PoseStamped() 142 | basePerson = PoseStamped() 143 | odomPerson.header = PersonArray.header 144 | odomPerson.pose = PersonArray.people[i].pose 145 | basePerson = self.listener.transformPose(self.base_frame, odomPerson) 146 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 147 | rospy.loginfo('tf listening failed') 148 | return -1, -1 149 | target_x = basePerson.pose.position.x 150 | target_y = basePerson.pose.position.y 151 | r = math.sqrt( math.pow(target_x, 2) + math.pow(target_y, 2) ) 152 | th = math.atan2(target_y,target_x) 153 | return r,th 154 | 155 | def get_target_position(self, PersonArray): 156 | target_get = False 157 | for i in range(0,len(PersonArray.people)): 158 | if PersonArray.people[i].id == self.target_id: 159 | r,th = self.xyToPolar(PersonArray,i) 160 | target_get = True 161 | return r,th 162 | if not target_get: 163 | self.target_id = -1 164 | rospy.loginfo('target lost') 165 | self.send_cmd(0.05, 0.0) 166 | return -1, -1 167 | 168 | def lock_target_id(self, PersonArray): 169 | for i in range(0, len(PersonArray.people)): 170 | r,th = self.xyToPolar(PersonArray,i) 171 | if r <= self.search_radius and -self.search_theta <= th and th <= self.search_theta: 172 | self.target_id = PersonArray.people[i].id 173 | rospy.loginfo('target locked') 174 | 175 | def constrain(self, num, limit_upper, limit_lower): 176 | if num >= limit_upper: 177 | num = limit_upper 178 | return num 179 | elif num <= limit_lower: 180 | num = limit_lower 181 | return num 182 | else: 183 | return num 184 | 185 | def send_cmd(self, linear_x, angular_z): 186 | motion_cmd = Twist() 187 | motion_cmd.angular.z = angular_z 188 | motion_cmd.linear.x = linear_x 189 | if self.bumper_flag == False: 190 | self.cmd_pub.publish(motion_cmd) 191 | 192 | def following(self, r, th): 193 | if r == -1: 194 | rospy.loginfo('target pose error !') 195 | return -1 196 | current_time = rospy.Time.now().to_sec() 197 | dt = current_time - self.previous 198 | # Calculate PI control 199 | error_r = r - self.following_distance 200 | error_th = th - 0.0; 201 | if abs(error_r) >= self.dead_zone_radius: 202 | self.sum_error_r = self.Ki_factor_linear*self.Ki_linear_window*self.sum_error_r + error_r*dt 203 | follow_speed_x = self.Kp_linear * error_r + self.Ki_linear * (self.sum_error_r) 204 | follow_speed_x = self.constrain(follow_speed_x, self.linear_bound, -self.linear_bound) 205 | else: 206 | follow_speed_x = 0.0 207 | if self.debug: 208 | rospy.loginfo('Linear Dead Zone!') 209 | 210 | if abs(error_th) >= self.dead_zone_theta: 211 | self.sum_error_th = self.Ki_factor_angular*self.Ki_angular_window*self.sum_error_th + error_th*dt 212 | follow_speed_z = self.Kp_angular * (th) + self.Ki_angular * (self.sum_error_th) 213 | follow_speed_z = self.constrain(follow_speed_z, self.angular_bound, -self.angular_bound) 214 | else: 215 | follow_speed_z = 0.0 216 | if self.debug: 217 | rospy.loginfo('Angular Dead Zone!') 218 | 219 | # Pub vel cmd 220 | self.send_cmd(follow_speed_x, follow_speed_z) 221 | self.previous = current_time 222 | if self.debug: 223 | rospy.loginfo('Target ID: ' + str(self.target_id) + ', r: ' + str(r) + ', th: ' + str(th)) 224 | rospy.loginfo('PID linear: ' + str(follow_speed_x) + ', angular: ' + str(follow_speed_z)) 225 | 226 | 227 | # Main function. 228 | if __name__ == '__main__': 229 | # Initialize the node and name it. 230 | rospy.init_node("Follow_me", anonymous = False) 231 | rospy.loginfo("===== neuronbot following node (legs) =====") 232 | rospy.loginfo("Waiting for leg_tracker msg ...") 233 | # Go to class functions that do all the heavy lifting. Do error checking. 234 | try: 235 | fm = FollowMe(True) 236 | rospy.spin() 237 | except rospy.ROSInterruptException: 238 | pass 239 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright 2018 ADLINK Technology, Inc. 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | 204 | -------------------------------------------------------------------------------- /rviz/neuronbot_demo_host.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /PointCloud21/Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 554 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.588679016 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: LaserScan 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.0299999993 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Class: rviz/TF 52 | Enabled: true 53 | Frame Timeout: 15 54 | Frames: 55 | All Enabled: false 56 | astra_pro_link: 57 | Value: true 58 | base_footprint: 59 | Value: true 60 | camera_aligned_depth_to_color_frame: 61 | Value: true 62 | camera_aligned_depth_to_infra1_frame: 63 | Value: true 64 | camera_aligned_depth_to_infra2_frame: 65 | Value: true 66 | camera_color_frame: 67 | Value: true 68 | camera_color_optical_frame: 69 | Value: true 70 | camera_depth_frame: 71 | Value: true 72 | camera_depth_optical_frame: 73 | Value: true 74 | camera_infra1_frame: 75 | Value: true 76 | camera_infra1_optical_frame: 77 | Value: true 78 | camera_infra2_frame: 79 | Value: true 80 | camera_infra2_optical_frame: 81 | Value: true 82 | camera_link: 83 | Value: true 84 | laser_frame: 85 | Value: true 86 | map: 87 | Value: false 88 | odom: 89 | Value: false 90 | top_base_link: 91 | Value: true 92 | Marker Scale: 1 93 | Name: TF 94 | Show Arrows: true 95 | Show Axes: true 96 | Show Names: true 97 | Tree: 98 | map: 99 | {} 100 | odom: 101 | base_footprint: 102 | laser_frame: 103 | {} 104 | top_base_link: 105 | astra_pro_link: 106 | {} 107 | camera_link: 108 | camera_aligned_depth_to_color_frame: 109 | camera_color_optical_frame: 110 | {} 111 | camera_aligned_depth_to_infra1_frame: 112 | camera_infra1_optical_frame: 113 | {} 114 | camera_aligned_depth_to_infra2_frame: 115 | camera_infra2_optical_frame: 116 | {} 117 | camera_color_frame: 118 | {} 119 | camera_depth_frame: 120 | camera_depth_optical_frame: 121 | {} 122 | camera_infra1_frame: 123 | {} 124 | camera_infra2_frame: 125 | {} 126 | Update Interval: 0 127 | Value: true 128 | - Alpha: 0.699999988 129 | Class: rviz/Map 130 | Color Scheme: map 131 | Draw Behind: false 132 | Enabled: true 133 | Name: Map 134 | Topic: /map 135 | Unreliable: false 136 | Use Timestamp: false 137 | Value: true 138 | - Alpha: 1 139 | Autocompute Intensity Bounds: true 140 | Autocompute Value Bounds: 141 | Max Value: 10 142 | Min Value: -10 143 | Value: true 144 | Axis: Z 145 | Channel Name: intensity 146 | Class: rviz/LaserScan 147 | Color: 255; 255; 255 148 | Color Transformer: Intensity 149 | Decay Time: 0 150 | Enabled: true 151 | Invert Rainbow: false 152 | Max Color: 255; 255; 255 153 | Max Intensity: 10 154 | Min Color: 0; 0; 0 155 | Min Intensity: 10 156 | Name: LaserScan 157 | Position Transformer: XYZ 158 | Queue Size: 10 159 | Selectable: true 160 | Size (Pixels): 3 161 | Size (m): 0.0199999996 162 | Style: Flat Squares 163 | Topic: /scan 164 | Unreliable: false 165 | Use Fixed Frame: true 166 | Use rainbow: true 167 | Value: true 168 | - Alpha: 1 169 | Buffer Length: 1 170 | Class: rviz/Path 171 | Color: 25; 255; 0 172 | Enabled: true 173 | Head Diameter: 0.300000012 174 | Head Length: 0.200000003 175 | Length: 0.300000012 176 | Line Style: Lines 177 | Line Width: 0.0299999993 178 | Name: Global_Path 179 | Offset: 180 | X: 0 181 | Y: 0 182 | Z: 0 183 | Pose Color: 255; 85; 255 184 | Pose Style: None 185 | Radius: 0.0299999993 186 | Shaft Diameter: 0.100000001 187 | Shaft Length: 0.100000001 188 | Topic: /move_base/NavfnROS/plan 189 | Unreliable: false 190 | Value: true 191 | - Alpha: 1 192 | Buffer Length: 1 193 | Class: rviz/Path 194 | Color: 0; 0; 255 195 | Enabled: true 196 | Head Diameter: 0.300000012 197 | Head Length: 0.200000003 198 | Length: 0.300000012 199 | Line Style: Lines 200 | Line Width: 0.0299999993 201 | Name: trajectory 202 | Offset: 203 | X: 0 204 | Y: 0 205 | Z: 0 206 | Pose Color: 255; 85; 255 207 | Pose Style: None 208 | Radius: 0.0299999993 209 | Shaft Diameter: 0.100000001 210 | Shaft Length: 0.100000001 211 | Topic: /move_base/TrajectoryPlannerROS/local_plan 212 | Unreliable: false 213 | Value: true 214 | - Alpha: 1 215 | Class: rviz/Polygon 216 | Color: 25; 255; 0 217 | Enabled: true 218 | Name: Polygon 219 | Topic: /move_base/global_costmap/footprint 220 | Unreliable: false 221 | Value: true 222 | - Alpha: 1 223 | Arrow Length: 0.100000001 224 | Axes Length: 0.300000012 225 | Axes Radius: 0.00999999978 226 | Class: rviz/PoseArray 227 | Color: 255; 25; 0 228 | Enabled: true 229 | Head Length: 0.0700000003 230 | Head Radius: 0.0299999993 231 | Name: PoseArray 232 | Shaft Length: 0.230000004 233 | Shaft Radius: 0.00999999978 234 | Shape: Arrow (Flat) 235 | Topic: /particlecloud 236 | Unreliable: false 237 | Value: true 238 | - Alpha: 0.699999988 239 | Class: spencer_tracking_rviz_plugin/DetectedPersons 240 | Color: 221; 207; 6 241 | Color map offset: 0 242 | Color transform: Constant color 243 | Enabled: true 244 | Excluded person IDs: "" 245 | Font color: 255; 255; 255 246 | Font color style: Constant color 247 | Font scale: 0.100000001 248 | Included person IDs: "" 249 | Low-confidence alpha: 0.5 250 | Low-confidence threshold: 0.5 251 | Name: LegPerson 252 | Render confidences: false 253 | Render covariances: 254 | Line width: 0.100000001 255 | Value: false 256 | Render detection IDs: true 257 | Render modality text: false 258 | Render orientation arrows: true 259 | Style: 260 | Line width: 0.0500000007 261 | Scaling factor: 1 262 | Value: Cylinders 263 | Text spacing: 1 264 | Topic: /spencer/perception_internal/detected_persons/laser_front 265 | Unreliable: false 266 | Value: true 267 | Z offset: 268 | Use Z position from message: false 269 | Value: 0 270 | - Class: rviz/Image 271 | Enabled: true 272 | Image Topic: /upper_body_detector/image 273 | Max Value: 1 274 | Median window: 5 275 | Min Value: 0 276 | Name: Upper_body 277 | Normalize Range: true 278 | Queue Size: 2 279 | Transport Hint: raw 280 | Unreliable: false 281 | Value: true 282 | - Alpha: 1 283 | Autocompute Intensity Bounds: true 284 | Autocompute Value Bounds: 285 | Max Value: 3.65694809 286 | Min Value: 0.38937819 287 | Value: true 288 | Axis: Z 289 | Channel Name: intensity 290 | Class: rviz/PointCloud2 291 | Color: 255; 255; 255 292 | Color Transformer: RGB8 293 | Decay Time: 0 294 | Enabled: true 295 | Invert Rainbow: false 296 | Max Color: 255; 255; 255 297 | Max Intensity: 4096 298 | Min Color: 0; 0; 0 299 | Min Intensity: 0 300 | Name: PointCloud2 301 | Position Transformer: XYZ 302 | Queue Size: 10 303 | Selectable: true 304 | Size (Pixels): 3 305 | Size (m): 0.00999999978 306 | Style: Points 307 | Topic: /camera/depth/color/points 308 | Unreliable: false 309 | Use Fixed Frame: true 310 | Use rainbow: true 311 | Value: true 312 | - Alpha: 1 313 | Class: spencer_tracking_rviz_plugin/TrackedPersons 314 | Color: 130; 130; 130 315 | Color map offset: 0 316 | Color transform: Rainbow 317 | Delete after no. cycles: 100 318 | Enabled: true 319 | Excluded person IDs: "" 320 | Font color: 255; 255; 255 321 | Font color style: Same color 322 | Font scale: 1 323 | History as line: 324 | Line width: 0.0500000007 325 | Value: true 326 | History size: 100 327 | Included person IDs: "" 328 | Min. history point distance: 0.400000006 329 | Missed alpha: 0.5 330 | Name: TrackedPersons 331 | Occlusion alpha: 0.300000012 332 | Render covariances: 333 | Line width: 0.100000001 334 | Value: true 335 | Render detection IDs: true 336 | Render history: true 337 | Render person visual: true 338 | Render track IDs: true 339 | Render track state: true 340 | Render velocities: true 341 | Show DELETED tracks: false 342 | Show MATCHED tracks: true 343 | Show MISSED tracks: true 344 | Show OCCLUDED tracks: true 345 | Style: 346 | Line width: 0.0500000007 347 | Scaling factor: 1 348 | Value: Cylinders 349 | Topic: /spencer/perception/tracked_persons 350 | Unreliable: false 351 | Value: true 352 | Z offset: 353 | Use Z position from message: false 354 | Value: 0 355 | - Alpha: 1 356 | Class: spencer_tracking_rviz_plugin/TrackedGroups 357 | Color: 130; 130; 130 358 | Color map offset: 0 359 | Color transform: Rainbow 360 | Connect group members: true 361 | Enabled: true 362 | Excluded group IDs: "" 363 | Excluded person IDs: "" 364 | Font color: 255; 255; 255 365 | Font color style: Same color 366 | Font scale: 2 367 | Global history size: 1000 368 | Group ID Z offset: 2 369 | Included group IDs: "" 370 | Included person IDs: "" 371 | Name: TrackedGroups 372 | Occlusion alpha: 0.5 373 | Render group IDs: 374 | Hide IDs of single-person groups: false 375 | Value: true 376 | Render history: false 377 | Single-person groups in constant color: true 378 | Style: 379 | Line width: 0.0500000007 380 | Scaling factor: 1 381 | Value: Cylinders 382 | Topic: /spencer/perception/tracked_groups 383 | Tracked persons topic: "" 384 | Unreliable: false 385 | Value: true 386 | Z offset: 387 | Use Z position from message: false 388 | Value: 0 389 | Enabled: true 390 | Global Options: 391 | Background Color: 48; 48; 48 392 | Default Light: true 393 | Fixed Frame: map 394 | Frame Rate: 30 395 | Name: root 396 | Tools: 397 | - Class: rviz/Interact 398 | Hide Inactive Objects: true 399 | - Class: rviz/MoveCamera 400 | - Class: rviz/Select 401 | - Class: rviz/FocusCamera 402 | - Class: rviz/Measure 403 | - Class: rviz/SetInitialPose 404 | Topic: /initialpose 405 | - Class: rviz/SetGoal 406 | Topic: /move_base_simple/goal 407 | - Class: rviz/PublishPoint 408 | Single click: true 409 | Topic: /clicked_point 410 | Value: true 411 | Views: 412 | Current: 413 | Class: rviz/Orbit 414 | Distance: 13.2468853 415 | Enable Stereo Rendering: 416 | Stereo Eye Separation: 0.0599999987 417 | Stereo Focal Distance: 1 418 | Swap Stereo Eyes: false 419 | Value: false 420 | Focal Point: 421 | X: -0.395050198 422 | Y: -0.462980896 423 | Z: -0.0572278909 424 | Focal Shape Fixed Size: true 425 | Focal Shape Size: 0.0500000007 426 | Invert Z Axis: false 427 | Name: Current View 428 | Near Clip Distance: 0.00999999978 429 | Pitch: 0.524796724 430 | Target Frame: 431 | Value: Orbit (rviz) 432 | Yaw: 5.13176298 433 | Saved: ~ 434 | Window Geometry: 435 | Displays: 436 | collapsed: false 437 | Height: 1056 438 | Hide Left Dock: false 439 | Hide Right Dock: false 440 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002b9000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000001d1000000b50000000000000000fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000a0049006d00610067006501000002ab000001130000000000000000fb0000001400550070007000650072005f0062006f0064007901000002e7000000d70000001600fffffffb0000000a0049006d00610067006500000002b6000000c7000000000000000000000001000001a30000025efc0200000004fb0000000a0049006d00610067006500000000280000025e0000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000025e000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 441 | Selection: 442 | collapsed: false 443 | Time: 444 | collapsed: false 445 | Tool Properties: 446 | collapsed: false 447 | Upper_body: 448 | collapsed: false 449 | Views: 450 | collapsed: false 451 | Width: 1855 452 | X: 65 453 | Y: 24 454 | --------------------------------------------------------------------------------