Remove custom message dependencies

CMakeLists.txt

@@ -13,7 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
   geometry_msgs
   message_filters
   message_generation
-  egn_messages
+  vehicle_msgs
 )
 
 ## System dependencies are found with CMake's conventions
@@ -71,6 +71,6 @@ install(DIRECTORY
 install(PROGRAMS
     src/main.py
     src/MaRRTPathPlanNode.py
-    src/inc/ma_rrt.py
+    src/ma_rrt.py
     DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )

README.md

@@ -17,10 +17,13 @@ A brief introduction to the main steps of the proposed algorithm is given in my
 - Exploration of [FSG'17 trackdrive circuit](https://www.youtube.com/watch?v=jJAjrCig3yE) in Gazebo.
 - *Your video (feel free to pull-request a link with it here).*
 
+#### Vehicle Messages
+Some custom messages are included inside the `ma_rrt_path_plan` package folder. Because the Git repository was setup before the creation of the messages (via a PR from [ekampourakis](https://github.com/ekampourakis/)), the `vehicle_msgs` package folder needs to be moved to the parent folder of the `ma_rrt_path_plan` in order to act as a seperate package and possibly included in the Makefile too.
+
 ## Inputs, outputs, params
 
 #### Inputs
-- rospy.Subscriber("/map", Map, ...)
+- rospy.Subscriber("/map", Track, ...)
 - rospy.Subscriber("/odometry", Odometry, ...)
 
 #### Outputs
@@ -34,6 +37,8 @@ A brief introduction to the main steps of the proposed algorithm is given in my
 - rospy.Publisher("/visual/waypoints", MarkerArray, ...)
 
 #### Parameters to tune (main)
+- `odom_topic` = `/odometry` - The topic to get the odometry information from
+- `world_frame` = `world` - The world frame to use for the visualizations
 - planDistance = 12 m - Maximum length of tree branch
 - expandDistance = 1 m - Length of tree node/step
 - expandAngle = 20 deg - constraining angle for next tree nodes

launch/startExploring.launch

@@ -3,6 +3,10 @@
     <node pkg="ma_rrt_path_plan" type="main.py" name="ma_rrt_path_plan_node" output="screen">
         <param name="desiredWaypointsFrequency" value="5" type="int" />
 
+        <!-- TODO: Comment me out -->
+        <!-- <param name="odom_topic" value="/odometry/real"/>
+        <param name="world_frame" value="map"/> -->
+
         <param name="publishWaypoints" type="bool" value="true" />
 
     </node>

package.xml

@@ -12,7 +12,7 @@
 
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
-  <build_depend>egn_messages</build_depend>
+  <build_depend>vehicle_msgs</build_depend>
 
   <build_export_depend>rospy</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>

src/MaRRTPathPlanNode.py

@@ -10,10 +10,7 @@
 
 import ma_rrt
 
-from egn_messages.msg import Map
-from egn_messages.msg import CarSensors
-from egn_messages.msg import WaypointsArray
-from egn_messages.msg import Waypoint
+from vehicle_msgs.msg import TrackCone, Track, Command, Waypoint, WaypointsArray
 
 from visualization_msgs.msg import Marker
 from visualization_msgs.msg import MarkerArray
@@ -45,14 +42,24 @@ def __init__(self):
         if rospy.has_param('~filename'):
             self.filename = rospy.get_param('~filename')
 
+        if rospy.has_param('~odom_topic'):
+            self.odometry_topic = rospy.get_param('~odom_topic')
+        else:
+            self.odometry_topic = "/odometry"
+
+        if rospy.has_param('~world_frame'):
+            self.world_frame = rospy.get_param('~world_frame')
+        else:
+            self.world_frame = "world"
+
         waypointsFrequency = rospy.get_param('~desiredWaypointsFrequency', 5)
         self.waypointsPublishInterval = 1.0 / waypointsFrequency
         self.lastPublishWaypointsTime = 0
 
         # All Subs and pubs
-        rospy.Subscriber("/map", Map, self.mapCallback)
-        rospy.Subscriber("/odometry", Odometry, self.odometryCallback)
-        # rospy.Subscriber("/car_sensors", CarSensors, self.carSensorsCallback)
+        rospy.Subscriber("/track", Track, self.mapCallback)
+        rospy.Subscriber(self.odometry_topic, Odometry, self.odometryCallback)
+        rospy.Subscriber("/dvsim/cmd", Command, self.carSensorsCallback)
 
         # Create publishers
         self.waypointsPub = rospy.Publisher("/waypoints", WaypointsArray, queue_size=0)
@@ -89,7 +96,7 @@ def __init__(self):
         self.filteredBestBranch = []
         self.discardAmount = 0
 
-        # print("MaRRTPathPlanNode Constructor has been called")
+        print("MaRRTPathPlanNode Constructor has been called")
 
     def __del__(self):
         print('MaRRTPathPlanNode: Destructor called.')
@@ -107,20 +114,20 @@ def odometryCallback(self, odometry):
         self.carPosYaw = yaw
         #print "Estimated processing odometry callback: {0} ms".format((time.time() - start)*1000)
 
-    def carSensorsCallback(self, carSensors):
+    def carSensorsCallback(self, command):
         # rospy.loginfo("carSensorsCallback")
 
         # start = time.time()
-        self.steerAngle = carSensors.steerAngle
+        # The ackermann angle [rad]
+        self.steerAngle = math.degrees((command.theta_l + command.theta_r) / 2.0)
 
         #print "Estimated processing map callback: {0} ms".format((time.time() - start)*1000);
 
-    def mapCallback(self, map):
-        self.map = map
+    def mapCallback(self, track):
+        self.map = track.cones
 
     def sampleTree(self):
         # sampleTreeStartTime = time.time()
-
         if self.loopClosure and len(self.savedWaypoints) > 0:
             # print("Publish savedWaypoints/predifined waypoints, return")
             self.publishWaypoints()
@@ -222,7 +229,7 @@ def sampleTree(self):
     def mergeWaypoints(self, newWaypoints):
         # print "mergeWaypoints:", "len(saved):", len(self.savedWaypoints), "len(new):", len(newWaypoints)
         if not newWaypoints:
-            return;
+            return
 
         maxDistToSaveWaypoints = 2.0
         maxWaypointAmountToSave = 2
@@ -371,7 +378,7 @@ def publishWaypoints(self, newWaypoints = None):
 
         # print "publishWaypoints(): start"
         waypointsArray = WaypointsArray()
-        waypointsArray.header.frame_id = "world"
+        waypointsArray.header.frame_id = self.world_frame
         waypointsArray.header.stamp = rospy.Time.now()
 
         waypointsArray.preliminaryLoopClosure = self.preliminaryLoopClosure
@@ -417,7 +424,7 @@ def publishWaypointsVisuals(self, newWaypoints = None):
         markerArray = MarkerArray()
 
         savedWaypointsMarker = Marker()
-        savedWaypointsMarker.header.frame_id = "world"
+        savedWaypointsMarker.header.frame_id = self.world_frame
         savedWaypointsMarker.header.stamp = rospy.Time.now()
         savedWaypointsMarker.lifetime = rospy.Duration(1)
         savedWaypointsMarker.ns = "saved-publishWaypointsVisuals"
@@ -441,7 +448,7 @@ def publishWaypointsVisuals(self, newWaypoints = None):
 
         if newWaypoints is not None:
             newWaypointsMarker = Marker()
-            newWaypointsMarker.header.frame_id = "world"
+            newWaypointsMarker.header.frame_id = self.world_frame
             newWaypointsMarker.header.stamp = rospy.Time.now()
             newWaypointsMarker.lifetime = rospy.Duration(1)
             newWaypointsMarker.ns = "new-publishWaypointsVisuals"
@@ -544,7 +551,7 @@ def publishDelaunayEdgesVisual(self, edges):
             return
 
         marker = Marker()
-        marker.header.frame_id = "world"
+        marker.header.frame_id = self.world_frame
         marker.header.stamp = rospy.Time.now()
         marker.lifetime = rospy.Duration(1)
         marker.ns = "publishDelaunayLinesVisual"
@@ -663,7 +670,7 @@ def isLeftCone(self, node, parentNode, cone):
 
     def publishBestBranchVisual(self, nodeList, leafNode):
         marker = Marker()
-        marker.header.frame_id = "world"
+        marker.header.frame_id = self.world_frame
         marker.header.stamp = rospy.Time.now()
         marker.lifetime = rospy.Duration(0.2)
         marker.ns = "publishBestBranchVisual"
@@ -699,7 +706,7 @@ def publishFilteredBranchVisual(self):
             return
 
         marker = Marker()
-        marker.header.frame_id = "world"
+        marker.header.frame_id = self.world_frame
         marker.header.stamp = rospy.Time.now()
         marker.lifetime = rospy.Duration(0.2)
         marker.ns = "publisshFilteredBranchVisual"
@@ -733,7 +740,7 @@ def publishTreeVisual(self, nodeList, leafNodes):
 
         # tree lines marker
         treeMarker = Marker()
-        treeMarker.header.frame_id = "world"
+        treeMarker.header.frame_id = self.world_frame
         treeMarker.header.stamp = rospy.Time.now()
         treeMarker.ns = "rrt"
 
@@ -760,7 +767,7 @@ def publishTreeVisual(self, nodeList, leafNodes):
 
         # leaves nodes marker
         leavesMarker = Marker()
-        leavesMarker.header.frame_id = "world"
+        leavesMarker.header.frame_id = self.world_frame
         leavesMarker.header.stamp = rospy.Time.now()
         leavesMarker.lifetime = rospy.Duration(0.2)
         leavesMarker.ns = "rrt-leaves"
@@ -803,7 +810,7 @@ def getFrontConeObstacles(self, map, frontDist):
         frontDistSq = frontDist ** 2
 
         frontConeList = []
-        for cone in map.cones:
+        for cone in map:
             if (headingVectorOrt[0] * (cone.y - carPosBehindPoint[1]) - headingVectorOrt[1] * (cone.x - carPosBehindPoint[0])) < 0:
                 if ((cone.x - self.carPosX) ** 2 + (cone.y - self.carPosY) ** 2) < frontDistSq:
                     frontConeList.append(cone)
@@ -818,7 +825,7 @@ def getHeadingVector(self):
     def getConesInRadius(self, map, x, y, radius):
         coneList = []
         radiusSq = radius * radius
-        for cone in map.cones:
+        for cone in map:
             if ((cone.x - x) ** 2 + (cone.y - y) ** 2) < radiusSq:
                 coneList.append(cone)
         return coneList

vehicle_msgs/CMakeLists.txt

@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 2.8.12)
+project(vehicle_msgs)
+
+find_package(catkin REQUIRED COMPONENTS
+  cmake_modules
+  geometry_msgs
+  message_generation
+  std_msgs
+)
+
+add_message_files(
+  FILES
+  Command.msg
+  TrackCone.msg
+  Track.msg
+  Waypoint.msg
+  WaypointsArray.msg
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  geometry_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS message_runtime std_msgs
+)

vehicle_msgs/msg/Command.msg

@@ -0,0 +1,6 @@
+float64 theta_l
+float64 theta_r
+float64 torque_fl
+float64 torque_fr
+float64 torque_rl
+float64 torque_rr

vehicle_msgs/msg/Track.msg

@@ -0,0 +1 @@
+vehicle_msgs/TrackCone[] cones

vehicle_msgs/msg/TrackCone.msg

@@ -0,0 +1,4 @@
+float64 x
+float64 y
+
+string type

vehicle_msgs/msg/Waypoint.msg

@@ -0,0 +1,4 @@
+float64 x
+float64 y
+
+float64 id

vehicle_msgs/msg/WaypointsArray.msg

@@ -0,0 +1,6 @@
+Header header
+
+vehicle_msgs/Waypoint[] waypoints
+
+bool preliminaryLoopClosure
+bool loopClosure

vehicle_msgs/package.xml

@@ -0,0 +1,14 @@
+<package format="2">
+    <name>vehicle_msgs</name>
+    <version>0.0.1</version>
+    <description>Messages for controlling the vehicle</description>
+    <maintainer email="[email protected]">Emmanouil Kampourakis</maintainer>
+    <license>TODO</license>
+
+    <buildtool_depend>catkin</buildtool_depend>
+
+    <build_depend>message_generation</build_depend>
+    <exec_depend>message_runtime</exec_depend>
+
+    <depend>std_msgs</depend>
+</package>