add velocity publisher (via Antareep)

CMakeLists.txt

@@ -29,6 +29,8 @@ add_message_files(
   PersonArray.msg
   Leg.msg
   LegArray.msg
+  PersonVelocity.msg
+  PeopleVelocity.msg
 )
 
 generate_messages(   
@@ -39,7 +41,7 @@ generate_messages(
 
 
 catkin_package(
-  CATKIN_DEPENDS message_runtime
+  CATKIN_DEPENDS message_runtime std_msgs
 )
 
 
@@ -119,9 +121,3 @@ install(
   PROGRAMS scripts/individual_leg_tracker.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
-
-
-
-
-
-

launch/joint_leg_tracker.launch

@@ -3,8 +3,11 @@
   <!-- params -->
   <param name="forest_file" value="$(find leg_tracker)/config/trained_leg_detector_res=0.33.yaml" />
   <param name="scan_topic" value="/scan" />
-  <param name="fixed_frame" value="/base_link"/>
+  <param name="fixed_frame" value="base_link"/>
   <param name="scan_frequency" value="10"/>  
+  <param name="max_detected_clusters" value="5" type="int"/>
+  <param name="publish_people_frame" value="map"/>
+
 
 
   <!-- run detect_leg_clusters -->  

msg/PeopleVelocity.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+PersonVelocity[] people

msg/PersonVelocity.msg

@@ -0,0 +1,4 @@
+int32 id
+geometry_msgs/Pose pose
+float64 velocity_x
+float64 velocity_y

rosbags/demos/rviz/demo_stationary_simple_environment.rviz

@@ -1,6 +1,6 @@
 Panels:
   - Class: rviz/Displays
-    Help Height: 78
+    Help Height: 85
     Name: Displays
     Property Tree Widget:
       Expanded:
@@ -10,7 +10,7 @@ Panels:
         - /Map1
         - /Axes1
       Splitter Ratio: 0.5
-    Tree Height: 595
+    Tree Height: 545
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -19,17 +19,20 @@ Panels:
       - /2D Nav Goal1
       - /Publish Point1
     Name: Tool Properties
-    Splitter Ratio: 0.588679
+    Splitter Ratio: 0.5886790156364441
   - Class: rviz/Views
     Expanded:
       - /Current View1
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
-    Experimental: false
     Name: Time
     SyncMode: 0
     SyncSource: LaserScan
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
 Visualization Manager:
   Class: ""
   Displays:
@@ -39,7 +42,7 @@ Visualization Manager:
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.03
+        Line Width: 0.029999999329447746
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -66,17 +69,16 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 4096
       Min Color: 0; 0; 0
-      Min Intensity: 0
       Name: LaserScan
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.06
+      Size (m): 0.05999999865889549
       Style: Flat Squares
       Topic: /scan
+      Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
@@ -86,28 +88,41 @@ Visualization Manager:
       Name: Marker
       Namespaces:
         LEGS: true
-        People_tracked: true
         objects_tracked: true
       Queue Size: 100
       Value: true
-    - Alpha: 0.7
+    - Alpha: 0.699999988079071
       Class: rviz/Map
       Color Scheme: map
       Draw Behind: false
       Enabled: true
       Name: Map
       Topic: /local_map
+      Unreliable: false
+      Use Timestamp: false
       Value: true
-    - Class: rviz/Axes
+    - Alpha: 1
+      Class: rviz/Axes
       Enabled: true
-      Length: 0.3
+      Length: 0.30000001192092896
       Name: Axes
-      Radius: 0.1
+      Radius: 0.10000000149011612
       Reference Frame: <Fixed Frame>
+      Show Trail: false
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /visualization_marker
+      Name: Marker
+      Namespaces:
+        LEGS: true
+        objects_tracked: true
+      Queue Size: 100
       Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
+    Default Light: true
     Fixed Frame: laser
     Frame Rate: 30
   Name: root
@@ -119,7 +134,10 @@ Visualization Manager:
     - Class: rviz/FocusCamera
     - Class: rviz/Measure
     - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
       Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
     - Class: rviz/SetGoal
       Topic: /move_base_simple/goal
     - Class: rviz/PublishPoint
@@ -129,30 +147,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 10.6941
+      Distance: 10.694100379943848
       Enable Stereo Rendering:
-        Stereo Eye Separation: 0.06
+        Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
+      Field of View: 0.7853981852531433
       Focal Point:
-        X: 0.692725
-        Y: 0.277637
-        Z: 0.563887
+        X: 0.6927250027656555
+        Y: 0.2776370048522949
+        Z: 0.5638870000839233
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.01
-      Pitch: 0.720399
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.720399022102356
       Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 3.0954
+      Yaw: 3.095400094985962
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
   Height: 876
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000013c000002e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002e2000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002e2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000003efc0100000002fb0000000800540069006d00650100000000000005ff000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003a8000002e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000161000002bdfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000047000002bd000000f100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002bdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000047000002bd000000c200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff00000040fc0100000002fb0000000800540069006d00650100000000000005ff0000045a00fffffffb0000000800540069006d0065010000000000000450000000000000000000000381000002bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -162,5 +183,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1535
-  X: 65
-  Y: 24
+  X: 72
+  Y: 34

src/velocity_pub.py

@@ -0,0 +1,65 @@
+#!/usr/bin/env python3
+import rospy
+from leg_tracker.msg import PersonArray, PersonVelocity, PeopleVelocity
+from time import perf_counter
+
+previous_data = {}
+average_time = []
+
+
+def calculate_velocity(current_pos, previous_pos, dt):
+    dx = current_pos[0] - previous_pos[0]
+    dy = current_pos[1] - previous_pos[1]
+    velocity_x = dx / dt
+    velocity_y = dy / dt
+    return velocity_x, velocity_y
+
+
+def people_callback(msg: PersonArray, velocity_publisher):
+    current_time = perf_counter()
+    people_list = []
+    for person in msg.people:
+        id = person.id
+        coord_x = person.pose.position.x
+        coord_y = person.pose.position.y
+
+        if id in previous_data:
+            prev_coord_x, prev_coord_y, prev_time = previous_data[id]
+            dt = current_time - prev_time
+
+            if dt > 0:
+                average_time.append(dt)
+                velocity_x, velocity_y = calculate_velocity(
+                    (coord_x, coord_y), (prev_coord_x, prev_coord_y), dt
+                )
+
+                vel_msg = PersonVelocity()
+                vel_msg.id = id
+                vel_msg.pose.position.x = coord_x
+                vel_msg.pose.position.y = coord_y
+                vel_msg.velocity_x = velocity_x
+                vel_msg.velocity_y = velocity_y
+                people_list.append(vel_msg)
+        previous_data[id] = (coord_x, coord_y, current_time)
+        if len(average_time)>=100:
+            print("Average Time: ", sum(average_time)/len(average_time))
+            average_time.clear()
+    people_msg = PeopleVelocity()
+    people_msg.people = people_list
+    velocity_publisher.publish(people_msg)
+
+
+def main():
+    rospy.init_node("people_velocity_node")
+    velocity_publisher = rospy.Publisher("vel_pub", PeopleVelocity, queue_size=10)
+    rospy.Subscriber(
+        "/people_tracked",
+        PersonArray,
+        people_callback,
+        callback_args=velocity_publisher,
+    )
+    rospy.spin()
+
+
+if __name__ == "__main__":
+    main()