Commit 8bc011c 1 parent 1709c7e commit 8bc011c Copy full SHA for 8bc011c
File tree 12 files changed +236
-0
lines changed
12 files changed +236
-0
lines changed Original file line number Diff line number Diff line change
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 ()
Original file line number Diff line number Diff line change
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 >
Original file line number Diff line number Diff line change
1
+ [develop]
2
+ script_dir =$base/lib/arm_controller
3
+ [install]
4
+ install_scripts =$base/lib/arm_controller
Original file line number Diff line number Diff line change
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
+ )
Original file line number Diff line number Diff line change
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'
Original file line number Diff line number Diff line change
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 )
Original file line number Diff line number Diff line change
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'
Original file line number Diff line number Diff line change
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 number Diff line number Diff line change
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
Original file line number Diff line number Diff line change
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 >
You can’t perform that action at this time.
0 commit comments