From f4c70578fde72e8463f4935635ee487287613af8 Mon Sep 17 00:00:00 2001 From: ErikParkerrr Date: Tue, 17 Sep 2024 06:47:47 -0600 Subject: [PATCH 1/5] fixed mock hardware for botwheel explorer --- .../description/urdf/diffbot.ros2_control.xacro | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/odrive_botwheel_explorer/description/urdf/diffbot.ros2_control.xacro b/odrive_botwheel_explorer/description/urdf/diffbot.ros2_control.xacro index 2ba4352..a253a06 100644 --- a/odrive_botwheel_explorer/description/urdf/diffbot.ros2_control.xacro +++ b/odrive_botwheel_explorer/description/urdf/diffbot.ros2_control.xacro @@ -18,12 +18,25 @@ 0 + + + + + + 1 + + + + + + + - \ No newline at end of file + From c6877bb9938412a1ccbea36ef11633fe633cc00a Mon Sep 17 00:00:00 2001 From: "Libor Wagner (wagnelib)" Date: Fri, 15 Mar 2024 14:41:46 +0100 Subject: [PATCH 2/5] Add service to clear errors Co-authored-by: Libor Wagner (wagnelib) --- odrive_node/CMakeLists.txt | 2 ++ odrive_node/README.md | 4 ++++ odrive_node/include/odrive_can_node.hpp | 7 +++++++ odrive_node/package.xml | 1 + odrive_node/src/odrive_can_node.cpp | 21 +++++++++++++++++++++ 5 files changed, 35 insertions(+) diff --git a/odrive_node/CMakeLists.txt b/odrive_node/CMakeLists.txt index e074883..3bfda36 100644 --- a/odrive_node/CMakeLists.txt +++ b/odrive_node/CMakeLists.txt @@ -9,6 +9,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rosidl_default_generators REQUIRED) +find_package(std_srvs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/ControlMessage.msg" @@ -30,6 +31,7 @@ add_executable(odrive_can_node ament_target_dependencies(odrive_can_node rclcpp + std_srvs ) target_compile_features(odrive_can_node PRIVATE cxx_std_20) diff --git a/odrive_node/README.md b/odrive_node/README.md index b087a1c..674934c 100644 --- a/odrive_node/README.md +++ b/odrive_node/README.md @@ -50,6 +50,10 @@ For information about installation, prerequisites and getting started, check out This service requires regular heartbeat messages from the ODrive to determine the procedure result and will block until the procedure completes, with a minimum call time of 1 second. +* `/clear_errors`: Clears disarm_reason and procedure_result and re-arms the brake resistor if applicable + + If the axis dropped into IDLE because of an error, clearing the errors does not put the axis back into CLOSED_LOOP_CONTROL. To do so, you must request CLOSED_LOOP_CONTROL again explicitly. + ### Data Types All of the Message/Service fields are directly related to their corresponding CAN message. For more detailed information about each type, and how to interpet the data, please refer to the [ODrive CAN protocol documentation](https://docs.odriverobotics.com/v/latest/manual/can-protocol.html#messages). diff --git a/odrive_node/include/odrive_can_node.hpp b/odrive_node/include/odrive_can_node.hpp index b151c8b..0b9afdf 100644 --- a/odrive_node/include/odrive_can_node.hpp +++ b/odrive_node/include/odrive_can_node.hpp @@ -6,6 +6,7 @@ #include "odrive_can/msg/controller_status.hpp" #include "odrive_can/msg/control_message.hpp" #include "odrive_can/srv/axis_state.hpp" +#include "std_srvs/srv/empty.hpp" #include "socket_can.hpp" #include @@ -23,6 +24,7 @@ using ControllerStatus = odrive_can::msg::ControllerStatus; using ControlMessage = odrive_can::msg::ControlMessage; using AxisState = odrive_can::srv::AxisState; +using Empty = std_srvs::srv::Empty; class ODriveCanNode : public rclcpp::Node { public: @@ -33,7 +35,9 @@ class ODriveCanNode : public rclcpp::Node { void recv_callback(const can_frame& frame); void subscriber_callback(const ControlMessage::SharedPtr msg); void service_callback(const std::shared_ptr request, std::shared_ptr response); + void service_clear_errors_callback(const std::shared_ptr request, std::shared_ptr response); void request_state_callback(); + void request_clear_errors_callback(); void ctrl_msg_callback(); inline bool verify_length(const std::string&name, uint8_t expected, uint8_t length); @@ -61,6 +65,9 @@ class ODriveCanNode : public rclcpp::Node { std::condition_variable fresh_heartbeat_; rclcpp::Service::SharedPtr service_; + EpollEvent srv_clear_errors_evt_; + rclcpp::Service::SharedPtr service_clear_errors_; + }; #endif // ODRIVE_CAN_NODE_HPP diff --git a/odrive_node/package.xml b/odrive_node/package.xml index c174ae8..0db7e05 100644 --- a/odrive_node/package.xml +++ b/odrive_node/package.xml @@ -13,6 +13,7 @@ rosidl_default_generators rclcpp + std_srvs ament_lint_auto ament_lint_common diff --git a/odrive_node/src/odrive_can_node.cpp b/odrive_node/src/odrive_can_node.cpp index b67b2bb..3ca3a71 100644 --- a/odrive_node/src/odrive_can_node.cpp +++ b/odrive_node/src/odrive_can_node.cpp @@ -16,6 +16,7 @@ enum CmdId : uint32_t { kGetIq = 0x014, // ControllerStatus - publisher kGetTemp, // SystemStatus - publisher kGetBusVoltageCurrent = 0x017, // SystemStatus - publisher + kClearErrors = 0x018, // ClearErrors - service kGetTorques = 0x01c, // ControllerStatus - publisher }; @@ -42,6 +43,9 @@ ODriveCanNode::ODriveCanNode(const std::string& node_name) : rclcpp::Node(node_n rclcpp::QoS srv_qos(rclcpp::KeepAll{}); service_ = rclcpp::Node::create_service("request_axis_state", std::bind(&ODriveCanNode::service_callback, this, _1, _2), srv_qos.get_rmw_qos_profile()); + + rclcpp::QoS srv_clear_errors_qos(rclcpp::KeepAll{}); + service_clear_errors_ = rclcpp::Node::create_service("clear_errors", std::bind(&ODriveCanNode::service_clear_errors_callback, this, _1, _2), srv_clear_errors_qos.get_rmw_qos_profile()); } void ODriveCanNode::deinit() { @@ -67,6 +71,10 @@ bool ODriveCanNode::init(EpollEventLoop* event_loop) { RCLCPP_ERROR(rclcpp::Node::get_logger(), "Failed to initialize service event"); return false; } + if (!srv_clear_errors_evt_.init(event_loop, std::bind(&ODriveCanNode::request_clear_errors_callback, this))) { + RCLCPP_ERROR(rclcpp::Node::get_logger(), "Failed to initialize clear errors service event"); + return false; + } RCLCPP_INFO(rclcpp::Node::get_logger(), "node_id: %d", node_id_); RCLCPP_INFO(rclcpp::Node::get_logger(), "interface: %s", interface.c_str()); return true; @@ -180,6 +188,11 @@ void ODriveCanNode::service_callback(const std::shared_ptr r response->procedure_result = ctrl_stat_.procedure_result; } +void ODriveCanNode::service_clear_errors_callback(const std::shared_ptr request, std::shared_ptr response) { + RCLCPP_INFO(rclcpp::Node::get_logger(), "clearing errors"); + srv_clear_errors_evt_.set(); +} + void ODriveCanNode::request_state_callback() { struct can_frame frame; frame.can_id = node_id_ << 5 | CmdId::kSetAxisState; @@ -191,6 +204,14 @@ void ODriveCanNode::request_state_callback() { can_intf_.send_can_frame(frame); } +void ODriveCanNode::request_clear_errors_callback() { + struct can_frame frame; + frame.can_id = node_id_ << 5 | CmdId::kClearErrors; + write_le(0, frame.data); + frame.can_dlc = 1; + can_intf_.send_can_frame(frame); +} + void ODriveCanNode::ctrl_msg_callback() { uint32_t control_mode; From 73dc509002918abf4d2f29cae90ef24436730c40 Mon Sep 17 00:00:00 2001 From: ErikParkerrr Date: Sat, 12 Oct 2024 17:53:50 -0600 Subject: [PATCH 3/5] changed torque to effort to match hardware interface --- .../description/urdf/diffbot.ros2_control.xacro | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/odrive_botwheel_explorer/description/urdf/diffbot.ros2_control.xacro b/odrive_botwheel_explorer/description/urdf/diffbot.ros2_control.xacro index a253a06..e9552f6 100644 --- a/odrive_botwheel_explorer/description/urdf/diffbot.ros2_control.xacro +++ b/odrive_botwheel_explorer/description/urdf/diffbot.ros2_control.xacro @@ -20,19 +20,19 @@ 0 - + - + 1 - + - + From 6b267f3403b9bd8b046a8e5c7502b2acc6be487c Mon Sep 17 00:00:00 2001 From: ErikParkerrr Date: Sat, 12 Oct 2024 18:06:16 -0600 Subject: [PATCH 4/5] added use_mock_hardware and use_rviz launch arguemtns back in --- .../config/odrive_botwheel_explorer.rviz | 172 ++++++++++++++++++ .../launch/botwheel_explorer.launch.py | 75 ++++---- 2 files changed, 209 insertions(+), 38 deletions(-) create mode 100644 odrive_botwheel_explorer/config/odrive_botwheel_explorer.rviz diff --git a/odrive_botwheel_explorer/config/odrive_botwheel_explorer.rviz b/odrive_botwheel_explorer/config/odrive_botwheel_explorer.rviz new file mode 100644 index 0000000..9790158 --- /dev/null +++ b/odrive_botwheel_explorer/config/odrive_botwheel_explorer.rviz @@ -0,0 +1,172 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 1112 + - 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 +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_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_frontal_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_rear_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: odom + 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 + 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: 3.359799385070801 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.05434183403849602 + Y: 0.6973574757575989 + Z: -0.00023954140488058329 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.48539823293685913 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.0053997039794921875 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1383 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000004fcfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000044000004fc000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000004fcfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000044000004fc000000d301000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000784000004fc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2560 + X: 0 + Y: 28 \ No newline at end of file diff --git a/odrive_botwheel_explorer/launch/botwheel_explorer.launch.py b/odrive_botwheel_explorer/launch/botwheel_explorer.launch.py index 700831b..0e5c54d 100644 --- a/odrive_botwheel_explorer/launch/botwheel_explorer.launch.py +++ b/odrive_botwheel_explorer/launch/botwheel_explorer.launch.py @@ -25,24 +25,24 @@ def generate_launch_description(): # Declare arguments declared_arguments = [] -# declared_arguments.append( -# DeclareLaunchArgument( -# "gui", -# default_value="true", -# description="Start RViz2 automatically with this launch file.", -# ) -# ) -# declared_arguments.append( -# DeclareLaunchArgument( -# "use_mock_hardware", -# default_value="false", -# description="Start robot with mock hardware mirroring command to its states.", -# ) -# ) + declared_arguments.append( + DeclareLaunchArgument( + "use_rviz", + default_value="false", + description="Start RViz2 automatically with this launch file.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Start robot with mock hardware mirroring command to its states.", + ) + ) # Initialize Arguments -# gui = LaunchConfiguration("gui") -# use_mock_hardware = LaunchConfiguration("use_mock_hardware") + use_rviz = LaunchConfiguration("use_rviz") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") # Get URDF via xacro robot_description_content = Command( @@ -52,9 +52,8 @@ def generate_launch_description(): PathJoinSubstitution( [FindPackageShare("odrive_botwheel_explorer"), "urdf", "diffbot.urdf.xacro"] ), -# " ", -# "use_mock_hardware:=", -# use_mock_hardware, + " ", + "use_mock_hardware:=", use_mock_hardware, ] ) robot_description = {"robot_description": robot_description_content} @@ -66,9 +65,9 @@ def generate_launch_description(): "diffbot_controllers.yaml", ] ) -# rviz_config_file = PathJoinSubstitution( -# [FindPackageShare("ros2_control_demo_description"), "diffbot/rviz", "diffbot.rviz"] -# ) + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("odrive_botwheel_explorer"), "config", "odrive_botwheel_explorer.rviz"] + ) control_node = Node( package="controller_manager", @@ -88,14 +87,14 @@ def generate_launch_description(): ("/botwheel_explorer/cmd_vel_unstamped", "/cmd_vel"), ], ) -# rviz_node = Node( -# package="rviz2", -# executable="rviz2", -# name="rviz2", -# output="log", -# arguments=["-d", rviz_config_file], -# condition=IfCondition(gui), -# ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + condition=IfCondition(use_rviz), + ) joint_state_broadcaster_spawner = Node( package="controller_manager", @@ -109,13 +108,13 @@ def generate_launch_description(): arguments=["botwheel_explorer", "--controller-manager", "/controller_manager"], ) -# # Delay rviz start after `joint_state_broadcaster` -# delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( -# event_handler=OnProcessExit( -# target_action=joint_state_broadcaster_spawner, -# on_exit=[rviz_node], -# ) -# ) + # Delay rviz start after `joint_state_broadcaster` + delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[rviz_node], + ) + ) # Delay start of robot_controller after `joint_state_broadcaster` delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( @@ -129,7 +128,7 @@ def generate_launch_description(): control_node, robot_state_pub_node, joint_state_broadcaster_spawner, - #delay_rviz_after_joint_state_broadcaster_spawner, + delay_rviz_after_joint_state_broadcaster_spawner, delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, ] From 2c3dde7d4d838ee4ce1d45a250ef706c4ff83c97 Mon Sep 17 00:00:00 2001 From: ErikParkerrr Date: Sat, 12 Oct 2024 18:10:40 -0600 Subject: [PATCH 5/5] added materials file from ros2_control_examples --- .../description/urdf/diffbot.urdf.xacro | 2 + .../urdf/diffbot_materials.urdf.xacro | 47 +++++++++++++++++++ 2 files changed, 49 insertions(+) create mode 100644 odrive_botwheel_explorer/description/urdf/diffbot_materials.urdf.xacro diff --git a/odrive_botwheel_explorer/description/urdf/diffbot.urdf.xacro b/odrive_botwheel_explorer/description/urdf/diffbot.urdf.xacro index d459c6a..9e9d000 100644 --- a/odrive_botwheel_explorer/description/urdf/diffbot.urdf.xacro +++ b/odrive_botwheel_explorer/description/urdf/diffbot.urdf.xacro @@ -4,6 +4,8 @@ + + diff --git a/odrive_botwheel_explorer/description/urdf/diffbot_materials.urdf.xacro b/odrive_botwheel_explorer/description/urdf/diffbot_materials.urdf.xacro new file mode 100644 index 0000000..01d7ae1 --- /dev/null +++ b/odrive_botwheel_explorer/description/urdf/diffbot_materials.urdf.xacro @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file