Added voice control and joint limits

2022-04-07 16:33:58 -04:00 · 2022-04-07 16:33:58 -04:00 · 44378f5455
commit 44378f5455
parent a0ee92d53a
27 changed files with 136458 additions and 7 deletions
--- a/src/ar3_config/launch/dev.launch.py
+++ b/src/ar3_config/launch/dev.launch.py
@ -113,6 +113,15 @@ def generate_launch_description():
            shell=True,
            output='screen'),
        Node(
                package='voice_move',
                executable='voice_listener',
                output='screen'),
        Node(
                package='high_lvl_cmd_srv',
                executable='high_lvl_cmd_srv',
                output='screen'),
        # Run platform for interface between teensy
        Node(
            package='cobot_platform',
--- a/src/ar3_description/urdf/ar3.urdf
+++ b/src/ar3_description/urdf/ar3.urdf
@ -82,7 +82,7 @@
  </link>
  <joint
    name="joint_1"
-    type="continuous">
+    type="revolute">
    <origin
      xyz="0 0 0.003445"
      rpy="3.1416 -1.0248E-21 -1.6736E-05" />
@ -92,6 +92,7 @@
      link="link_1" />
    <axis
      xyz="0 0 1" />
    <limit lower="-2.96706" upper="2.96706" effort="0" velocity="0"/>
  </joint>
  <link
    name="link_2">
@ -135,7 +136,7 @@
  </link>
  <joint
    name="joint_2"
-    type="continuous">
+    type="revolute">
    <origin
      xyz="0 0.064146 -0.16608"
      rpy="1.5708 0.5236 -1.5708" />
@ -145,6 +146,7 @@
      link="link_2" />
    <axis
      xyz="0 0 -1" />
    <limit lower="-0.69115024" upper="1.570796" effort="0" velocity="0"/>
  </joint>
  <link
    name="link_3">
@ -188,16 +190,17 @@
  </link>
  <joint
    name="joint_3"
-    type="continuous">
+    type="revolute">
    <origin
      xyz="0.1525 -0.26414 0"
-      rpy="3.3431E-16 -1.4164E-16 -1.4953816339" />
+      rpy="3.3431E-16 -1.4164E-16 -1.047197" />
    <parent
      link="link_2" />
    <child
      link="link_3" />
    <axis
      xyz="0 0 -1" />
    <limit lower="0" upper="2.5080381" effort="0" velocity="0"/>
  </joint>
  <link
    name="link_4">
@ -241,7 +244,7 @@
  </link>
  <joint
    name="joint_4"
-    type="continuous">
+    type="revolute">
    <origin
      xyz="0 0 0.00675"
      rpy="1.5708 -1.2554 -1.5708" />
@ -251,6 +254,7 @@
      link="link_4" />
    <axis
      xyz="0 0 -1" />
    <limit lower="-2.8710666" upper="2.8710666" effort="0" velocity="0"/>
  </joint>
  <link
    name="link_5">
@ -294,7 +298,7 @@
  </link>
  <joint
    name="joint_5"
-    type="continuous">
+    type="revolute">
    <origin
      xyz="0 0 -0.22225"
      rpy="3.1416 0 -2.8262" />
@ -304,6 +308,7 @@
      link="link_5" />
    <axis
      xyz="-1 0 0" />
    <limit lower="-1.81776042" upper="1.81776042" effort="0" velocity="0"/>
  </joint>
  <link
    name="link_6">
@ -347,7 +352,7 @@
  </link>
  <joint
    name="joint_6"
-    type="continuous">
+    type="revolute">
    <origin
      xyz="-0.000294 0 0.02117"
      rpy="0 0 3.1416" />
@ -357,6 +362,7 @@
      link="link_6" />
    <axis
      xyz="0 0 1" />
    <limit lower="-2.5848326" upper="2.5848326" effort="0" velocity="0"/>
  </joint>
--- a/src/defs/CMakeLists.txt
+++ b/src/defs/CMakeLists.txt
@ -0,0 +1,40 @@
 cmake_minimum_required(VERSION 3.5)
 project(defs)
 # Default to C99
 if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
 endif()
 # Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
 endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/HighLvlCmds.srv"
 )
 if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
 endif()
 ament_package()
--- a/src/defs/package.xml
+++ b/src/defs/package.xml
@ -0,0 +1,22 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
  <name>defs</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="dw268145@gmail.com">daniel</maintainer>
  <license>TODO: License declaration</license>
  <buildtool_depend>ament_cmake</buildtool_depend>
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>
  <build_depend>rosidl_default_generators</build_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>
  <member_of_group>rosidl_interface_packages</member_of_group>
  <export>
    <build_type>ament_cmake</build_type>
  </export>
 </package>
--- a/src/defs/srv/HighLvlCmds.srv
+++ b/src/defs/srv/HighLvlCmds.srv
@ -0,0 +1,7 @@
 int8 command
 int8 item
 int8 direction
 int8 position
 int16 degrees
 ---
 bool success
--- a/src/high_lvl_cmd_srv/high_lvl_cmd_srv/init.py
+++ b/src/high_lvl_cmd_srv/high_lvl_cmd_srv/init.py
--- a/src/high_lvl_cmd_srv/high_lvl_cmd_srv/cmd_constants.py
+++ b/src/high_lvl_cmd_srv/high_lvl_cmd_srv/cmd_constants.py
@ -0,0 +1,27 @@
 from enum import Enum
 class Cmd(Enum):
    NONE        = -1
    IGNORE      = 0     # Do not try to execute a command
    OPEN        = 1     # Open the claw
    CLOSE       = 2     # Close the claw
    MOVE        = 3     # Move to a stated position in space
    TURN        = 4     # Turn a stated degree amount and direction
    PICKUP      = 5     # Pick up a stated item
    SETDOWN     = 6     # Set down the currently held item
    GIVE        = 7     # Give the operator a stated item
 class Item(Enum):
    NONE        = -1
    CAN         = 0
    BLOCK       = 1
    BALL        = 2
    UNKNOWN     = 3
 class Direction(Enum):
    NONE        = -1
    LEFT        = 0
    RIGHT       = 1
    UP          = 2
    DOWN        = 3
