Merge pull request #888 from tue-robotics/rwc2019_robot_smach_states

Rwc2019 robot smach states

challenge_demo/CMakeLists.txt

@@ -159,5 +159,6 @@ include_directories(
 #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
 # endif()
 
-## Add folders to be run by python nosetests
-catkin_add_nosetests(test)
+if (CATKIN_ENABLE_TESTING)
+  catkin_add_nosetests(test)
+endif()

challenge_demo/package.xml

@@ -4,12 +4,10 @@
   <version>0.0.0</version>
   <description>The demo package</description>
 
-  <maintainer email="amigo@todo.todo">Matthijs van der Burghs</maintainer>
-
+  <maintainer email="MatthijsBurgh@outlook.com">Matthijs van der Burgh</maintainer>
 
   <license>BSD</license>
 
-
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>robot_skills</build_depend>
@@ -20,4 +18,7 @@
   <run_depend>robot_smach_states</run_depend>
   <run_depend>rospy</run_depend>
 
+  <test_depend>action_server</test_depend>
+  <test_depend>robocup_knowledge</test_depend>
+
 </package>

challenge_rips/src/rips.py

@@ -53,7 +53,7 @@ def setup_statemachine(robot):
                                             'goal_not_defined': 'ASK_CONTINUE'})
 
         smach.StateMachine.add("ASK_CONTINUE",
-                               states.AskContinue(robot, rospy.Duration(30)),
+                               states.AskContinue(robot, 30),
                                transitions={'continue': 'SAY_CONTINUEING',
                                             'no_response': 'SAY_CONTINUEING'})
 

challenge_spr/src/challenge_spr.py

@@ -2,18 +2,20 @@
 
 import rospy
 import smach
-import time
-import os
-import datetime
 import math
 
-from robot_smach_states import Initialize, Say, WaitForPersonInFront, Turn, WaitTime
+from robot_smach_states.human_interaction.answer_questions import HearAndAnswerQuestions
+from robot_smach_states import Initialize, Say, WaitForPersonInFront, ForceDrive, WaitTime
 from robot_smach_states.util.startup import startup
 import robot_smach_states.util.designators as ds
 
 from challenge_spr_states import detect
 from challenge_spr_states import bluff_game
-from challenge_spr_states import riddle_game
+
+from robocup_knowledge import load_knowledge
+knowledge = load_knowledge('challenge_spr')
+common_knowledge = load_knowledge('common')
+
 
 class ChallengeSpeechPersonRecognition(smach.StateMachine):
     def __init__(self, robot):
@@ -35,7 +37,7 @@ def __init__(self, robot):
                                                 "preempted": "TURN"})
 
             smach.StateMachine.add("TURN",
-                                   Turn(robot, math.pi),
+                                   ForceDrive(robot, vx=0.0, vy=0.0, vth=1.0, duration=math.pi),
                                    transitions={"turned": "DETECT_CROWD"})
 
             smach.StateMachine.add("DETECT_CROWD",
@@ -56,7 +58,12 @@ def __init__(self, robot):
             # Riddle Game
 
             smach.StateMachine.add('RIDDLE_GAME',
-                                   riddle_game.HearAndAnswerQuestions(robot, num_questions=5),
+                                   HearAndAnswerQuestions(
+                                       robot,
+                                       grammar=knowledge.grammar,
+                                       knowledge=common_knowledge,
+                                       num_questions=5,
+                                   ),
                                    transitions={'done':'TRANSITION'})
 
             # Transition:
@@ -78,6 +85,7 @@ def __init__(self, robot):
 
             ds.analyse_designators(self, "person_recognition")
 
+
 if __name__ == "__main__":
     rospy.init_node('speech_person_recognition_exec')
 

robot_skills/src/robot_skills/__init__.py

@@ -1,8 +1,8 @@
 # Robots
 from .amigo import Amigo
 from .hero import Hero
-# from .mockbot import Mockbot  # ToDo: enable after merging Loys MockBot update
+from .mockbot import Mockbot
 from .sergio import Sergio
 
 # Helper methods
-from get_robot import get_robot, get_robot_from_argv
+from .get_robot import get_robot, get_robot_from_argv

robot_skills/src/robot_skills/base.py

@@ -198,17 +198,19 @@ def move(self, position_constraint_string, frame):
 
         return plan
 
-    def force_drive(self, vx, vy, vth, timeout, ax=float('inf'), ay=float('inf'), ath=float('inf')):
+    def force_drive(self, vx, vy, vth, timeout, loop_rate=10, stop=True, ax=float('inf'), ay=float('inf'), ath=float('inf')):
         """ Forces the robot to drive by sending a command velocity directly to the base controller. N.B.: all collision
         avoidance is bypassed.
 
-        :param vx: forward velocity in m/s
-        :param vy: sideways velocity in m/s
-        :param vth: rotational velocity in rad/s
-        :param ax: forward acceleration in m/s^2
-        :param ay: sideways acceleration in m/s^2
-        :param ath: rotational acceleration in rad/s^2
-        :param timeout: duration for this motion in seconds
+        :param vx: forward velocity [m/s]
+        :param vy: sideways velocity [m/s]
+        :param vth: rotational velocity in [rad/s]
+        :param timeout: duration for this motion [s]
+        :param loop_rate: Rate of sending twist messages [Hz]
+        :param stop: send stop message afterwards
+        :param ax: forward acceleration [m/s^2]
+        :param ay: sideways acceleration [m/s^2]
+        :param ath: rotational acceleration [rad/s^2]
         """
         # Cancel the local planner goal
         self.local_planner.cancelCurrentPlan()
@@ -221,20 +223,24 @@ def _abs_max(value, max_value):
             return sign(value) * min(abs(max_value), abs(value))
 
         # Drive
+        rate = rospy.Rate(loop_rate)
         while rospy.Time.now() < t_end:
             seconds_from_start = rospy.Time.now().to_sec() - t_start.to_sec()
 
-            v.linear.x = _abs_max(vx, ax * seconds_from_start)
-            v.linear.y = _abs_max(vy, ay * seconds_from_start)
-            v.angular.z = _abs_max(vth, ath * seconds_from_start)
+            # 0 * inf =  NaN, so in case 'seconds_from_start' is close to zero, aka in the first loop iteration,
+            # use zero to prevent NaNs
+            v.linear.x = _abs_max(vx, ax * seconds_from_start if seconds_from_start > 1/loop_rate else 0)
+            v.linear.y = _abs_max(vy, ay * seconds_from_start if seconds_from_start > 1/loop_rate else 0)
+            v.angular.z = _abs_max(vth, ath * seconds_from_start if seconds_from_start > 1/loop_rate else 0)
             self._cmd_vel.publish(v)
-            rospy.sleep(0.1)
+            rate.sleep()
 
-        # Stop driving
-        v.linear.x = 0.0
-        v.linear.y = 0.0
-        v.angular.z = 0.0
-        self._cmd_vel.publish(v)
+        if stop:
+            # Stop driving
+            v.linear.x = 0.0
+            v.linear.y = 0.0
+            v.angular.z = 0.0
+            self._cmd_vel.publish(v)
 
         return True
 

robot_skills/src/robot_skills/force_sensor.py

@@ -2,7 +2,7 @@
 from numpy.linalg import norm as np_norm
 import rospy
 from geometry_msgs.msg import WrenchStamped
-from .exceptions import TimeOutException
+from util.exceptions import TimeOutException
 
 # TU/e Robotics
 from robot_skills.robot_part import RobotPart