Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Ported over odometry and control using new serial communication

Browse Source
This commit is contained in:
Pimpest
2026-02-08 00:49:45 +01:00
parent 05e7398731
commit fc5fecdfc1
20 changed files with 1213 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
firmware/base/src/crc.h Normal file
View File

@@ -0,0 +1,47 @@
#pragma once
#include <stdint.h>
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;
}

80
firmware/base/src/main.c
View File

@@ -1,4 +1,6 @@
#include <pico/stdio.h> #include <pico/stdio.h>
#include <pico/types.h>
#include <stdint.h>
#include <stdio.h> #include <stdio.h>
#include <math.h> #include <math.h>
#include <string.h> #include <string.h>
@@ -21,12 +23,18 @@
#include "config.h" #include "config.h"
#include "stepper.h" #include "stepper.h"
#include "msgs.h"
#include "crc.h"
static substep_state_t state_l; static substep_state_t state_l;
static substep_state_t state_r; static substep_state_t state_r;
static double base_x = 0; static double base_x = 0;
static double base_y = 0; static double base_y = 0;
static double base_theta = 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_separation = WHEEL_SEPARATION;
static double wheel_ratio_l = 1; static double wheel_ratio_l = 1;
static double wheel_ratio_r = 1; static double wheel_ratio_r = 1;
@@ -80,6 +88,10 @@ bool update_pos_cb() {
vel_l /=64 * ENCODER_CPR; vel_l /=64 * ENCODER_CPR;
vel_r /=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_l *= WHEEL_RADIUS * 2 * M_PI * calib_enc.left_gain;
vel_r *= -WHEEL_RADIUS * 2 * M_PI * calib_enc.right_gain; vel_r *= -WHEEL_RADIUS * 2 * M_PI * calib_enc.right_gain;
@@ -142,45 +154,60 @@ void core2_entry()
prev_time = time_us_32(); prev_time = time_us_32();
uint16_t cmd = 0; uint16_t cmd = 0;
uint64_t data = 0;
uint readNum = 0; uint readNum = 0;
char type = 'w'; uint8_t cmd_id = 0;
tMsgBuf data;
uint16_t cmd_timeout = 100;
while (true) { while (true) {
uint ch; uint ch;
if(( ch = stdio_getchar_timeout_us(0)) != PICO_ERROR_TIMEOUT) { if(( ch = stdio_getchar_timeout_us(0)) != PICO_ERROR_TIMEOUT) {
cmd_timeout = 100;
cmd = (cmd << 8) | ch; cmd = (cmd << 8) | ch;
if(readNum > 0) { if(cmd_id > 0) {
data = (data >> 8ll) | ((uint64_t)ch<<56ll); if(push_tMsgBuf(&data, readNum++, ch,cmd_id)) {
readNum--; if(cmd_id == TMSG_SET_WIDTH && crcFooter(data.msg_width) == data.msg_width.crc){
if(!readNum) { wheel_separation = data.msg_width.width;
if(type == 'w') } else if (cmd_id == TMSG_SET_RATIO && crcFooter(data.msg_ratio) == data.msg_width.crc) {
wheel_separation = *((double*)&data); wheel_ratio_l = 1 + data.msg_ratio.ratio;
else {
wheel_ratio_l = 1 + (*((double *)&data));
wheel_ratio_r = 1 - (*((double *)&data));
} }
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(); update_pos_cb();
if(!cmd_timeout--) {
cmd_id = 0;
}
sleep_ms(1); sleep_ms(1);
} }
} }
@@ -247,7 +274,6 @@ int main()
while (true) { while (true) {
if((c = stdio_getchar_timeout_us(0) ) != PICO_ERROR_TIMEOUT) { if((c = stdio_getchar_timeout_us(0) ) != PICO_ERROR_TIMEOUT) {
stepper_fifo(c); stepper_fifo(c);
stdio_putchar(c);
} }
sem_acquire_blocking(&stdio_dual_usb_mutex); sem_acquire_blocking(&stdio_dual_usb_mutex);
tud_task(); tud_task();

92
firmware/base/src/msgs.h Normal file
View File

@@ -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;
}

4
toid_bot_description/src/toid_bot_control.xacro
View File

@@ -6,7 +6,7 @@
<ros2_control name="${name}" type="system"> <ros2_control name="${name}" type="system">
<xacro:unless value="${use_mock_hardware}"> <xacro:unless value="${use_mock_hardware}">
<hardware> <hardware>
<!-- plugin would eventually go here--> <plugin>toid_control/StepperInterface</plugin>
</hardware> </hardware>
</xacro:unless> </xacro:unless>
<xacro:if value="${use_mock_hardware}"> <xacro:if value="${use_mock_hardware}">
@@ -19,13 +19,11 @@
<joint name="drivewhl_l_joint"> <joint name="drivewhl_l_joint">
<command_interface name="velocity"/> <command_interface name="velocity"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/>
</joint> </joint>
<joint name="drivewhl_r_joint"> <joint name="drivewhl_r_joint">
<command_interface name="velocity"/> <command_interface name="velocity"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/>
</joint> </joint>
</ros2_control> </ros2_control>

63
toid_control/CMakeLists.txt Normal file
View File

@@ -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()

45
toid_control/include/toid_control/toid_control.hpp Normal file
View File

@@ -0,0 +1,45 @@
#ifndef MG_WHEEL_INTERFACE_HPP_
#define MG_WHEEL_INTERFACE_HPP_
#include <vector>
#include <string>
#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<hardware_interface::StateInterface> export_state_interfaces() override;
std::vector<hardware_interface::CommandInterface> 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

120
toid_control/launch/toid.launch.py Normal file
View File

@@ -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
])

35
toid_control/package.xml Normal file
View File

@@ -0,0 +1,35 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>toid_control</name>
<version>0.0.1</version>
<description>Ros2 control compatible drivers for magrob</description>
<maintainer email="82343504+Pimpest@users.noreply.github.com">Pimpest</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>ros2launch</depend>
<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>
<depend>rclcpp_components</depend>
<depend>pluginlib </depend>
<depend>ros2_control</depend>
<depend>hardware_interface</depend>
<depend>controller_manager</depend>
<depend>diff_drive_controller</depend>
<depend>robot_state_publisher</depend>
<depend>joint_state_broadcaster</depend>
<depend>boost</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

22
toid_control/params/toid_general_params.yaml Normal file
View File

@@ -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

194
toid_control/rviz/rviz.rviz Normal file
View File

@@ -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: <Fixed 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: <Fixed 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

87
toid_control/src/toid_control.cpp Normal file
View File

@@ -0,0 +1,87 @@
#include "toid_control/toid_control.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include <cmath>
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<hardware_interface::StateInterface> toid::StepperInterface::export_state_interfaces() {
std::vector<hardware_interface::StateInterface> 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<hardware_interface::CommandInterface> toid::StepperInterface::export_command_interfaces() {
std::vector<hardware_interface::CommandInterface> 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;
}

7
toid_control/toid_control_interfaces.xml Normal file
View File

@@ -0,0 +1,7 @@
<library path="toid_control">
<class
name="toid_control/StepperInterface"
type="toid::StepperInterface"
base_class_type="hardware_interface::SystemInterface"
/>
</library>

16
toid_msgs/CMakeLists.txt Normal file
View File

@@ -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()

19
toid_msgs/package.xml Normal file
View File

@@ -0,0 +1,19 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>toid_msgs</name>
<version>0.0.0</version>
<description>This contains various msg/action used within the project</description>
<maintainer email="82343504+Pimpest@users.noreply.github.com">Pimpest</maintainer>
<license>MIT</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

2
toid_msgs/srv/SendDouble.srv Normal file
View File

@@ -0,0 +1,2 @@
float64 data
---

47
toid_odometry/CMakeLists.txt Normal file
View File

@@ -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()

47
toid_odometry/include/toid_odometry/crc.h Normal file
View File

@@ -0,0 +1,47 @@
#pragma once
#include <stdint.h>
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;
}

79
toid_odometry/include/toid_odometry/msgs.h Normal file
View File

@@ -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;
}

26
toid_odometry/package.xml Normal file
View File

@@ -0,0 +1,26 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>toid_odometry</name>
<version>0.0.0</version>
<description>Odometry publisher for robotoid</description>
<maintainer email="82343504+pimpest@users.noreply.github.com">pimpest</maintainer>
<license>MIT</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>boost</depend>
<depend>std_srvs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<depend>toid_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

211
toid_odometry/src/odometry.cpp Normal file
View File

@@ -0,0 +1,211 @@
#include <iostream>
#include <chrono>
#include <boost/asio.hpp>
#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<tf2_ros::TransformBroadcaster>(this);
tf_static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(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<geometry_msgs::msg::PoseWithCovarianceStamped>(
"/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<SendDoubleSrv> (
"/set_width", std::bind(&ToidOdomNode::set_width, this, _1));
set_ratio_service_ = create_service<SendDoubleSrv> (
"/set_ratio", std::bind(&ToidOdomNode::set_ratio, this, _1));
calib_service_ = create_service<ZeroSrv> (
"/end_calib", std::bind(&ToidOdomNode::end_calib, this, _1));
zero_service_ = create_service<ZeroSrv> (
"/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<SendDoubleSrv::Request> req) {
boost::system::error_code ec;
const std::array<uint8_t, 2> 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<SendDoubleSrv::Request> req) {
boost::system::error_code ec;
const std::array<uint8_t, 2> 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<ZeroSrv::Request>) {
boost::system::error_code ec;
const std::array<uint8_t, 2> 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<ZeroSrv::Request>) {
boost::system::error_code ec;
const std::array<uint8_t, 2> 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<uint8_t, 2> 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<tf2_ros::TransformBroadcaster> tf_broadcaster_;
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster_;
rclcpp::Service<SendDoubleSrv>::SharedPtr set_width_service_;
rclcpp::Service<SendDoubleSrv>::SharedPtr set_ratio_service_;
rclcpp::Service<ZeroSrv>::SharedPtr calib_service_;
rclcpp::Service<ZeroSrv>::SharedPtr zero_service_;
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr startpose_listener_;
};
int main(const int argc,const char** argv) {
rclcpp::init(argc,argv);
rclcpp::spin(std::make_shared<ToidOdomNode>());
rclcpp::shutdown();
return 0;
}