--- a/src/high_lvl_cmd_srv/high_lvl_cmd_srv/high_lvl_cmd_srv.py
+++ b/src/high_lvl_cmd_srv/high_lvl_cmd_srv/high_lvl_cmd_srv.py
@ -0,0 +1,353 @@
 import rclpy
 from rclpy.node import Node
 from std_msgs.msg import Header
 from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
 from defs.srv import HighLvlCmds
 from high_lvl_cmd_srv.cmd_constants import *
 import transforms3d
 # To add a new voice command, item, etc.:
 # 1. Add to Enum in cmd_constants.py
 # 2. Add words to word list in voice_listener.py
 # 3. Add function to HighLvlCmdSrv
 # 4. Add that function to to HighLvlCmdSrv.CmdFunc
 class VoiceCommandException(Exception):
    pass
 class OpenError(VoiceCommandException):
    pass
 class MoveError(VoiceCommandException):
    pass
 class TurnError(VoiceCommandException):
    pass
 class PickUpError(VoiceCommandException):
    pass
 class SetDownError(VoiceCommandException):
    pass
 class GiveError(VoiceCommandException):
    pass
 class NotRecognizedError(VoiceCommandException):
    pass
 # Temp for demo -- position presets
 Position = [    [ 0.0, 0.0, 0.0 ],
                [ 0.5, 0.0, 0.0 ],
                [ 0.0, -0.5, 0.4 ],
                [ 0.0, 0.0, 0.5 ],
                [ 0.2, 0.7, 0.1 ]   ]
 class HighLvlCmdSrv(Node):
    def __init__(self):
        super().__init__('high_lvl_cmd_srv')
        self.logger = self.get_logger()
        self.srv = self.create_service(HighLvlCmds, 'high_lvl_cmds', self._high_lvl_cmd_callback)
        self.fake_action_server_pub = self.create_publisher(PoseStamped, 'fake_action_server_topic', 10) # for testing
        self.pose_pub = self.create_publisher(PoseStamped, 'move_pose', 10)
        # # Subscribe to hand for self.holdingItem
        # self.subscription = self.create_subscription(
        #     ???,
        #     '???',
        #     self._hand_callback,
        #     10)
        self.holdingItem = Item.NONE   # Item currently being held; won't be necessary when hand/arbiter knows this
        self.CmdFunc = {    Cmd.NONE     : self.none,
                            Cmd.IGNORE   : self.ignore,
                            Cmd.OPEN     : self.open,
                            Cmd.CLOSE    : self.close,
                            Cmd.MOVE     : self.move,
                            Cmd.TURN     : self.turn,
                            Cmd.PICKUP   : self.pickup,
                            Cmd.SETDOWN  : self.setdown,
                            Cmd.GIVE     : self.give    }
    def _hand_callback(self, msg):
        # Receive info on which object is currently being held
        self.holdingItem = msg.data
    def _high_lvl_cmd_callback(self, request, response):
        self.requestedCommand       = Cmd(request.command)
        self.requestedItem          = Item(request.item)
        self.requestedDirection     = Direction(request.direction)
        self.requestedPosition      = request.position
        self.requestedDegrees       = request.degrees
        if not (    self.requestedCommand == Cmd.NONE       or  \
                    self.requestedCommand == Cmd.PICKUP     or  \
                    self.requestedCommand == Cmd.SETDOWN    or  \
                    self.requestedCommand == Cmd.GIVE               ):
            self.tts('This command does not yet interface with the arm. Try pickup, setdown, or give')
        response.success = False
        try:
            self.CmdFunc[self.requestedCommand]()
            response.success = True
        except Exception as e:
            self.tts(f"Error: {e}") # Send to some tts node?
        self.requestedCommand       = Cmd.NONE
        self.requestedItem          = Item.NONE
        self.requestedDirection     = Direction.NONE
        self.requestedPosition      = -1
        self.requestedDegrees       = -1
        return response
    def move_pose(self, point, level=False):
        poseStamped = PoseStamped()
        # Default header for now
        poseStamped.header = Header()
        poseStamped.pose.position = Point(x=point[0],
                                          y=point[1],
                                          z=point[2])
        vector = [point[0], point[1], point[2]]
        if level:
            vector[2] = 0
        quat = transforms3d.quaternions.axangle2quat(vector, 0)
        #poseStamped.pose.orientation = Quaternion(w=quat[0],
        #                                          x=quat[1],
        #                                          y=quat[2],
        #                                          z=quat[3])
        poseStamped.pose.orientation = Quaternion(w=0.7071069,
                                                  x=0.70710667,
                                                  y=0.0,
                                                  z=0.0)
        self.pose_pub.publish(poseStamped)
        self.fake_action_server_pub.publish(poseStamped)
    def open(self):
        # requestedItem = self.parseWords(WordsType.ITEM)
        if self.requestedItem != Item.NONE:
            if self.requestedItem != self.holdingItem:
                raise OpenError(f"Can't drop {self.requestedItem} because currently holding {self.holdingItem}")
        if self.holdingItem == Item.NONE:
            self.tts('Attempting to open the claw')
        else:
            self.tts(f'Attempting to drop the {self.holdingItem}')
        # Send command to action server to open the claw
        # self._send_goal_future = self._action_client.send_goal_async(
        #     goal_msg,
        #     feedback_callback=self.feedback_callback)
        self.holdingItem = Item.NONE # Delete this line once hand/arbiter is publishing this info
    def close(self):
        self.tts('Attempting to close the claw')
        # Send command to action server to close the claw
        self.holdingItem = Item.UNKNOWN # Delete this line once hand/arbiter is publishing this info
    def move(self):
        if self.requestedDirection == Direction.NONE:
            raise TurnError('A direction to move in must be specified')
        # if not (1 <= self.requestedPosition or self.requestedPosition <= len(Position)):
        #     raise TurnError(f'Please state a position from 1 to {len(Position)}')
        self.tts (f'Attempting to move')
        # self.tts(f'Attempting to move to a preset position {self.requestedPosition} {self.requestedDirection}')
        # self.move_pose(Position[self.requestedPosition-1])
    def turn(self):
        # self.requestedDegrees = self.parseWords(WordsType.DEGREES)
        # self.requestedDirection = self.parseWords(WordsType.DIRECTION)
        if self.requestedDirection == Direction.NONE:
            if self.requestedDegrees % 180 == 0:
                # Left/right does not matter if doing a 180
                self.requestedDirection = Direction.LEFT
            else:
                raise TurnError('A direction to turn must be specified')
        self.tts(f'Attempting to turn {self.requestedDegrees} degrees {self.requestedDirection}')
        # Calculate new position from current position + degrees to rotate
        # Send command to action server to move to new position
    def pickup(self):
        # requestedItem = self.parseWords(WordsType.ITEM)
        # Make sure hand is currently empty
        if self.holdingItem != Item.NONE:
            raise PickUpError(f"Can't pick up {self.requestedItem}, already holding {self.holdingItem}")
        if self.requestedItem == Item.NONE:
            raise PickUpError(f'Did not hear a valid item specified to pick up')
        self.tts(f'Attempting to pick up {self.requestedItem}')
        # Temp
        self.move_pose(Position[2])
        # Send command to action server to open the claw
        # Use video processing service client to ask for position of target
        # if !targetFound:
            # raise PickUpError(f'Unable to find the requested item')
        # Move to current item position, checking if it has moved
        # prevTargetPos = None
        # targetPos = None
        # firstTime = True
        # While (robot hand hasn't yet reached the target) or (firstTime)
            # targetPos = video_processing_service_request(target)
            # if targetPos == None:  # If the target wasn't found by video processing serivce
                # if firstTime:
                    # raise PickUpError(f'Unable to find the requested item')
                # targetPos = prevTargetPos
            # if firstTime or distance(targetPos, prevTargetPos) > some_threshold_distance
                # Send goal to action server to move to that position
        # Send command to action server to close the claw
        self.holdingItem = self.requestedItem # Delete this line once hand/arbiter is publishing this info
    def setdown(self):
        # Make sure hand is currently holding something
        if self.holdingItem == Item.NONE:
            raise SetDownError(f'Not currently holding anything to set down')
        # requestedItem = self.parseWords(WordsType.ITEM)
        # If an item was mentioned
        if self.requestedItem != Item.NONE:
            # Make sure it is the currently held item
            if self.requestedItem != self.holdingItem:
                raise SetDownError(f"Can't set down {self.requestedItem}, not currently holding it")
        self.tts(f'Attempting to set down {self.requestedItem}')
        # Temp
        self.move_pose(Position[3])
        # Send command to action server to go down to the table/surface (just current position but with Z = 0?)
        # Send command to action server to open the claw
        self.holdingItem = Item.NONE # Delete this line once hand/arbiter is publishing this info
    def give(self):
        # If the claw is not currently holding anything
        if self.holdingItem == Item.NONE:
            # Pick up the item first
            self.pickup()
        else:
            # requestedItem = self.parseWords(WordsType.ITEM)
            # If a specific item was requested
            if self.requestedItem != Item.NONE:
                # Make sure the requested item is currently being held
                if self.requestedItem != self.holdingItem:
                    raise GiveError(f"Can't give you {self.requestedItem}, already holding {self.holdingItem}")
        self.tts(f'Attempting to give {self.holdingItem} to the operator')
        # Temp
        self.move_pose(Position[5])
        # prevHandPos = None
        # handPos = None
        # firstTime = True
        # While (robot hand hasn't yet reached operator hand) or (firstTime)
            # handPos = video_processing_service_request(opHand)
            # if handPos == None:  # If operator hand wasn't found by video processing serivce
                # if firstTime:
                    # raise GiveError('Unable to find the requested item')
                # handPos = prevHandPos
            # if firstTime or distance(handPos, prevHandPos) > some_threshold_distance
                # Send goal to action server to move to that position
        # isGrabbed = hand_service_client(something) to block until the hand determines that the operator is holding the target
        # if isGrabbed:
            # Send command to action server to open the claw
        # else:
            # raise GrabError(f'Couldn't tell whether the {self.holdingItem} was being grabbed')
        self.holdingItem = Item.NONE # Delete this line once hand/arbiter is publishing this info
    def ignore(self):
        self.tts('Ignoring')
    def none(self):
        raise NotRecognizedError("Sorry, I didn't get that.")
    def tts(self, phrase):
        # Send to some text-to-speech node, maybe the arbiter?
        self.logger.info(phrase)
 def main(args=None):
    rclpy.init(args=args)
    high_lvl_cmd_service = HighLvlCmdSrv()
    rclpy.spin(high_lvl_cmd_service)
    high_lvl_cmd_service.destroy_node()
    rclpy.shutdown()
 if __name__ == '__main__':
    main()
