Skip to content

Commit 8bc011c

Browse files
committedMay 7, 2024
Add arm_interface and arm_controller
1 parent 1709c7e commit 8bc011c

12 files changed

+236
-0
lines changed
 

‎src/arm_controller/arm_controller/__init__.py

Whitespace-only changes.
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,55 @@
1+
#!/usr/bin/env python3
2+
import rclpy
3+
from rclpy.node import Node
4+
from rclpy.action import ActionServer
5+
from rclpy.action.server import ServerGoalHandle
6+
from arm_interface.action import ExecArmAction
7+
8+
### CONSTANT ###
9+
# ID for action
10+
MOVE_HOME_POS = 1
11+
MOVE_READY_POS = 2
12+
MOVE_PUT_TO_STOCK = 3
13+
MOVE_GET_FROM_STOCK = 4
14+
MOVE_GRAB = 5
15+
MOVE_RELEASE = 6
16+
17+
class ArmController(Node):
18+
def __init__(self):
19+
super().__init__('arm_controller')
20+
self.exec_arm_action_ = ActionServer(
21+
self,
22+
ExecArmAction,
23+
"ArmController",
24+
execute_callback=self.arm_execute_callback)
25+
# Initialize current position of arm
26+
self.current_pos_ = [0, 0, 0]
27+
# Initialize the status of the arm
28+
self.is_busy_ = False
29+
self.get_logger().info('Arm Controller Node Started')
30+
31+
def arm_execute_callback(self, goal_handle : ServerGoalHandle):
32+
# Get the request for action
33+
action_id = goal_handle.request.action_id
34+
self.get_logger().info('Received request for action: %d' % action_id)
35+
# Execute the action
36+
## TODO: Send command to Herkulex motor via can TX
37+
# Update goal final status
38+
goal_handle.succeed()
39+
# Send the result
40+
result = ExecArmAction.Result()
41+
result.action_result = True
42+
return result
43+
44+
45+
46+
def main(args=None):
47+
try:
48+
rclpy.init(args=args)
49+
arm_controller = ArmController()
50+
rclpy.spin(arm_controller)
51+
except KeyboardInterrupt:
52+
rclpy.shutdown()
53+
54+
if __name__ == '__main__':
55+
main()

‎src/arm_controller/package.xml

+21
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>arm_controller</name>
5+
<version>0.0.0</version>
6+
<description>TODO: Package description</description>
7+
<maintainer email="hoangtriet26@gmail.com">robot</maintainer>
8+
<license>TODO: License declaration</license>
9+
10+
<depend>rclpy</depend>
11+
<depend>arm_interface</depend>
12+
13+
<test_depend>ament_copyright</test_depend>
14+
<test_depend>ament_flake8</test_depend>
15+
<test_depend>ament_pep257</test_depend>
16+
<test_depend>python3-pytest</test_depend>
17+
18+
<export>
19+
<build_type>ament_python</build_type>
20+
</export>
21+
</package>

‎src/arm_controller/resource/arm_controller

Whitespace-only changes.

‎src/arm_controller/setup.cfg

+4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
[develop]
2+
script_dir=$base/lib/arm_controller
3+
[install]
4+
install_scripts=$base/lib/arm_controller

‎src/arm_controller/setup.py

+26
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
from setuptools import find_packages, setup
2+
3+
package_name = 'arm_controller'
4+
5+
setup(
6+
name=package_name,
7+
version='0.1.0',
8+
packages=find_packages(exclude=['test']),
9+
data_files=[
10+
('share/ament_index/resource_index/packages',
11+
['resource/' + package_name]),
12+
('share/' + package_name, ['package.xml']),
13+
],
14+
install_requires=['setuptools'],
15+
zip_safe=True,
16+
maintainer='robot',
17+
maintainer_email='hoangtriet26@gmail.com',
18+
description='TODO: Package description',
19+
license='TODO: License declaration',
20+
tests_require=['pytest'],
21+
entry_points={
22+
'console_scripts': [
23+
'arm_controller_server = arm_controller.arm_controller_server:main',
24+
],
25+
},
26+
)
+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
# Copyright 2015 Open Source Robotics Foundation, Inc.
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
from ament_copyright.main import main
16+
import pytest
17+
18+
19+
# Remove the `skip` decorator once the source file(s) have a copyright header
20+
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
21+
@pytest.mark.copyright
22+
@pytest.mark.linter
23+
def test_copyright():
24+
rc = main(argv=['.', 'test'])
25+
assert rc == 0, 'Found errors'
+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
# Copyright 2017 Open Source Robotics Foundation, Inc.
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
from ament_flake8.main import main_with_errors
16+
import pytest
17+
18+
19+
@pytest.mark.flake8
20+
@pytest.mark.linter
21+
def test_flake8():
22+
rc, errors = main_with_errors(argv=[])
23+
assert rc == 0, \
24+
'Found %d code style errors / warnings:\n' % len(errors) + \
25+
'\n'.join(errors)
+23
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
# Copyright 2015 Open Source Robotics Foundation, Inc.
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
from ament_pep257.main import main
16+
import pytest
17+
18+
19+
@pytest.mark.linter
20+
@pytest.mark.pep257
21+
def test_pep257():
22+
rc = main(argv=['.', 'test'])
23+
assert rc == 0, 'Found code style errors / warnings'

‎src/arm_interface/CMakeLists.txt

+18
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(arm_interface)
3+
4+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
5+
add_compile_options(-Wall -Wextra -Wpedantic)
6+
endif()
7+
8+
# find dependencies
9+
find_package(ament_cmake REQUIRED)
10+
find_package(rosidl_default_generators REQUIRED)
11+
12+
rosidl_generate_interfaces(${PROJECT_NAME}
13+
"action/ExecArmAction.action"
14+
)
15+
16+
ament_export_dependencies(rosidl_default_runtime)
17+
18+
ament_package()
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
# Goal
2+
uint8 action_id
3+
---
4+
# Result
5+
bool action_result
6+
---
7+
8+
## ExecArmAction.action
9+
#@brief An interface for executing arm actions
10+
#@param action_id The id of the action to execute
11+
# 1 : Move to home position
12+
# 2 : Move to ready position (down to pick up/place object)
13+
# 3 : Put object to stock
14+
# 4 : Get object from stock
15+
# 5 : Grab
16+
# 6 : Release
17+
#@param action_result True if the action was successful, False otherwise

‎src/arm_interface/package.xml

+22
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>arm_interface</name>
5+
<version>0.0.0</version>
6+
<description>TODO: Package description</description>
7+
<maintainer email="hoangtriet26@gmail.com">robot</maintainer>
8+
<license>TODO: License declaration</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
12+
<buildtool_depend>rosidl_default_generators</buildtool_depend>
13+
<exec_depend>rosidl_default_runtime</exec_depend>
14+
<member_of_group>rosidl_interface_packages</member_of_group>
15+
16+
<test_depend>ament_lint_auto</test_depend>
17+
<test_depend>ament_lint_common</test_depend>
18+
19+
<export>
20+
<build_type>ament_cmake</build_type>
21+
</export>
22+
</package>

0 commit comments

Comments
 (0)
Please sign in to comment.