diff --git a/mg_bringup/launch/base.launch.py b/mg_bringup/launch/base.launch.py new file mode 100644 index 0000000..98b6374 --- /dev/null +++ b/mg_bringup/launch/base.launch.py @@ -0,0 +1,63 @@ +################################################################################ +# # +# Launch file meant to be used on the robot itself # +# uses: # +# * mg_control - for sending commands to the stepper drivers # +# * mg_odometry - for reading and controlling the MCU encoders # +# * mg_navigation - runs the local planner and handles move behaviors # +# * mg_navigation - runs the local planner and handles move behaviors # +# * mg_lidar - optionally runs the lidar node to read opponents # +# positions # +# # +################################################################################ + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + is_local_test = DeclareLaunchArgument( + 'local_test', + default_value="False", + description='Launch with simulated components' + ) + + return LaunchDescription([ + is_local_test, + IncludeLaunchDescription( + PathJoinSubstitution([ + FindPackageShare("mg_control"), + 'launch', + 'toid.launch.py' + ]), + launch_arguments={ + 'use_mock': LaunchConfiguration('local_test') + }.items() + ), + Node( + package="mg_odometry", + executable="mg_odom_publisher", + name="mg_odom_publisher", + condition=UnlessCondition(LaunchConfiguration('local_test')), + parameters=[{ + 'odom': "odom", + 'serial_path': "/dev/ttyACM0", + }], + + emulate_tty=True, + output='screen' + ), + Node( + package="mg_navigation", + executable="mg_nav_server", + name="mg_nav_server", + emulate_tty=True, + output='screen', + ) + ]) + \ No newline at end of file diff --git a/mg_bringup/launch/bt.launch.py b/mg_bringup/launch/bt.launch.py new file mode 100644 index 0000000..96af774 --- /dev/null +++ b/mg_bringup/launch/bt.launch.py @@ -0,0 +1,29 @@ +################################################################################ +# # +# Launch file meant to be used on the robot itself # +# uses: # +# * mg_bt - runs the robot's behavior tree server # +# # +################################################################################ + +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + basedir = FindPackageShare("mg_bt") + + bt_exec_config = PathJoinSubstitution([basedir, "config/mg_bt_executor.yaml"]) + + return LaunchDescription([ + Node( + package='mg_bt', + executable='mg_bt_executor', + output="screen", + parameters=[bt_exec_config] + ), + ]) + \ No newline at end of file diff --git a/mg_bt/CMakeLists.txt b/mg_bt/CMakeLists.txt index 9a9932d..e541508 100644 --- a/mg_bt/CMakeLists.txt +++ b/mg_bt/CMakeLists.txt @@ -37,7 +37,7 @@ set(SOURCES add_executable(mg_bt_executor ${SOURCES}) -add_executable(mg_i2cnode i2cmodule/i2cnode.cpp) +# add_executable(mg_i2cnode i2cmodule/i2cnode.cpp) target_include_directories( mg_bt_executor @@ -46,14 +46,14 @@ target_include_directories( include ) -target_link_libraries(mg_i2cnode i2c) +# target_link_libraries(mg_i2cnode i2c) ament_target_dependencies(mg_bt_executor ${PACKAGE_DEPS}) -ament_target_dependencies(mg_i2cnode rclcpp mg_msgs) +# ament_target_dependencies(mg_i2cnode rclcpp mg_msgs) install( TARGETS mg_bt_executor - mg_i2cnode +# mg_i2cnode DESTINATION lib/${PROJECT_NAME} ) diff --git a/mg_bt/src/mg_tree_executor.cpp b/mg_bt/src/mg_tree_executor.cpp index 24d41d5..5950d3d 100644 --- a/mg_bt/src/mg_tree_executor.cpp +++ b/mg_bt/src/mg_tree_executor.cpp @@ -37,7 +37,7 @@ namespace mg { double theta; glm::vec2 pos; - auto tmsg = tf_buffer_->lookupTransform("odom", "base-link", tf2::TimePointZero); + auto tmsg = tf_buffer_->lookupTransform("odom", "base_footprint", tf2::TimePointZero); tf2::Transform t; tf2::convert(tmsg.transform, t); diff --git a/mg_control/assets/mg_base.urdf b/mg_control/assets/mg_base.urdf index 362d87d..c2b9faa 100644 --- a/mg_control/assets/mg_base.urdf +++ b/mg_control/assets/mg_base.urdf @@ -55,7 +55,7 @@ - + mg_control/MgStepperInterface diff --git a/mg_control/assets/mg_base_controllers.yaml b/mg_control/assets/mg_base_controllers.yaml index bc8e32c..73edb37 100644 --- a/mg_control/assets/mg_base_controllers.yaml +++ b/mg_control/assets/mg_base_controllers.yaml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 100 + update_rate: 50 diffdrive_controller: type: diff_drive_controller/DiffDriveController @@ -14,7 +14,7 @@ diffdrive_controller: enable_odom_tf: false odom_frame_id: odom_excpected - base_frame_id: base-link + base_frame_id: base_footprint open_loop: true diff --git a/mg_control/assets/mg_base_controllers_local.yaml b/mg_control/assets/mg_base_controllers_local.yaml index a423e51..fb0f31f 100644 --- a/mg_control/assets/mg_base_controllers_local.yaml +++ b/mg_control/assets/mg_base_controllers_local.yaml @@ -14,7 +14,7 @@ diffdrive_controller: enable_odom_tf: true odom_frame_id: odom - base_frame_id: base-link + base_frame_id: base_footprint open_loop: true diff --git a/mg_control/assets/mg_base_local.urdf b/mg_control/assets/mg_base_local.urdf index 95f38a5..baaec04 100644 --- a/mg_control/assets/mg_base_local.urdf +++ b/mg_control/assets/mg_base_local.urdf @@ -55,7 +55,7 @@ - + mock_components/GenericSystem true diff --git a/mg_control/assets/toid_general_params.yaml b/mg_control/assets/toid_general_params.yaml new file mode 100644 index 0000000..2a1ad41 --- /dev/null +++ b/mg_control/assets/toid_general_params.yaml @@ -0,0 +1,22 @@ +controller_manager: + ros__parameters: + update_rate: 50 + + diffdrive_controller: + type: diff_drive_controller/DiffDriveController + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +diffdrive_controller: + ros__parameters: + left_wheel_names: ["drivewhl_l_joint"] + right_wheel_names: ["drivewhl_r_joint"] + + enable_odom_tf: true + odom_frame_id: odom + base_frame_id: base_footprint + + open_loop: true + + wheel_separation: 0.258 + wheel_radius: 0.0375 \ No newline at end of file diff --git a/mg_control/launch/toid.launch.py b/mg_control/launch/toid.launch.py new file mode 100644 index 0000000..969a78c --- /dev/null +++ b/mg_control/launch/toid.launch.py @@ -0,0 +1,111 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import Command, LaunchConfiguration, IfElseSubstitution +from launch_ros.actions import Node, LifecycleNode +from launch_ros.substitutions import FindPackageShare +import os + +def generate_launch_description(): + pkg_share = FindPackageShare("").find('mg_control') + params = os.path.join(pkg_share, 'assets', 'toid_general_params.yaml') + default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'ros_control.rviz') + + + description_pkg_share = FindPackageShare("").find('toid_bot_description') + default_model_path = os.path.join( + description_pkg_share, + 'src', + 'toid_bot_description.urdf' + ) + + visualize = LaunchConfiguration("visualize") + + visualize_arg = DeclareLaunchArgument( + 'visualize', + default_value='False', + description="Whether to launch rviz2" + ) + + use_mock = LaunchConfiguration("use_mock") + + use_mock_arg = DeclareLaunchArgument( + 'use_mock', + default_value='True', + description="Whether to use mock controller" + ) + + odom_broadcast = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='map_to_odom_broadcaster', + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'], + condition=IfCondition(LaunchConfiguration('visualize')) + ) + + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{'robot_description': Command(['xacro ', default_model_path, ' use_sim:=', use_mock])}] + ) + + controller_manager = Node( + package='controller_manager', + executable='ros2_control_node', + output='screen', + parameters=[params] + ) + + joint_state_broadcaster = Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=["joint_state_broadcaster"] + ) + + diffbot_base_controller = Node( + package='controller_manager', + executable='spawner', + output='both', + arguments=[ + "diffdrive_controller", + "-p", + params, + "--controller-ros-args", + "-r diffdrive_controller/cmd_vel:=/cmd_vel", + "--controller-ros-args", + "-r diffdrive_controller/odom:=/odom", + "--controller-ros-args", + IfElseSubstitution(use_mock, + "--param odom_frame_id:=odom", + "--param odom_frame_id:=odom_expected" + ), + "--controller-ros-args", + IfElseSubstitution(use_mock, + "--param enable_odom_tf:=true", + "--param enable_odom_tf:=false" + ), + ] + ) + + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', default_rviz_config_path], + condition=IfCondition(visualize) + ) + + return LaunchDescription([ + visualize_arg, + use_mock_arg, + odom_broadcast, + robot_state_publisher, + controller_manager, + joint_state_broadcaster, + diffbot_base_controller, + rviz_node + ]) \ No newline at end of file diff --git a/mg_lidar/config/lidar.yaml b/mg_lidar/config/lidar.yaml index d7bec53..c810329 100644 --- a/mg_lidar/config/lidar.yaml +++ b/mg_lidar/config/lidar.yaml @@ -1,4 +1,4 @@ rplidar_node: ros__parameters: scan_mode: "ppbig" - topic_name: "base-link" \ No newline at end of file + topic_name: "base_link" \ No newline at end of file diff --git a/mg_lidar/src/scanner.cpp b/mg_lidar/src/scanner.cpp index 5c8d3a5..3962d9f 100644 --- a/mg_lidar/src/scanner.cpp +++ b/mg_lidar/src/scanner.cpp @@ -54,7 +54,7 @@ class MgScanner : public rclcpp::Node { double y = NAN; double rot = NAN; - auto tmsg = tf_buf_->lookupTransform("odom", "base-link", tf2::TimePointZero); + auto tmsg = tf_buf_->lookupTransform("odom", "base_footprint", tf2::TimePointZero); tf2::Transform t; tf2::convert(tmsg.transform, t); diff --git a/mg_navigation/include/handlers/dwa_core.hpp b/mg_navigation/include/handlers/dwa_core.hpp index 81edbe2..7ac9a56 100644 --- a/mg_navigation/include/handlers/dwa_core.hpp +++ b/mg_navigation/include/handlers/dwa_core.hpp @@ -152,7 +152,7 @@ namespace mg { double x = NAN; double y = NAN; - auto tmsg = tfbuf->lookupTransform("odom", "base-link", tf2::TimePointZero); + auto tmsg = tfbuf->lookupTransform("odom", "base_footprint", tf2::TimePointZero); tf2::Transform t; tf2::convert(tmsg.transform, t); diff --git a/mg_navigation/src/mg_navigation.cpp b/mg_navigation/src/mg_navigation.cpp index beaaa3f..9d14e49 100644 --- a/mg_navigation/src/mg_navigation.cpp +++ b/mg_navigation/src/mg_navigation.cpp @@ -51,7 +51,7 @@ namespace mg { path_buffer_ = std::make_shared(this); - pub_twist = create_publisher("diffdrive_controller/cmd_vel", 2); + pub_twist = create_publisher("/cmd_vel", 2); obstacle_manager_ = std::make_shared(this, tf2_buffer); diff --git a/mg_odometry/src/mg_odom_publisher.cpp b/mg_odometry/src/mg_odom_publisher.cpp index 32ecbfb..abdbed7 100644 --- a/mg_odometry/src/mg_odom_publisher.cpp +++ b/mg_odometry/src/mg_odom_publisher.cpp @@ -23,7 +23,7 @@ #define TIMEOUT 10u -constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM1"; +constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM0"; using SendDoubleSrv = mg_msgs::srv::SendDouble; using ZeroSrv = std_srvs::srv::Empty; @@ -32,7 +32,7 @@ class MgOdomPublisher : public rclcpp::Node { public: MgOdomPublisher() : Node("mg_odom_publisher") { this->declare_parameter("odom", "odom"); - this->declare_parameter("target", "base-link"); + this->declare_parameter("target", "base_footprint"); this->declare_parameter("serial_path", ENCODER_SERIAL_PATH_DEFAULT); tf_broadcaster_ = std::make_shared(this); diff --git a/mg_planner/src/mg_planner_node.cpp b/mg_planner/src/mg_planner_node.cpp index c9e95df..b5f33f2 100644 --- a/mg_planner/src/mg_planner_node.cpp +++ b/mg_planner/src/mg_planner_node.cpp @@ -51,7 +51,7 @@ mg::PlannerNode::PlannerNode() : Node("planner"), astar_(this) { glm::ivec2 mg::PlannerNode::get_pos() { try { - auto tmsg = tf_buf_->lookupTransform("odom", "base-link", tf2::TimePointZero); + auto tmsg = tf_buf_->lookupTransform("odom", "base_footprint", tf2::TimePointZero); tf2::Transform t; tf2::convert(tmsg.transform, t); diff --git a/toid_bot_description/src/toid_bot_control.xacro b/toid_bot_description/src/toid_bot_control.xacro index 32f2b6b..eac33b9 100644 --- a/toid_bot_description/src/toid_bot_control.xacro +++ b/toid_bot_description/src/toid_bot_control.xacro @@ -6,7 +6,7 @@ - + mg_control/MgStepperInterface