--- a/src/high_lvl_cmd_srv/package.xml
+++ b/src/high_lvl_cmd_srv/package.xml
@ -0,0 +1,18 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
  <name>high_lvl_cmd_srv</name>
  <version>0.0.0</version>
  <description>Service server for high-level commands</description>
  <maintainer email="dwall2018@my.fit.edu">Daniel Wall</maintainer>
  <license>TODO: License declaration</license>
  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
  <test_depend>python3-pytest</test_depend>
  <export>
    <build_type>ament_python</build_type>
  </export>
 </package>
--- a/src/high_lvl_cmd_srv/resource/high_lvl_cmd_srv
+++ b/src/high_lvl_cmd_srv/resource/high_lvl_cmd_srv
--- a/src/high_lvl_cmd_srv/setup.cfg
+++ b/src/high_lvl_cmd_srv/setup.cfg
@ -0,0 +1,4 @@
 [develop]
 script-dir=$base/lib/high_lvl_cmd_srv
 [install]
 install-scripts=$base/lib/high_lvl_cmd_srv
--- a/src/high_lvl_cmd_srv/setup.py
+++ b/src/high_lvl_cmd_srv/setup.py
@ -0,0 +1,26 @@
 from setuptools import setup
 package_name = 'high_lvl_cmd_srv'
 setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Daniel Wall',
    maintainer_email='dwall2018@my.fit.edu',
    description='Service server for high-level commands',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'high_lvl_cmd_srv = high_lvl_cmd_srv.high_lvl_cmd_srv:main'
        ],
    },
 )
--- a/src/high_lvl_cmd_srv/test/test_copyright.py
+++ b/src/high_lvl_cmd_srv/test/test_copyright.py
@ -0,0 +1,23 @@
 # Copyright 2015 Open Source Robotics Foundation, Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
 # You may obtain a copy of the License at
 #
 #     http://www.apache.org/licenses/LICENSE-2.0
 #
 # Unless required by applicable law or agreed to in writing, software
 # distributed under the License is distributed on an "AS IS" BASIS,
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 # See the License for the specific language governing permissions and
 # limitations under the License.
 from ament_copyright.main import main
 import pytest
@pytest.mark.copyright
@pytest.mark.linter
 def test_copyright():
    rc = main(argv=['.', 'test'])
    assert rc == 0, 'Found errors'
--- a/src/high_lvl_cmd_srv/test/test_flake8.py
+++ b/src/high_lvl_cmd_srv/test/test_flake8.py
@ -0,0 +1,25 @@
 # Copyright 2017 Open Source Robotics Foundation, Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
 # You may obtain a copy of the License at
 #
 #     http://www.apache.org/licenses/LICENSE-2.0
 #
 # Unless required by applicable law or agreed to in writing, software
 # distributed under the License is distributed on an "AS IS" BASIS,
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 # See the License for the specific language governing permissions and
 # limitations under the License.
 from ament_flake8.main import main_with_errors
 import pytest
@pytest.mark.flake8
@pytest.mark.linter
 def test_flake8():
    rc, errors = main_with_errors(argv=[])
    assert rc == 0, \
        'Found %d code style errors / warnings:\n' % len(errors) + \
        '\n'.join(errors)
--- a/src/high_lvl_cmd_srv/test/test_pep257.py
+++ b/src/high_lvl_cmd_srv/test/test_pep257.py
@ -0,0 +1,23 @@
 # Copyright 2015 Open Source Robotics Foundation, Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
 # You may obtain a copy of the License at
 #
 #     http://www.apache.org/licenses/LICENSE-2.0
 #
 # Unless required by applicable law or agreed to in writing, software
 # distributed under the License is distributed on an "AS IS" BASIS,
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 # See the License for the specific language governing permissions and
 # limitations under the License.
 from ament_pep257.main import main
 import pytest
@pytest.mark.linter
@pytest.mark.pep257
 def test_pep257():
    rc = main(argv=['.', 'test'])
    assert rc == 0, 'Found code style errors / warnings'
--- a/src/voice_move/package.xml
+++ b/src/voice_move/package.xml
@ -0,0 +1,18 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
  <name>voice_move</name>
  <version>0.0.0</version>
  <description>AR3 voice command package</description>
  <maintainer email="dw268145@gmail.com">Daniel Wall</maintainer>
  <license>TODO: License declaration</license>
  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
  <test_depend>python3-pytest</test_depend>
  <export>
    <build_type>ament_python</build_type>
  </export>
 </package>
--- a/src/voice_move/resource/voice_move
+++ b/src/voice_move/resource/voice_move
--- a/src/voice_move/setup.cfg
+++ b/src/voice_move/setup.cfg
@ -0,0 +1,4 @@
 [develop]
 script-dir=$base/lib/voice_move
 [install]
 install-scripts=$base/lib/voice_move
