diff --git a/rasms_manipulator/.vscode/settings.json b/rasms_manipulator/.vscode/settings.json index 7d24ba4..9507a65 100644 --- a/rasms_manipulator/.vscode/settings.json +++ b/rasms_manipulator/.vscode/settings.json @@ -10,5 +10,6 @@ "/home/splinter1984/plansys_ws/install/ros2_knowledge_graph_msgs/lib/python3.8/site-packages", "/opt/ros/foxy/lib/python3.8/site-packages" ], - "ros.distro": "foxy" + "ros.distro": "foxy", + "C_Cpp.errorSquiggles": "Disabled" } \ No newline at end of file diff --git a/rasms_manipulator/behavior_trees/move.xml b/rasms_manipulator/behavior_trees/move.xml index f8a6dae..3cd0226 100644 --- a/rasms_manipulator/behavior_trees/move.xml +++ b/rasms_manipulator/behavior_trees/move.xml @@ -1,6 +1,6 @@ - + diff --git a/rasms_manipulator/config/params.yaml b/rasms_manipulator/config/params.yaml index 5288e3c..50dbf02 100644 --- a/rasms_manipulator/config/params.yaml +++ b/rasms_manipulator/config/params.yaml @@ -1,7 +1,5 @@ -move_1: +move: ros_parameters: - plugins: - - rasms_move_bt_node waypoints: ["zero", "one"] waypoints_coords: zero: [0.0, -2.0, 0.0] diff --git a/rasms_manipulator/include/rasms_bt/behavior_tree_nodes/Move.hpp b/rasms_manipulator/include/rasms_bt/behavior_tree_nodes/Move.hpp index 37b1a57..0337a65 100644 --- a/rasms_manipulator/include/rasms_bt/behavior_tree_nodes/Move.hpp +++ b/rasms_manipulator/include/rasms_bt/behavior_tree_nodes/Move.hpp @@ -13,6 +13,7 @@ #include "moveit_msgs/msg/place_location.hpp" #include "moveit_msgs/msg/move_it_error_codes.hpp" +#include "moveit_msgs/action/move_group.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" #include "tf2/transform_datatypes.h" diff --git a/rasms_manipulator/launch/rasms_manipulation.launch.py b/rasms_manipulator/launch/rasms_manipulation.launch.py new file mode 100644 index 0000000..129d47d --- /dev/null +++ b/rasms_manipulator/launch/rasms_manipulation.launch.py @@ -0,0 +1,47 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + pkg_dir = get_package_share_directory('rasms_manipulator') + namespace = LaunchConfiguration('namespace') + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Namespace') + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1') + + move_1 = Node( + package='plansys2_bt_actions', + executable='bt_action_node', + name='move_1', + namespace=namespace, + output='screen', + parameters=[ + pkg_dir + '/config/params.yaml', + { + 'action_name': 'move', + 'bt_xml_file': pkg_dir + '/behavior_trees_xml/move.xml' + } + ]) + + ld = LaunchDescription() + + ld.add_action(stdout_linebuf_envvar) + ld.add_action(declare_namespace_cmd) + + # Declare the launch options + ld.add_action(move_1) + + return ld \ No newline at end of file diff --git a/rasms_manipulator/src/rasms_controller_node.cpp b/rasms_manipulator/src/rasms_controller_node.cpp index 24b6b13..24a8a81 100644 --- a/rasms_manipulator/src/rasms_controller_node.cpp +++ b/rasms_manipulator/src/rasms_controller_node.cpp @@ -89,7 +89,7 @@ private: std::shared_ptr planner_client_; }; -int main(argc, char ** argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = std::make_shared();