Merge pull request #69 from JdeRobot/issue-67

Support for dockerized execution environments

backend/filesystem/demo/actions/CheckObstacle.py → backend/filesystem/laser_bump_and_go/actions/CheckObstacle.py

@@ -1,6 +1,6 @@
 import py_trees
 import sensor_msgs
-from tree_gardener import tree_tools
+import tree_tools
 
 def check_obstacle_in_laser(laser_measures, amplitude):
 
@@ -44,13 +44,10 @@ def setup(self, **kwargs: int) -> None:
             self.listener_callback,
             10)
 
-        self.scan = sensor_msgs.msg.LaserScan()
-
-        # Init the obstacle counter
-        self.n_obs = 0
+        self.last_scan_ = sensor_msgs.msg.LaserScan()
 
     def listener_callback(self, msg) -> None:
-        self.scan = msg
+        self.last_scan_ = msg
 
     def initialise(self) -> None:
 
@@ -64,16 +61,16 @@ def update(self) -> py_trees.common.Status:
         """ Executed when the action is ticked. Do not block! """
 
         # Check the laser measures
-        if len(self.scan.ranges) == 0: new_status = py_trees.common.Status.INVALID
+        if len(self.last_scan_.ranges) == 0: new_status = py_trees.common.Status.INVALID
 
-        # Check if there is an obstacle
+        # Get params from ports
         amplitude = int(tree_tools.get_port_content(self.ports["amplitude"]))
-        obstacle = check_obstacle_in_laser(self.scan.ranges, amplitude)
-        if not obstacle: new_status = py_trees.common.Status.SUCCESS
-        else: 
-            self.n_obs += 1
-            tree_tools.set_port_content(self.ports["obs_port"], self.n_obs)
-            new_status = py_trees.common.Status.FAILURE
+        obs_dist = float(tree_tools.get_port_content(self.ports["obs_dist"]))
+
+        # Check if there is an obstacle
+        obstacle = check_obstacle_in_laser(self.last_scan_.ranges, amplitude)
+        if obstacle: new_status = py_trees.common.Status.SUCCESS
+        else: new_status = py_trees.common.Status.FAILURE
 
         return new_status
 
@@ -82,4 +79,4 @@ def terminate(self, new_status: py_trees.common.Status) -> None:
         """ Called whenever the behaviour switches to a non-running state """
 
         # Debugging
-        self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+        self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status))

backend/filesystem/demo/actions/Forward.py → backend/filesystem/laser_bump_and_go/actions/Forward.py

@@ -1,10 +1,6 @@
 import py_trees
 import geometry_msgs
-import std_msgs
-from tree_gardener import tree_tools
-from cv_bridge import CvBridge
-import sensor_msgs
-import cv2
+import tree_tools
 
 class Forward(py_trees.behaviour.Behaviour):
 
@@ -37,29 +33,6 @@ def setup(self, **kwargs) -> None:
             qos_profile=10
         )
 
-        # Setup the publisher for n_obs
-        self.publisher2 = self.node.create_publisher(
-            msg_type=std_msgs.msg.String,
-            topic="/n_obs",
-            qos_profile=10
-        )
-
-        # Setup the subscription to camera
-        self.subscription = self.node.create_subscription(
-            sensor_msgs.msg.Image,
-            '/camera/image_raw',
-            self.listener_callback,
-            10
-        )
-
-        self.bridge = CvBridge()
-        self.img_received = False
-
-    def listener_callback(self, msg):
-
-        self.last_img = self.bridge.imgmsg_to_cv2(msg)
-        self.img_received = True
-
     def initialise(self) -> None:
 
         """ Executed when coming from an idle state """
@@ -76,16 +49,6 @@ def update(self) -> py_trees.common.Status:
         msg.linear.x = float(tree_tools.get_port_content(self.ports["speed"]))
         self.publisher.publish(msg)
 
-        # Publish the number of obstacles retrieved from the port
-        nobs = tree_tools.get_port_content(self.ports["obs_port"])
-        str_pub = std_msgs.msg.String()
-        str_pub.data = str(nobs)
-        self.publisher2.publish(str_pub)
-
-        if self.img_received:
-            cv2.imshow("Robot img", self.last_img)
-            cv2.waitKey(1)
-
         return py_trees.common.Status.RUNNING 
 
     def terminate(self, new_status: py_trees.common.Status) -> None:

backend/filesystem/demo/actions/Turn.py → backend/filesystem/laser_bump_and_go/actions/Turn.py

@@ -53,6 +53,11 @@ def update(self) -> py_trees.common.Status:
     def terminate(self, new_status: py_trees.common.Status) -> None:
 
         """ Called whenever the behaviour switches to a non-running state """
+
+        # Stop the robot
+        msg = geometry_msgs.msg.Twist()
+        msg.linear.x = 0.0
+        self.publisher.publish(msg)
 
         # Debugging
-        self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+        self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status))