--- a/src/voice_move/setup.py
+++ b/src/voice_move/setup.py
@ -0,0 +1,25 @@
 from setuptools import setup
 package_name = 'voice_move'
 setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Daniel Wall',
    maintainer_email='dw268145@gmail.com',
    description='AR3 voice command package',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'voice_listener = voice_move.voice_listener:main'        ],
    },
 )
--- a/src/voice_move/test/test_copyright.py
+++ b/src/voice_move/test/test_copyright.py
@ -0,0 +1,23 @@
 # Copyright 2015 Open Source Robotics Foundation, Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
 # You may obtain a copy of the License at
 #
 #     http://www.apache.org/licenses/LICENSE-2.0
 #
 # Unless required by applicable law or agreed to in writing, software
 # distributed under the License is distributed on an "AS IS" BASIS,
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 # See the License for the specific language governing permissions and
 # limitations under the License.
 from ament_copyright.main import main
 import pytest
@pytest.mark.copyright
@pytest.mark.linter
 def test_copyright():
    rc = main(argv=['.', 'test'])
    assert rc == 0, 'Found errors'
--- a/src/voice_move/test/test_flake8.py
+++ b/src/voice_move/test/test_flake8.py
@ -0,0 +1,25 @@
 # Copyright 2017 Open Source Robotics Foundation, Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
 # You may obtain a copy of the License at
 #
 #     http://www.apache.org/licenses/LICENSE-2.0
 #
 # Unless required by applicable law or agreed to in writing, software
 # distributed under the License is distributed on an "AS IS" BASIS,
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 # See the License for the specific language governing permissions and
 # limitations under the License.
 from ament_flake8.main import main_with_errors
 import pytest
@pytest.mark.flake8
@pytest.mark.linter
 def test_flake8():
    rc, errors = main_with_errors(argv=[])
    assert rc == 0, \
        'Found %d code style errors / warnings:\n' % len(errors) + \
        '\n'.join(errors)
--- a/src/voice_move/test/test_pep257.py
+++ b/src/voice_move/test/test_pep257.py
@ -0,0 +1,23 @@
 # Copyright 2015 Open Source Robotics Foundation, Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
 # You may obtain a copy of the License at
 #
 #     http://www.apache.org/licenses/LICENSE-2.0
 #
 # Unless required by applicable law or agreed to in writing, software
 # distributed under the License is distributed on an "AS IS" BASIS,
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 # See the License for the specific language governing permissions and
 # limitations under the License.
 from ament_pep257.main import main
 import pytest
@pytest.mark.linter
@pytest.mark.pep257
 def test_pep257():
    rc = main(argv=['.', 'test'])
    assert rc == 0, 'Found code style errors / warnings'
