diff --git a/firmware/base/src/crc.h b/firmware/base/src/crc.h new file mode 100644 index 0000000..6f165a4 --- /dev/null +++ b/firmware/base/src/crc.h @@ -0,0 +1,47 @@ +#pragma once +#include +const unsigned char CRC_TABLE[256] = { + 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, + 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, + 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, + 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, + 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, + 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, + 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, + 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, + 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, + 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, + 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, + 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, + 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, + 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, + 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, + 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, + 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, + 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, + 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, + 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, + 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, + 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, + 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, + 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, + 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, + 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, + 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, + 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, + 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, + 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, + 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, + 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 +}; + +#define crcFooter(data_type) _crc((const uint8_t *) &(data_type), sizeof(data_type)-1) +#define crcNoFooter(data_type) _crc((const uint8_t *) &(data_type), sizeof(data_type)) + +inline unsigned char _crc(const uint8_t *data, unsigned int size) { + uint8_t crc = 0; + while(size--) { + crc = CRC_TABLE[ (crc ^ *(data++)) & 0xff]; + } + return crc; +} \ No newline at end of file diff --git a/firmware/base/src/main.c b/firmware/base/src/main.c index 0ef89f6..0258175 100644 --- a/firmware/base/src/main.c +++ b/firmware/base/src/main.c @@ -1,4 +1,6 @@ #include +#include +#include #include #include #include @@ -21,12 +23,18 @@ #include "config.h" #include "stepper.h" +#include "msgs.h" +#include "crc.h" + static substep_state_t state_l; static substep_state_t state_r; static double base_x = 0; static double base_y = 0; static double base_theta = 0; +static double base_vl = 0; +static double base_vr = 0; + static double wheel_separation = WHEEL_SEPARATION; static double wheel_ratio_l = 1; static double wheel_ratio_r = 1; @@ -80,6 +88,10 @@ bool update_pos_cb() { vel_l /=64 * ENCODER_CPR; vel_r /=64 * ENCODER_CPR; + + base_vl = vel_l; + base_vr = vel_r; + vel_l *= WHEEL_RADIUS * 2 * M_PI * calib_enc.left_gain; vel_r *= -WHEEL_RADIUS * 2 * M_PI * calib_enc.right_gain; @@ -142,45 +154,60 @@ void core2_entry() prev_time = time_us_32(); uint16_t cmd = 0; - uint64_t data = 0; uint readNum = 0; - char type = 'w'; + uint8_t cmd_id = 0; + tMsgBuf data; + uint16_t cmd_timeout = 100; + while (true) { uint ch; if(( ch = stdio_getchar_timeout_us(0)) != PICO_ERROR_TIMEOUT) { + cmd_timeout = 100; cmd = (cmd << 8) | ch; - if(readNum > 0) { - data = (data >> 8ll) | ((uint64_t)ch<<56ll); - readNum--; - if(!readNum) { - if(type == 'w') - wheel_separation = *((double*)&data); - else { - wheel_ratio_l = 1 + (*((double *)&data)); - wheel_ratio_r = 1 - (*((double *)&data)); + if(cmd_id > 0) { + if(push_tMsgBuf(&data, readNum++, ch,cmd_id)) { + if(cmd_id == TMSG_SET_WIDTH && crcFooter(data.msg_width) == data.msg_width.crc){ + wheel_separation = data.msg_width.width; + } else if (cmd_id == TMSG_SET_RATIO && crcFooter(data.msg_ratio) == data.msg_width.crc) { + wheel_ratio_l = 1 + data.msg_ratio.ratio; } + cmd_id = 0; + } + } else { + if(cmd == TCMD_ID(TMSG_REQ_STATE)) { + tRespState resp = { + base_x, + base_y, + base_theta, + base_vl, + base_vr, + }; + send_tRespState(&resp); + } else if(cmd == TCMD_ID(TMSG_SET_RATIO)) { + readNum = 0; + cmd_id = TMSG_SET_RATIO; + } else if(cmd == TCMD_ID(TMSG_SET_WIDTH)){ + readNum = 0; + cmd_id = TMSG_SET_WIDTH; + } else if(cmd == TCMD_ID(TMSG_ZERO)) { + zero(); + } else if(cmd == TCMD_ID(TMSG_END_CALIB)) { + end_calib(); + tRespEndCalib resp = { + calib_enc.left_gain, + calib_enc.right_gain + }; + send_tRespEndCalib(&resp); } } - if(cmd == (((uint16_t)'g' << 8) | ';')) { - printf("%lf %lf %lf\n", base_x, base_y, base_theta); - cmd = 0; - } else if(cmd == (((uint16_t)'w' << 8) | ';')) { - readNum = 8; - type = (cmd >> 8) & 0xFF; - } else if(cmd == (((uint16_t)'r' << 8) | ';')){ - readNum = 8; - type = (cmd >> 8) & 0xFF; - } else if(cmd == (((uint16_t)'z' << 8) | ';')) { - zero(); - } else if(cmd == (((uint16_t)'c' << 8) | ';')) { - end_calib(); - printf("%lf %lf\n", calib_enc.left_gain, calib_enc.right_gain); - } } update_pos_cb(); + if(!cmd_timeout--) { + cmd_id = 0; + } sleep_ms(1); } } @@ -247,7 +274,6 @@ int main() while (true) { if((c = stdio_getchar_timeout_us(0) ) != PICO_ERROR_TIMEOUT) { stepper_fifo(c); - stdio_putchar(c); } sem_acquire_blocking(&stdio_dual_usb_mutex); tud_task(); diff --git a/firmware/base/src/msgs.h b/firmware/base/src/msgs.h new file mode 100644 index 0000000..36624c2 --- /dev/null +++ b/firmware/base/src/msgs.h @@ -0,0 +1,92 @@ +#pragma once + +#include "crc.h" +#include "stdio.h" + +typedef enum tCmdId{ + TMSG_REQ_STATE = 0x7F, + TMSG_SET_WIDTH, + TMSG_SET_RATIO, + TMSG_END_CALIB, + TMSG_SET_SPEED, + TMSG_ZERO, + TMSG_DELIM, +} tCmdId; + +typedef struct tMsgRatio { + double ratio; + unsigned char crc; +}__attribute__((packed)) tMsgRatio; + +typedef struct tMsgWidth { + double width; + unsigned char crc; +}__attribute__((packed)) tMsgWidth; + +typedef struct tMsgSpeed { + double vl; + double vr; + unsigned char crc; +}__attribute__((packed)) tMsgSpeed; + +typedef struct tRespState { + double x; + double y; + double theta; + double vl; + double vr; + unsigned char crc; +}__attribute__((packed)) tRespState; + +typedef struct tRespEndCalib { + double left_gain; + double right_gain; + unsigned char crc; +}__attribute__((packed)) tRespEndCalib; + +typedef union tMsgBuf { + tMsgRatio msg_ratio; + tMsgWidth msg_width; + tMsgSpeed msg_speed; +}__attribute__((packed)) tMsgBuf; + +#define TCMD_ID(id) ((uint16_t)((id << 8) | TMSG_DELIM)) +#define TGET_CMD(cmd) ((cmd >> 8) & 0xFF) + +enum { + TSTATE_IDLE, + TSTATE_BUILD_MSG, +}; + +inline void send_tRespState(tRespState *resp) { + resp->crc = crcFooter(*resp); + fwrite(resp, 1, sizeof(tRespState), stdout); + fflush(stdout); +} + +inline void send_tRespEndCalib(tRespEndCalib *state) { + state->crc = crcFooter(*state); + fwrite(state, 1, sizeof(tRespEndCalib), stdout); + fflush(stdout); +} + +inline unsigned push_tMsgBuf(tMsgBuf *buf, unsigned cnt, unsigned char byte, tCmdId id) { + unsigned size = 0; + switch (id) { + case TMSG_SET_RATIO: + size = sizeof(tMsgRatio); + break; + case TMSG_SET_WIDTH: + size = sizeof(tMsgRatio); + break; + case TMSG_SET_SPEED: + size = sizeof(tMsgRatio); + break; + default: + return 0; + } + if(cnt < size) { + ((unsigned char*)buf)[cnt] = byte; + } + return cnt == size-1; +} \ No newline at end of file diff --git a/toid_bot_description/src/toid_bot_control.xacro b/toid_bot_description/src/toid_bot_control.xacro index 32f2b6b..4117934 100644 --- a/toid_bot_description/src/toid_bot_control.xacro +++ b/toid_bot_description/src/toid_bot_control.xacro @@ -6,7 +6,7 @@ - + toid_control/StepperInterface @@ -19,13 +19,11 @@ - - diff --git a/toid_control/CMakeLists.txt b/toid_control/CMakeLists.txt new file mode 100644 index 0000000..93201c2 --- /dev/null +++ b/toid_control/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 3.8) +project(toid_control) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(library_name toid_control) + +set(PACKAGE_DEPS + rclcpp + pluginlib + hardware_interface + rclcpp_lifecycle + rclcpp_components +) + +# find dependencies +find_package(ament_cmake REQUIRED) + +foreach(PACKAGE ${PACKAGE_DEPS}) + find_package(${PACKAGE} REQUIRED) +endforeach() + + +add_library( + ${library_name} + SHARED + src/toid_control.cpp +) + +target_include_directories( + ${library_name} + PRIVATE + include +) + +ament_target_dependencies(${library_name} + ${PACKAGE_DEPS} +) + +pluginlib_export_plugin_description_file(hardware_interface toid_control_interfaces.xml) + +install( + TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY + launch + params + rviz + DESTINATION share/${PROJECT_NAME}/ +) + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${PACKAGE_DEPS}) + +ament_package() diff --git a/toid_control/include/toid_control/toid_control.hpp b/toid_control/include/toid_control/toid_control.hpp new file mode 100644 index 0000000..95c7d3d --- /dev/null +++ b/toid_control/include/toid_control/toid_control.hpp @@ -0,0 +1,45 @@ +#ifndef MG_WHEEL_INTERFACE_HPP_ +#define MG_WHEEL_INTERFACE_HPP_ +#include +#include + +#include "boost/asio.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "pluginlib/class_list_macros.hpp" + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +namespace toid { + namespace asio = boost::asio; + + class StepperInterface : public hardware_interface::SystemInterface { + public: + + CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams& info) override; + CallbackReturn on_configure(const rclcpp_lifecycle::State&) override; + CallbackReturn on_shutdown(const rclcpp_lifecycle::State&) override; + + std::vector export_state_interfaces() override; + std::vector export_command_interfaces() override; + + hardware_interface::return_type read(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) override; + hardware_interface::return_type write(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) override; + + private: + std::string serial_port_name_; + asio::io_context io_context_; + asio::serial_port serial_port_ = asio::serial_port(io_context_); + + double left_wheel_vel_cmd_ = 0; + double left_wheel_pos_state_ = 0; + double right_wheel_vel_cmd_ = 0; + double right_wheel_pos_state_ = 0; + }; +} + +PLUGINLIB_EXPORT_CLASS(toid::StepperInterface, hardware_interface::SystemInterface) + +#endif \ No newline at end of file diff --git a/toid_control/launch/toid.launch.py b/toid_control/launch/toid.launch.py new file mode 100644 index 0000000..0a733f9 --- /dev/null +++ b/toid_control/launch/toid.launch.py @@ -0,0 +1,120 @@ +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('toid_control') + params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml') + default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'rviz.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')) + # ) + + odom_broadcast = Node( + package='toid_odometry', + executable='toid_odometry', + name='map_to_odom_broadcaster', + arguments={'use_mock': use_mock}, + 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_mock:=', 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 + ]) diff --git a/toid_control/package.xml b/toid_control/package.xml new file mode 100644 index 0000000..d96c227 --- /dev/null +++ b/toid_control/package.xml @@ -0,0 +1,35 @@ + + + + toid_control + 0.0.1 + Ros2 control compatible drivers for magrob + Pimpest + Apache-2.0 + + ament_cmake + + ros2launch + + rclcpp + rclcpp_lifecycle + rclcpp_components + pluginlib + + ros2_control + hardware_interface + + controller_manager + diff_drive_controller + robot_state_publisher + joint_state_broadcaster + + boost + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/toid_control/params/toid_general_params.yaml b/toid_control/params/toid_general_params.yaml new file mode 100644 index 0000000..78b6d8f --- /dev/null +++ b/toid_control/params/toid_general_params.yaml @@ -0,0 +1,22 @@ +controller_manager: + ros__parameters: + update_rate: 200 + + 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.192 + wheel_radius: 0.032 diff --git a/toid_control/rviz/rviz.rviz b/toid_control/rviz/rviz.rviz new file mode 100644 index 0000000..c4263d1 --- /dev/null +++ b/toid_control/rviz/rviz.rviz @@ -0,0 +1,194 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 1144 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + drivewhl_l_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + drivewhl_r_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_caster: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lidar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_caster: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.1797966957092285 + Target Frame: + Value: Orbit (rviz) + Yaw: 6.1635823249816895 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1662 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001f000000574fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000070000005740000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015b00000574fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000070000005740000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000b400000005efc0100000002fb0000000800540069006d0065010000000000000b400000048700fffffffb0000000800540069006d00650100000000000004500000000000000000000007dd0000057400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2880 + X: 0 + Y: 64 diff --git a/toid_control/src/toid_control.cpp b/toid_control/src/toid_control.cpp new file mode 100644 index 0000000..af6f261 --- /dev/null +++ b/toid_control/src/toid_control.cpp @@ -0,0 +1,87 @@ +#include "toid_control/toid_control.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +#include + +CallbackReturn toid::StepperInterface::on_init(const hardware_interface::HardwareComponentInterfaceParams& info) { + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { + return CallbackReturn::ERROR; + } + + if (info_.hardware_parameters.find("device_path") != info.hardware_info.hardware_parameters.end()) { + serial_port_name_ = info_.hardware_parameters["device_path"]; + } else { + serial_port_name_ = "/dev/ttyACM0"; + } + + left_wheel_pos_state_ = 0; + right_wheel_pos_state_ = 0; + left_wheel_vel_cmd_ = 0; + right_wheel_vel_cmd_ = 0; + return CallbackReturn::SUCCESS; +} + +CallbackReturn toid::StepperInterface::on_configure(const rclcpp_lifecycle::State&) { + try { + serial_port_.open(serial_port_name_); + } catch (const std::exception& er) { + RCLCPP_ERROR(rclcpp::get_logger("StepperInterface"), + "Failed to open serial port (Is the stepper driver plugged in)"); + } + return CallbackReturn::SUCCESS; +} + +CallbackReturn toid::StepperInterface::on_shutdown(const rclcpp_lifecycle::State&) { + if (serial_port_.is_open()) { + boost::system::error_code ec; + ec = serial_port_.close(ec); + } + return CallbackReturn::SUCCESS; +} + +std::vector toid::StepperInterface::export_state_interfaces() { + std::vector state_interfaces; + state_interfaces.reserve(2); + state_interfaces.emplace_back("drivewhl_l_joint", hardware_interface::HW_IF_POSITION, &left_wheel_pos_state_); + state_interfaces.emplace_back("drivewhl_r_joint", hardware_interface::HW_IF_POSITION, &right_wheel_pos_state_); + + return state_interfaces; +} + +std::vector toid::StepperInterface::export_command_interfaces() { + std::vector command_interfaces; + command_interfaces.reserve(2); + command_interfaces.emplace_back("drivewhl_l_joint", hardware_interface::HW_IF_VELOCITY, &left_wheel_vel_cmd_); + command_interfaces.emplace_back("drivewhl_r_joint", hardware_interface::HW_IF_VELOCITY, &right_wheel_vel_cmd_); + + return command_interfaces; +} + +hardware_interface::return_type toid::StepperInterface::read(const rclcpp::Time&, const rclcpp::Duration&) { + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type toid::StepperInterface::write(const rclcpp::Time&, const rclcpp::Duration&) { + using namespace asio::buffer_literals; + + try { + double values[] = { + -left_wheel_vel_cmd_ / (2 * M_PI), + right_wheel_vel_cmd_ / (2 * M_PI), + }; + asio::write(serial_port_, "s;"_buf); + asio::write(serial_port_, asio::buffer(values, sizeof(values))); + } catch (const std::runtime_error& e) { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), + *this->get_clock(), + 5000, + "Failed to write to serial port: %s", + e.what() + ); + boost::system::error_code ec; + ec = serial_port_.close(ec); + ec = serial_port_.open(serial_port_name_, ec); + } + return hardware_interface::return_type::OK; +} diff --git a/toid_control/toid_control_interfaces.xml b/toid_control/toid_control_interfaces.xml new file mode 100644 index 0000000..9f1a7c9 --- /dev/null +++ b/toid_control/toid_control_interfaces.xml @@ -0,0 +1,7 @@ + + + \ No newline at end of file diff --git a/toid_msgs/CMakeLists.txt b/toid_msgs/CMakeLists.txt new file mode 100644 index 0000000..f35ea38 --- /dev/null +++ b/toid_msgs/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(toid_msgs) + +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) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/SendDouble.srv" +) + +ament_package() \ No newline at end of file diff --git a/toid_msgs/package.xml b/toid_msgs/package.xml new file mode 100644 index 0000000..dc19523 --- /dev/null +++ b/toid_msgs/package.xml @@ -0,0 +1,19 @@ + + + + toid_msgs + 0.0.0 + This contains various msg/action used within the project + Pimpest + MIT + + ament_cmake + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/toid_msgs/srv/SendDouble.srv b/toid_msgs/srv/SendDouble.srv new file mode 100644 index 0000000..3368ac0 --- /dev/null +++ b/toid_msgs/srv/SendDouble.srv @@ -0,0 +1,2 @@ +float64 data +--- \ No newline at end of file diff --git a/toid_odometry/CMakeLists.txt b/toid_odometry/CMakeLists.txt new file mode 100644 index 0000000..9ee98c3 --- /dev/null +++ b/toid_odometry/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 3.8) +project(toid_odometry) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +set(PACKAGE_DEPS + rclcpp + Boost + std_srvs + tf2 + tf2_ros + tf2_geometry_msgs + toid_msgs +) + +foreach(PACKAGE ${PACKAGE_DEPS}) + find_package(${PACKAGE} REQUIRED) +endforeach() + +set(SOURCES + src/odometry.cpp +) + +add_executable(toid_odometry ${SOURCES}) + +target_include_directories( + toid_odometry + PRIVATE + include +) + +ament_target_dependencies(toid_odometry ${PACKAGE_DEPS}) + +install(TARGETS toid_odometry DESTINATION lib/${PROJECT_NAME}) + +target_compile_features( + toid_odometry PUBLIC + c_std_99 + cxx_std_17 +) + + +ament_package() diff --git a/toid_odometry/include/toid_odometry/crc.h b/toid_odometry/include/toid_odometry/crc.h new file mode 100644 index 0000000..6f165a4 --- /dev/null +++ b/toid_odometry/include/toid_odometry/crc.h @@ -0,0 +1,47 @@ +#pragma once +#include +const unsigned char CRC_TABLE[256] = { + 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, + 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, + 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, + 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, + 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, + 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, + 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, + 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, + 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, + 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, + 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, + 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, + 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, + 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, + 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, + 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, + 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, + 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, + 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, + 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, + 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, + 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, + 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, + 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, + 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, + 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, + 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, + 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, + 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, + 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, + 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, + 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 +}; + +#define crcFooter(data_type) _crc((const uint8_t *) &(data_type), sizeof(data_type)-1) +#define crcNoFooter(data_type) _crc((const uint8_t *) &(data_type), sizeof(data_type)) + +inline unsigned char _crc(const uint8_t *data, unsigned int size) { + uint8_t crc = 0; + while(size--) { + crc = CRC_TABLE[ (crc ^ *(data++)) & 0xff]; + } + return crc; +} \ No newline at end of file diff --git a/toid_odometry/include/toid_odometry/msgs.h b/toid_odometry/include/toid_odometry/msgs.h new file mode 100644 index 0000000..3c04edb --- /dev/null +++ b/toid_odometry/include/toid_odometry/msgs.h @@ -0,0 +1,79 @@ +#pragma once + +#include "crc.h" + +typedef enum tCmdId { + TMSG_REQ_STATE = 0x7F, + TMSG_SET_WIDTH, + TMSG_SET_RATIO, + TMSG_END_CALIB, + TMSG_SET_SPEED, + TMSG_ZERO, + TMSG_DELIM, +} tCmdId; + +typedef struct tMsgRatio { + double ratio; + unsigned char crc; +}__attribute__((packed)) tMsgRatio; + +typedef struct tMsgWidth { + double width; + unsigned char crc; +}__attribute__((packed)) tMsgWidth; + +typedef struct tMsgSpeed { + double vl; + double vr; + unsigned char crc; +}__attribute__((packed)) tMsgSpeed; + +typedef struct tRespState { + double x; + double y; + double theta; + double vl; + double vr; + unsigned char crc; +}__attribute__((packed)) tRespState; + +typedef struct tRespEndCalib { + double left_gain; + double right_gain; + unsigned char crc; +}__attribute__((packed)) tRespEndCalib; + +typedef union tMsgBuf { + tMsgRatio msg_ratio; + tMsgWidth msg_width; + tMsgSpeed msg_speed; +}__attribute__((packed)) tMsgBuf; + +#define TCMD_ID(id) ((uint16_t)(id << 8 | TMSG_DELIM)) +#define TGET_CMD(cmd) ((cmd >> 8) & 0xFF) + +enum { + TSTATE_IDLE, + TSTATE_BUILD_MSG, +}; + +inline unsigned push_tMsgBuf(tMsgBuf *buf, unsigned cnt, unsigned char byte, tCmdId id) { + unsigned size = 0; + switch (id) { + case TMSG_SET_RATIO: + size = sizeof(tMsgRatio); + break; + case TMSG_SET_WIDTH: + size = sizeof(tMsgRatio); + break; + case TMSG_SET_SPEED: + size = sizeof(tMsgRatio); + break; + default: + return 0; + } + if(cnt < size) { + ((unsigned char*)buf)[cnt] = byte; + } + return cnt == size-1; +} \ No newline at end of file diff --git a/toid_odometry/package.xml b/toid_odometry/package.xml new file mode 100644 index 0000000..41b160c --- /dev/null +++ b/toid_odometry/package.xml @@ -0,0 +1,26 @@ + + + + toid_odometry + 0.0.0 + Odometry publisher for robotoid + pimpest + MIT + + ament_cmake + + rclcpp + boost + std_srvs + tf2 + tf2_ros + tf2_geometry_msgs + toid_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/toid_odometry/src/odometry.cpp b/toid_odometry/src/odometry.cpp new file mode 100644 index 0000000..55d6d88 --- /dev/null +++ b/toid_odometry/src/odometry.cpp @@ -0,0 +1,211 @@ +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "toid_odometry/msgs.h" +#include "toid_odometry/crc.h" + +#include "toid_msgs/srv/send_double.hpp" +#include "std_srvs/srv/empty.hpp" + +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/static_transform_broadcaster.hpp" +#include "tf2/LinearMath/Transform.hpp" + +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/convert.hpp" + + +namespace asio = boost::asio; + +class ToidOdomNode : public rclcpp::Node { + using SendDoubleSrv = toid_msgs::srv::SendDouble; + using ZeroSrv = std_srvs::srv::Empty; +public: + ToidOdomNode() : rclcpp::Node("OdometryNode"), serial_(io_ctx_) { + using std::placeholders::_1; + this->declare_parameter("base_frame", "base_footprint"); + this->declare_parameter("odometry_frame", "odom"); + this->declare_parameter("map_frame", "map"); + this->declare_parameter("serial_port", "/dev/ttyACM1"); + + this->declare_parameter("initial_pose", "0 0 0"); + this->declare_parameter("mock_odom", false); + + odometry_frame_id_ = this->get_parameter("odometry_frame").as_string(); + base_frame_id_ = this->get_parameter("base_frame").as_string(); + map_frame_id_ = this->get_parameter("map_frame").as_string(); + serial_port_path_ = this->get_parameter("serial_port").as_string(); + + tf_broadcaster_ = std::make_shared(this); + tf_static_broadcaster_ = std::make_shared(this); + geometry_msgs::msg::TransformStamped t; + t.header.stamp = this->get_clock()->now(); + t.header.frame_id = map_frame_id_; + t.child_frame_id = odometry_frame_id_; + t.transform = this->make_transform(0,0,0); + tf_static_broadcaster_->sendTransform(t); + + startpose_listener_ = this->create_subscription( + "/initialpose", 1, std::bind(&ToidOdomNode::set_pose,this,_1) + ); + + if(!this->get_parameter("mock_odom").as_bool()) { + timer_ = this->create_wall_timer( + std::chrono::seconds(1/100), + [this]{this->publish_robot_state();}); + + set_width_service_ = create_service ( + "/set_width", std::bind(&ToidOdomNode::set_width, this, _1)); + + set_ratio_service_ = create_service ( + "/set_ratio", std::bind(&ToidOdomNode::set_ratio, this, _1)); + + calib_service_ = create_service ( + "/end_calib", std::bind(&ToidOdomNode::end_calib, this, _1)); + + zero_service_ = create_service ( + "/zero", std::bind(&ToidOdomNode::zero, this, _1)); + } + + } +private: + + void set_pose(const geometry_msgs::msg::PoseWithCovarianceStamped &pose) { + geometry_msgs::msg::TransformStamped t; + + t.header.stamp = this->get_clock()->now(); + t.header.frame_id = map_frame_id_; + t.child_frame_id = odometry_frame_id_; + + t.transform.rotation = pose.pose.pose.orientation; + auto[x,y,z] =pose.pose.pose.position; + t.transform.translation.x = x; + t.transform.translation.y = y; + t.transform.translation.z = z; + + RCLCPP_INFO(this->get_logger(), "Setting new initial_pose"); + + tf_static_broadcaster_->sendTransform(t); + } + + void set_width(const std::shared_ptr req) { + boost::system::error_code ec; + const std::array cmd{TMSG_SET_WIDTH, TMSG_DELIM}; + tMsgWidth msg = { req->data, 0}; + msg.crc = crcFooter(msg); + asio::write(this->serial_, asio::buffer(cmd), ec); + asio::write(this->serial_, asio::buffer(&msg, sizeof(msg)), ec); + } + + void set_ratio(const std::shared_ptr req) { + boost::system::error_code ec; + const std::array cmd{TMSG_SET_RATIO, TMSG_DELIM}; + tMsgRatio msg = { req->data, 0 }; + msg.crc = crcFooter(msg); + asio::write(this->serial_, asio::buffer(cmd), ec); + asio::write(this->serial_, asio::buffer(&msg, sizeof(msg)), ec); + } + + void end_calib(const std::shared_ptr) { + boost::system::error_code ec; + const std::array cmd{TMSG_END_CALIB, TMSG_DELIM}; + asio::write(this->serial_, asio::buffer(cmd), ec); + tRespEndCalib resp; + asio::read(this->serial_, asio::buffer(&resp, sizeof(resp)) , ec); + if(!ec.failed()) { + RCLCPP_INFO(this->get_logger(), "Calculated ratio: %lf, %lf", resp.left_gain, resp.right_gain); + } + } + + void zero(const std::shared_ptr) { + boost::system::error_code ec; + const std::array cmd{TMSG_ZERO, TMSG_DELIM}; + asio::write(this->serial_, asio::buffer(cmd), ec); + } + + void publish_robot_state() { + tRespState state; + if(!read_robot_state(state)) { + return; + } + send_transform(state); + } + + geometry_msgs::msg::Transform make_transform(double x, double y, double theta){ + geometry_msgs::msg::Transform t; + tf2::Quaternion q; + q.setRPY(0, 0, theta); + const tf2::Transform tf(q, tf2::Vector3(x, y, 0)); + tf2::convert(tf, t); + return t; + } + + void send_transform(tRespState &state) { + geometry_msgs::msg::TransformStamped t; + + t.header.stamp = this->get_clock()->now(); + t.header.frame_id = this->odometry_frame_id_; + t.child_frame_id = this->base_frame_id_; + + t.transform = make_transform(state.x, state.y, state.theta); + + tf_broadcaster_->sendTransform(t); + } + + bool read_robot_state(tRespState &state) { + const std::array cmd{TMSG_REQ_STATE, TMSG_DELIM}; + try { + asio::write(this->serial_, asio::buffer(cmd)); + asio::read(this->serial_, asio::buffer(&state, sizeof(state))); + } catch (const std::exception &e){ + RCLCPP_ERROR_SKIPFIRST_THROTTLE( + this->get_logger(), + *this->get_clock(), + 5000, + "%s", + e.what() + ); + boost::system::error_code ec; + if(serial_.close(ec)) {} + if(serial_.open(serial_port_path_,ec)) { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), + *this->get_clock(), + 5000, + "%s", + ec.what().c_str() + ); + } + return false; + } + return state.crc == crcFooter(state); + } +private: + asio::io_context io_ctx_; + asio::serial_port serial_; + rclcpp::TimerBase::SharedPtr timer_; + + std::string odometry_frame_id_; + std::string base_frame_id_; + std::string map_frame_id_; + std::string serial_port_path_; + + std::shared_ptr tf_broadcaster_; + std::shared_ptr tf_static_broadcaster_; + rclcpp::Service::SharedPtr set_width_service_; + rclcpp::Service::SharedPtr set_ratio_service_; + rclcpp::Service::SharedPtr calib_service_; + rclcpp::Service::SharedPtr zero_service_; + + rclcpp::Subscription::SharedPtr startpose_listener_; +}; + +int main(const int argc,const char** argv) { + rclcpp::init(argc,argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file