--- a/src/voice_move/voice_move/init.py
+++ b/src/voice_move/voice_move/init.py
--- a/src/voice_move/voice_move/resources/ar3.dict
+++ b/src/voice_move/voice_move/resources/ar3.dict
--- a/src/voice_move/voice_move/resources/ar3.lm
+++ b/src/voice_move/voice_move/resources/ar3.lm
@ -0,0 +1,801 @@
 \data\
 ngram 1=217
 ngram 2=496
 ngram 3=75
 \1-grams:
 -0.927389	</s>
 -99	<s>	-0.2633025
 -2.554981	English	-0.1232651
 -2.276134	I	-0.1232651
 -2.554981	I'm	-0.1232651
 -1.945257	a	-0.1298478
 -2.554981	able	-0.1232652
 -2.554981	ah	-0.1232651
 -2.554981	all	-0.1232652
 -2.309698	also	-0.1232651
 -2.554981	am	-0.1232651
 -2.554981	an	-0.1232652
 -2.276134	and	-0.1232652
 -2.309698	apple	-0.1232651
 -2.554981	apricot	-0.1232651
 -2.484276	ar3	-0.1455714
 -2.136016	are	-0.1831803
 -2.554981	arm	-0.1232652
 -2.554981	arms	-0.1232651
 -2.309698	as	-0.1232651
 -2.554981	at	-0.1232652
 -2.554981	ay	-0.1232652
 -2.484276	ball	-0.1433201
 -2.484276	banana	-0.1433201
 -2.309698	be	-0.1232652
 -2.554981	below	-0.1232652
 -2.554981	best	-0.1232652
 -2.484276	block	-0.1433201
 -2.554981	bop	-0.1232651
 -2.554981	but	-0.1232652
 -2.030246	can	-0.2001237
 -2.309698	claw	-0.2527046
 -2.309698	close	-0.2051412
 -2.554981	concerns	-0.1232651
 -2.554981	conscience	-0.1232651
 -2.554981	cool	-0.1232651
 -2.484276	could	-0.2051412
 -2.554981	couple	-0.1232652
 -2.554981	cut	-0.1232651
 -2.554981	dangerous	-0.1232651
 -1.630039	degrees	-0.3655295
 -2.484276	do	-0.1232651
 -2.554981	doesn't	-0.1232651
 -2.554981	doing	-0.1232652
 -2.554981	don't	-0.1232651
 -2.030246	down	-0.1731316
 -2.309698	drop	-0.2527047
 -2.309698	eighty	-0.2051412
 -2.554981	ethical	-0.1232651
 -2.554981	expand	-0.1232651
 -2.554981	extra	-0.1232651
 -2.309698	false	-0.1232651
 -2.554981	familiar	-0.1232652
 -2.554981	fantastic	-0.1232651
 -2.554981	favorite	-0.1232651
 -2.484276	fifteen	-0.1433201
 -2.554981	fifty	-0.1232652
 -2.554981	filler	-0.1232651
 -2.554981	firmly	-0.1232652
 -2.136016	five	-0.3651688
 -2.554981	for	-0.1232651
 -2.554981	forever	-0.1232651
 -2.554981	forty	-0.1232652
 -2.554981	fourty	-0.1232651
 -2.309698	free	-0.2527046
 -2.554981	fruit	-0.1232652
 -2.554981	fruits	-0.1232652
 -2.554981	fun	-0.1232651
 -2.309698	get	-0.2051412
 -2.309698	gimme	-0.2527047
 -2.484276	give	-0.1351866
 -2.554981	go	-0.1232652
 -2.554981	gonna	-0.1232651
 -2.554981	good	-0.1232651
 -2.484276	grab	-0.1382189
 -2.309698	grasp	-0.1831803
 -2.136016	hand	-0.1731316
 -2.484276	have	-0.1232651
 -2.484276	here	-0.2051412
 -2.554981	hey	-0.1232651
 -2.554981	hundred	-0.1232652
 -2.484276	if	-0.2051412
 -2.554981	ignore	-0.1232651
 -2.554981	in	-0.1232651
 -2.554981	infamy	-0.1232651
 -2.554981	into	-0.1232652
 -2.554981	irrelevant	-0.1232651
 -2.484276	is	-0.1232651
 -1.501625	it	-0.3470554
 -2.554981	it's	-0.1232651
 -2.554981	its	-0.1232651
 -2.554981	job	-0.1232651
 -2.554981	knife	-0.1232651
 -2.554981	know	-0.1232652
 -2.554981	language	-0.1232651
 -2.554981	lanugage	-0.1232652
 -2.554981	leave	-0.1232652
 -2.484276	left	-0.1521903
 -2.309698	let	-0.2527047
 -2.554981	list	-0.1232652
 -2.554981	live	-0.1232651
 -2.554981	long	-0.1232652
 -2.554981	matter	-0.1232652
 -2.309698	maybe	-0.2527046
 -2.484276	me	-0.1831803
 -2.554981	might	-0.1232651
 -2.554981	mind	-0.1232651
 -2.554981	minimize	-0.1232652
 -2.309698	model	-0.2527046
 -2.554981	model's	-0.1232651
 -2.484276	move	-0.1232651
 -2.554981	my	-0.1232651
 -2.554981	need	-0.1232652
 -2.554981	needs	-0.1232652
 -2.309698	never	-0.2527047
 -2.554981	nice	-0.1232651
 -2.554981	nine	-0.1232651
 -2.309698	ninety	-0.2527047
 -2.309698	no	-0.1232652
 -2.554981	nothing	-0.1232652
 -2.554981	now	-0.1232652
 -2.554981	object	-0.1232651
 -2.136016	of	-0.2051412
 -2.309698	on	-0.1232652
 -2.276134	one	-0.1831803
 -2.554981	open	-0.1232652
 -2.554981	operator	-0.1232651
 -2.554981	or	-0.1232652
 -2.554981	orange	-0.1232651
 -2.554981	other	-0.1232651
 -2.309698	our	-0.1232652
 -2.554981	outside	-0.1232651
 -2.554981	pear	-0.1232651
 -2.554981	peel	-0.1232652
 -2.554981	phrase	-0.1232651
 -2.309698	phrases	-0.1232652
 -2.276134	pick	-0.2682588
 -2.276134	please	-0.1705177
 -2.554981	positive	-0.1232651
 -2.554981	positives	-0.1232651
 -2.554981	possible	-0.1232651
 -2.554981	pull	-0.1232651
 -2.554981	put	-0.1232651
 -2.554981	raise	-0.1232651
 -2.554981	rate	-0.1232652
 -2.554981	recognize	-0.1232651
 -2.554981	reduces	-0.1232651
 -2.309698	ree	-0.2527046
 -2.309698	release	-0.1232652
 -2.136016	right	-0.1564843
 -2.309698	robot	-0.1232651
 -2.554981	rotate	-0.1232652
 -2.554981	says	-0.1232652
 -2.554981	second	-0.1232651
 -2.484276	set	-0.2527046
 -2.554981	seventy	-0.1232651
 -2.554981	shoehorn	-0.1232651
 -2.554981	should	-0.1232651
 -2.554981	shouldn't	-0.1232652
 -2.554981	similar	-0.1232652
 -2.554981	six	-0.1232651
 -2.309698	sixty	-0.1232652
 -2.554981	so	-0.1232652
 -2.554981	some	-0.1232652
 -2.554981	someone	-0.1232651
 -2.484276	something	-0.2051412
 -2.554981	sound	-0.1232651
 -2.554981	spoken	-0.1232651
 -2.554981	stabbed	-0.1232651
 -2.554981	still	-0.1232651
 -2.554981	stuff	-0.1232651
 -2.309698	take	-0.2051412
 -2.554981	task	-0.1232651
 -2.309698	ten	-0.2527047
 -1.945257	that	-0.1705177
 -1.304082	the	-0.2475785
 -2.554981	these	-0.1232651
 -2.554981	things	-0.1232652
 -2.484276	thirty	-0.1831803
 -2.309698	this	-0.1232651
 -2.554981	though	-0.1232652
 -2.554981	thought	-0.1232651
 -2.484276	three	-0.2051412
 -1.669099	to	-0.1629548
 -2.554981	train	-0.1232651
 -2.554981	training	-0.1232651
 -2.554981	turn	-0.1232651
 -2.309698	twelve	-0.2527047
 -2.136016	twenty	-0.2351289
 -2.554981	twist	-0.1232651
 -2.309698	two	-0.1232651
 -2.554981	type	-0.1232651
 -2.484276	uh	-0.1232651
 -2.484276	uhh	-0.1232651
 -2.309698	um	-0.1232652
 -2.309698	umm	-0.1232651
 -2.276134	up	-0.1831803
 -2.554981	use	-0.1232652
 -2.554981	useless	-0.1232651
 -2.554981	very	-0.1232651
 -2.309698	wait	-0.2051412
 -2.554981	wake	-0.1232651
 -2.554981	want	-0.1232652
 -2.484276	we	-0.1232652
 -2.554981	we're	-0.1232651
 -2.309698	what	-0.1232652
 -2.554981	whatever	-0.1232652
 -2.554981	why	-0.1232652
 -2.554981	will	-0.1232652
 -2.484276	with	-0.1232651
 -2.554981	without	-0.1232651
 -2.554981	word	-0.1232651
 -2.030246	words	-0.3143101
 -2.484276	would	-0.1831803
 -2.554981	write	-0.1232652
 -2.484276	you	-0.1232651
 -2.309698	your	-0.2051412
 \2-grams:
 -2.072016	<s> I
 -2.510955	<s> I'm
 -2.510955	<s> ah
 -2.351471	<s> and
 -2.372946	<s> apple
 -2.510955	<s> apricot
 -1.193228	<s> ar3	-0.106434
 -2.255607	<s> are
 -2.163895	<s> ay	-0.1525484
 -2.474674	<s> banana
 -2.510955	<s> below
 -2.510955	<s> bop
 -1.971793	<s> can	-0.2651552
 -2.083168	<s> close
 -2.510955	<s> cool
 -2.474674	<s> could
 -2.132391	<s> do	-0.2651552
 -2.510955	<s> doesn't
 -2.096368	<s> drop	-0.1468107
 -2.510955	<s> fantastic
 -2.510955	<s> firmly
 -2.083168	<s> get
 -2.083168	<s> gimme	-0.2651552
 -2.132391	<s> give
 -2.510955	<s> good
 -2.147203	<s> grab	-0.1551153
 -2.096368	<s> grasp
 -2.01902	<s> hand
 -2.474674	<s> have
 -2.132391	<s> here	-0.2651552
 -1.414515	<s> hey	-0.1100757
 -2.474674	<s> if
 -2.510955	<s> ignore
 -1.647452	<s> it
 -2.148514	<s> leave
 -2.096368	<s> let	-0.1593523
 -2.372946	<s> maybe
 -1.114947	<s> move	-0.1288264
 -2.510955	<s> my
 -2.372946	<s> never
 -2.510955	<s> nice
 -2.372946	<s> no
 -2.163895	<s> now
 -2.255607	<s> of
 -2.372946	<s> on
 -2.163895	<s> open	-0.1526322
 -2.510955	<s> orange
 -2.510955	<s> other
 -2.510955	<s> pear
 -2.510955	<s> peel
 -2.351471	<s> pick
 -2.084877	<s> please	-0.1558526
 -2.510955	<s> pull
 -2.148514	<s> put
 -2.372946	<s> release
 -2.148514	<s> rotate
 -2.147203	<s> set	-0.1475396
 -2.510955	<s> so
 -2.510955	<s> spoken
 -2.083168	<s> take
 -1.7483	<s> that	-0.1113207
 -2.372946	<s> this
 -1.762749	<s> to
 -1.291067	<s> turn	-0.08356892
 -2.510955	<s> twist
 -2.147203	<s> uh
 -2.147203	<s> uhh
 -2.083168	<s> um
 -2.372946	<s> umm
 -2.083168	<s> wait
 -2.474674	<s> we
 -2.372946	<s> what
 -2.474674	<s> with
 -2.510955	<s> without
 -2.177124	<s> words
 -0.4735377	English </s>
 -1.288036	I am
 -1.248388	I can
 -1.288036	I don't
 -1.288036	I want
 -1.288036	I write
 -0.6034485	I'm gonna
 -1.52451	a banana
 -1.463785	a can
 -1.529851	a couple
 -1.018816	a hundred	-0.1525484
 -1.529851	a knife
 -1.503311	a one
 -1.52451	a three
 -0.5796575	able to
 -0.4735377	ah </s>
 -0.5975516	all of
 -0.8995579	also have
 -0.900838	also very
 -0.6034485	am training
 -0.6006823	an apple
 -0.8216937	and </s>
 -1.192163	and fifteen
 -1.183983	and our
 -1.183983	and phrases
 -0.6725531	apple </s>
 -0.900838	apple or
 -0.4735377	apricot </s>
 -0.8256481	ar3 </s>
 -1.64756	ar3 close
 -1.67054	ar3 could
 -1.64756	ar3 drop
 -1.64756	ar3 gimme	-0.2651552
 -1.615522	ar3 hand
 -1.153575	ar3 move
 -1.64756	ar3 never
 -1.64216	ar3 please
 -1.67054	ar3 uh
 -0.9649264	are </s>
 -1.467213	are free
 -0.9503368	are some
 -1.480979	are three
 -1.480979	are we
 -0.949087	are you	-0.2651552
 -0.5796575	arm to
 -0.6034485	arms though
 -0.900838	as it's
 -0.900838	as long
 -0.5975516	at hand
 -0.8892196	ay are
 -0.8953688	ay ree
 -0.4364524	ball </s>
 -0.4364524	banana </s>
 -1.073319	be able
 -1.073319	be dangerous
 -1.065208	be on
 -0.5975516	below are
 -0.5796575	best to
 -0.4364524	block </s>
 -0.5673129	bop it
 -0.6006823	but also
 -0.471241	can </s>
 -1.385271	can down
 -1.431117	can type
 -1.42749	can you
 -0.2948165	claw </s>
 -0.434388	close </s>
 -0.9456691	close the
 -0.4735377	concerns </s>
 -0.4735377	conscience </s>
 -0.4735377	cool </s>
 -1.068419	could also
 -0.5284965	could you
 -0.5975516	couple of
 -0.6034485	cut an
 -0.6034485	dangerous but
 -0.3024544	degrees </s>
 -1.816673	degrees down
 -1.898075	degrees left
 -1.842149	degrees right	-0.2651552
 -1.322732	degrees to	-0.284291
 -1.898075	degrees uhh
 -1.391353	degrees up	-0.1593523
 -1.041396	do a
 -1.073319	do its
 -1.071416	do with
 -0.6034485	doesn't matter
 -0.5923633	doing that
 -0.6034485	don't know
 -0.6578178	down </s>
 -1.351895	down ten	-0.2651552
 -0.7438208	down the	-0.07963848
 -0.6230923	drop it	-0.1593523
 -0.6049852	drop the	-0.2651552
 -0.8066849	eighty </s>
 -0.5104414	eighty degrees	-0.1560122
 -0.6034485	ethical concerns
 -0.54594	expand the
 -0.6034485	extra useless
 -0.900838	false positive
 -0.900838	false positives
 -0.6028026	familiar with
 -0.4735377	fantastic </s>
 -0.6034485	favorite fruit
 -0.5258819	fifteen degrees	-0.1238342
 -0.5975516	fifty five
 -0.5949497	filler words
 -0.6006823	firmly grasp
 -0.2375018	five degrees	-0.198637
 -0.54594	for the
 -0.6034485	forever live
 -0.5975516	forty five
 -0.5771604	fourty degrees
 -0.2948165	free </s>
 -0.6028026	fruit is
 -0.6028026	fruits here
 -0.4735377	fun </s>
 -1.048463	get that
 -0.4880389	get the
 -0.3289228	gimme the	-0.1087829
 -1.139672	give it
 -0.7668757	give me	-0.1459006
 -1.066564	give the
 -0.5975516	go of	-0.1121509
 -0.6034485	gonna list
 -0.6034485	good job
 -1.154732	grab that
 -0.6074318	grab the	-0.1593523
 -0.6175543	grasp it	-0.2651552
 -1.025304	grasp the
 -1.187149	grasp your
 -0.6451473	hand </s>
 -1.205297	hand it
 -0.84545	hand me	-0.1593522
 -0.9743345	have it
 -1.073319	have nothing
 -0.9217272	have the
 -0.434388	here </s>
 -1.060846	here are	-0.2651552
 -1.192163	hey ar3
 -1.17211	hey are
 -1.183983	hey ree
 -1.192163	hey three
 -0.894358	hundred and
 -0.8892196	hundred twenty	-0.2651552
 -0.4880389	if the
 -1.075176	if we're
 -0.4735377	ignore </s>
 -0.6034485	in infamy
 -0.4735377	infamy </s>
 -0.6028026	into something
 -0.6034485	irrelevant stuff
 -1.071416	is banana
 -1.073319	is familiar
 -1.073319	is why
 -0.3316526	it </s>
 -1.845925	it cut
 -1.764467	it down	-0.2651552
 -1.839229	it is
 -1.845925	it might
 -1.845925	it needs
 -1.251256	it to	-0.2651552
 -1.845925	it will
 -1.839229	it would
 -0.6034485	it's English
 -0.6034485	its best
 -0.4735377	job </s>
 -0.4735377	knife </s>
 -0.6028026	know if
 -0.6034485	language model's
 -0.6006823	lanugage model
 -0.8318802	leave it
 -0.7933542	leave the
 -0.7132193	left </s>
 -1.483456	left fifty
 -1.443241	left five
 -1.483456	left seventy
 -1.483456	left six
 -1.478893	left thirty
 -1.464195	left twelve
 -0.3538837	let go	-0.284291
 -0.5923633	list a
 -0.6034485	live in
 -0.6006823	long as
 -0.6006823	matter what
 -0.3536137	maybe we
 -0.8560517	me </s>
 -1.083867	me it
 -0.5966411	me the	-0.07963848
 -0.6034485	might raise
 -0.4735377	mind </s>
 -0.6006823	minimize false
 -0.2948165	model </s>
 -0.6034485	model's possible
 -1.535624	move a
 -1.558863	move down	-0.2651552
 -1.64417	move forty
 -1.637128	move left
 -1.614714	move ninety
 -1.609443	move one
 -1.583417	move right
 -1.614714	move ten
 -1.637128	move thirty
 -1.435073	move to
 -1.614714	move twelve
 -1.609443	move up
 -0.6034485	my favorite
 -0.6006823	need two
 -0.5796575	needs to
 -0.3538837	never mind	-0.2651552
 -0.4735377	nice </s>
 -0.5771604	nine degrees
 -0.3427056	ninety degrees
 -0.6725531	no </s>
 -0.8953688	no wait
 -0.5796575	nothing to
 -1.065208	now get
 -1.056147	now hand
 -1.065208	now release
 -0.4735377	object </s>
 -1.367327	of fruits
 -1.215895	of it	-0.1593523
 -0.7495599	of the
 -0.8236019	of this
 -0.8953688	on our
 -0.900838	on second
 -0.6501603	one eighty
 -1.194319	one fifteen
 -1.176713	one twenty
 -0.7933542	open the
 -0.8953688	open your
 -0.6034485	operator says
 -0.6028026	or something
 -0.4735377	orange </s>
 -0.5949497	other words
 -0.900838	our conscience
 -0.8953688	our robot
 -0.4735377	outside </s>
 -0.4735377	pear </s>
 -0.5923633	peel a
 -0.6034485	phrase into
 -0.6725531	phrases </s>
 -0.8791218	phrases that
 -0.3337949	pick up	-0.1094048
 -1.287056	please give
 -1.287056	please grab
 -1.275753	please pick
 -0.7479384	please set	-0.2651552
 -0.6034485	positive rate
 -0.6034485	positives for
 -0.5949497	possible words
 -0.5673129	pull it
 -0.8841414	put down
 -0.7933542	put the
 -0.6034485	raise ethical
 -0.5975516	rate of
 -0.6034485	recognize things
 -0.54594	reduces the
 -0.2948165	ree </s>
 -0.8318802	release it
 -0.7933542	release the
 -0.6809343	right </s>
 -1.428945	right fourty
 -1.409006	right one
 -1.412076	right sixty
 -1.393621	right twenty
 -1.412076	right um
 -1.073319	robot stabbed
 -1.073319	robot use
 -1.071416	robot would
 -0.8892196	rotate right
 -0.854859	rotate to
 -0.6028026	says something
 -0.6034485	second thought
 -0.6463002	set down	-0.284291
 -0.6230923	set it	-0.2651552
 -0.5771604	seventy degrees
 -0.54594	shoehorn the
 -0.6034485	should train
 -0.6028026	shouldn't have
 -0.5796575	similar to
 -0.5771604	six degrees
 -0.6725531	sixty </s>
 -0.8953688	sixty two
 -0.6006823	so maybe
 -1.073319	some filler
 -1.065208	some phrases
 -1.048708	some words
 -0.4735377	someone </s>
 -0.434388	something </s>
 -1.075176	something outside
 -0.6034485	sound similar
 -0.6034485	spoken phrase
 -0.6034485	stabbed someone
 -0.6034485	still doing
 -0.4735377	stuff </s>
 -0.9913476	take it
 -0.4880389	take the
 -0.6034485	task at
 -0.3427056	ten degrees	-0.1238342
 -0.7745703	that </s>
 -1.569851	that ball
 -1.569851	that block
 -1.024437	that can	-0.2651552
 -1.569851	that is
 -1.337385	that it
 -1.575251	that sound
 -1.569851	that would
 -1.135635	the </s>
 -2.101737	the arm
 -1.126343	the ball	-0.6364735
 -1.30877	the block	-0.284291
 -0.8878639	the can	-0.4995283
 -1.612732	the claw	-0.1593523
 -2.040598	the false
 -2.101737	the language
 -2.086708	the left	-0.06976753
 -1.595258	the model	-0.2651552
 -1.31124	the object	-0.284291
 -2.101737	the operator
 -1.980039	the right	-0.1593523
 -1.595258	the robot
 -2.101737	the task
 -2.086708	the uh
 -2.086708	the uhh
 -2.040598	the umm
 -2.101737	the wake
 -0.6034485	these extra
 -0.5923633	things that
 -0.6271824	thirty degrees
 -1.176713	thirty five
 -1.196519	thirty nine
 -1.073319	this irrelevant
 -1.073319	this lanugage
 -1.073319	this reduces
 -0.6001691	though and
 -0.5673129	thought it
 -0.434388	three </s>
 -1.068419	three sixty
 -0.9990164	to </s>
 -1.672415	to ar3
 -1.650221	to be
 -1.672415	to do
 -1.679385	to expand
 -1.672415	to me	-0.2651552
 -1.679385	to minimize
 -1.679385	to recognize
 -1.679385	to shoehorn
 -0.7547235	to the	-0.3354435
 -0.54594	train the
 -0.54594	training the
 -1.409039	turn eighty	-0.2651552
 -1.409039	turn ninety
 -1.405749	turn one
 -1.389285	turn right
 -1.422862	turn thirty
 -1.288778	turn to	-0.1593522
 -1.389285	turn twenty
 -0.3427056	twelve degrees
 -0.417608	twenty degrees	-0.05376146
 -1.270225	twenty five
 -0.5673129	twist it
 -0.900838	two arms
 -0.8501649	two degrees
 -0.6034485	type whatever
 -0.8588269	uh </s>
 -1.248388	uh can
 -1.272398	uh pick
 -1.272398	uh please
 -1.284921	uh set
 -0.8588269	uhh </s>
 -1.284921	uhh ball
 -1.274816	uhh let
 -1.284921	uhh move
 -1.272398	uhh up
 -0.766093	um </s>
 -1.041396	um a
 -1.063714	um please
 -0.6725531	umm </s>
 -0.8995579	umm block
 -0.7262802	up </s>
 -0.9291229	up a
 -1.480979	up fifteen
 -1.416486	up that
 -1.197555	up the	-0.2651552
 -1.447536	up twenty
 -0.5923633	use a
 -0.5949497	useless words
 -0.6034485	very fun
 -0.434388	wait </s>
 -1.068419	wait no
 -0.6034485	wake word
 -0.6028026	want here
 -0.8216937	we </s>
 -1.192163	we could
 -1.194677	we should
 -1.194677	we shouldn't
 -0.6034485	we're still
 -0.894358	what I
 -0.8995579	what if
 -0.6001691	whatever I
 -0.6001691	why I
 -0.6028026	will do
 -0.766093	with </s>
 -1.073319	with all
 -0.9217272	with the
 -0.6034485	without these
 -0.4735377	word </s>
 -0.2747475	words </s>
 -1.359004	words and
 -0.6501603	would be
 -1.196519	would forever
 -1.196519	would need
 -0.6006823	write as
 -1.274816	you free	-0.2651552
 -1.284921	you give
 -1.284921	you grab
 -1.272398	you pick
 -1.274816	you take
 -0.5270156	your claw	-0.2651552
 -1.060846	your hand
 \3-grams:
 -1.03808	<s> ar3 drop
 -1.03808	<s> ar3 gimme
 -0.9054043	<s> ar3 move
 -0.2858141	here are some
 -0.4122591	<s> ay are
 -0.06875182	the ball </s>
 -0.1735543	the block </s>
 -0.3212658	<s> can you
 -0.1935282	that can </s>
 -0.1021581	the can </s>
 -0.1814045	the claw </s>
 -0.1352608	your claw </s>
 -0.4749602	eighty degrees to
 -0.2056416	fifteen degrees </s>
 -0.3012768	five degrees </s>
 -0.7169678	five degrees right
 -0.2056416	ten degrees </s>
 -0.2543235	twenty degrees </s>
 -0.2955816	<s> do a
 -0.2393137	it down </s>
 -0.3177687	move down ten
 -0.2410243	set down the
 -0.3403704	<s> drop it
 -0.2044024	turn eighty degrees
 -0.1352608	you free </s>
 -0.1477756	<s> gimme the
 -0.1477756	ar3 gimme the
 -0.2135184	let go of
 -0.4567531	<s> grab that
 -0.2974403	<s> here are
 -0.7908074	<s> hey ar3
 -0.7849265	<s> hey are
 -0.4122591	a hundred twenty
 -0.2006717	drop it </s>
 -0.1487505	grasp it </s>
 -0.2006717	of it </s>
 -0.3313491	set it down
 -0.7528197	the left thirty
 -0.2119222	<s> let go
 -0.3313827	give me the
 -0.3164783	hand me the
 -0.2736062	to me </s>
 -0.194185	never mind </s>
 -0.1352608	the model </s>
 -1.074147	<s> move down
 -1.090619	<s> move left
 -1.076775	<s> move right
 -1.08223	<s> move up
 -0.1837565	the object </s>
 -0.5613433	go of it
 -0.4135609	<s> open your
 -0.4716026	<s> please give
 -0.2440029	degrees right </s>
 -0.3452604	the right </s>
 -0.3479612	<s> set down
 -0.2369023	please set down
 -0.1526577	down ten degrees
 -0.6073971	<s> that would
 -0.5601725	down the can
 -0.3031707	drop the ball
 -0.5532022	gimme the ball
 -0.4449862	grab the ball
 -0.5601725	me the can
 -0.3570458	to the left
 -0.9697438	to the right
 -0.2779852	up the can
 -0.2427858	degrees to the
 -0.3293	it to me
 -0.3675284	turn to the
 -0.9211988	<s> turn eighty
 -0.886919	<s> turn to
 -0.1774735	hundred twenty degrees
 -0.3592568	degrees up </s>
 -0.5654147	pick up the
 -0.313555	are you free
 \end\
--- a/src/voice_move/voice_move/srv/HighLvlCmds.srv
+++ b/src/voice_move/voice_move/srv/HighLvlCmds.srv
@ -0,0 +1,6 @@
 int8 command
 int8 item
 int8 direction
 int16 degrees
 ---
 bool success
--- a/src/voice_move/voice_move/voice_listener.py
+++ b/src/voice_move/voice_move/voice_listener.py
@ -0,0 +1,154 @@
 import rclpy
 from rclpy.node import Node
 from defs.srv import HighLvlCmds
 from high_lvl_cmd_srv.cmd_constants import *
 import random
 from enum import Enum
 from pocketsphinx import LiveSpeech
 from word2number import w2n
 WakeWord        = 'hey ar3'
 LanguageModel   = '/ros_root/Control-Software/src/voice_move/voice_move/resources/ar3.lm'
 Dict            = '/ros_root/Control-Software/src/voice_move/voice_move/resources/ar3.dict'
 ConfirmWords    = ['yes', 'confirm', 'go ahead', 'do it']
 DenyWords       = ['no', 'deny', 'stop', 'wait', 'never mind', 'ignore', 'hold on']
 VoiceCmdWords   = { Cmd.IGNORE.value        : ['ignore', 'never mind', 'wait'],
                    Cmd.OPEN.value          : ['drop', 'let it', 'let go', 'open', 'release'],
                    Cmd.CLOSE.value         : ['close', 'grab', 'grasp', 'take'],
                    Cmd.MOVE.value          : ['move', 'shift', 'go to', 'position'],
                    Cmd.TURN.value          : ['turn', 'rotate'],
                    Cmd.PICKUP.value        : ['pick', 'get'],
                    Cmd.SETDOWN.value       : ['set', 'put', 'leave'],
                    Cmd.GIVE.value          : ['give', 'gimme', 'hand the', 'hand it', 'hand me', 'pass'] }
 ItemWords       = { Item.CAN.value          : ['a can', 'the can', 'that can', 'this can', 'soda can'],
                    Item.BLOCK.value        : ['block', 'cube'],
                    Item.BALL.value         : ['ball', 'sphere'] }
 DirectionWords  = { Direction.LEFT.value    : ['left', 'clockwise'],
                    Direction.RIGHT.value   : ['right', 'counter clockwise'],
                    Direction.UP.value      : ['up'],
                    Direction.DOWN.value    : ['down'] }
 # Used for choosing which set of word lists to use for word parsing
 EnumType = [Cmd, Item, Direction]
 Words = [VoiceCmdWords, ItemWords, DirectionWords]
 class WordsType(Enum):
    CMD         = 0
    ITEM        = 1
    DIRECTION   = 2
    POSITION    = 3
    DEGREES     = 4
 class VoiceListener(Node):
    def __init__(self):
        self.wakeWordSpoken = False
        self.speech = LiveSpeech(
            audio_device='plughw:CARD=Generic_1,DEV=0',
            lm=LanguageModel,
            dic=Dict
        )
        super().__init__('voice_move')
        self.logger = self.get_logger()
        self.logger.info('voice_listener init')
        self.cli = self.create_client(HighLvlCmds, 'high_lvl_cmds')
        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.logger.info('service not available, waiting again...')
        self.req = HighLvlCmds.Request()
        self._timer = self.create_timer(0.01, self._listen)
    def _listen(self):
        if self.wakeWordSpoken:
            self.logger.info(random.choice(['Yes?', 'Go on.', 'Go ahead.']))
        else:
            self.logger.info('Listening...')
        for phrase in self.speech:
            if self.wakeWordSpoken:
                phraseStr = str(phrase)
                self.logger.info(f"Heard '{phraseStr}'")
                command     = self.parseWords(phraseStr, WordsType.CMD)
                item        = Item.NONE
                direction   = Direction.NONE
                position    = -1
                degrees     = -1
                if command == Cmd.PICKUP or command == Cmd.SETDOWN or command == Cmd.GIVE:
                    item = self.parseWords(phraseStr, WordsType.ITEM)
                if command == Cmd.MOVE:
                    direction = self.parseWords(phraseStr, WordsType.DIRECTION)
                    degrees = self.parseWords(phraseStr, WordsType.DEGREES)
                self.req.command        = command.value
                self.req.item           = item.value
                self.req.direction      = direction.value
                self.req.position       = position
                self.req.degrees        = degrees
                self.future = self.cli.call_async(self.req)
                self.wakeWordSpoken = False
            else:
                if WakeWord in str(phrase):
                    # Wait for the command in the next phrase
                    self.wakeWordSpoken = True
            return
    def parseWords(self, phrase, wordsType):
        if wordsType == WordsType.POSITION or wordsType == WordsType.DEGREES:
            try:
                return w2n.word_to_num(phrase.replace('a hundred', 'one hundred'))
            except Exception:
                return -1
                # raise MoveError('Failed to interpret number of degrees')
        theWords = Words[wordsType.value]
        theEnum = EnumType[wordsType.value]
        for i in range(len(theWords)):
            wordList = theWords[i]
            for word in wordList:
                if word in phrase:
                    return theEnum(i)
        return theEnum.NONE
 def main(args=None):
    rclpy.init(args=args)
    voice_listener = VoiceListener()
    rclpy.spin(voice_listener)
    voice_listener.destroy_node()
    rclpy.shutdown
 if __name__ == '__main__':
    main()