Skip to content

Commit 824fc72

Browse files
ottojoottojo
committed
Add 'ipa_ros2_control/' from commit '55f31a212582defc49c95d7038bcc951365f7374'
git-subtree-dir: ipa_ros2_control git-subtree-mainline: 7aa8f93 git-subtree-split: 55f31a2
2 parents 7aa8f93 + 55f31a2 commit 824fc72

File tree

9 files changed

+852
-0
lines changed

9 files changed

+852
-0
lines changed

‎ipa_ros2_control/CMakeLists.txt‎

Lines changed: 122 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,122 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
project(ipa_ros2_control)
3+
4+
# Default to C++14
5+
if(NOT CMAKE_CXX_STANDARD)
6+
set(CMAKE_CXX_STANDARD 17)
7+
endif()
8+
9+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
10+
add_compile_options(-Wall -Wextra)
11+
endif()
12+
13+
# find dependencies
14+
find_package(ament_cmake REQUIRED)
15+
find_package(hardware_interface REQUIRED)
16+
find_package(pluginlib REQUIRED)
17+
find_package(rclcpp REQUIRED)
18+
find_package(rclcpp_lifecycle REQUIRED)
19+
find_package(tl_expected REQUIRED)
20+
find_package(candle_ros2 REQUIRED)
21+
find_package(sensor_msgs REQUIRED)
22+
23+
unset( CATKIN_INSTALL_INTO_PREFIX_ROOT )
24+
25+
include_directories(include)
26+
27+
link_directories(/usr/local/lib)
28+
29+
add_library(
30+
${PROJECT_NAME}
31+
SHARED
32+
src/curt_mini_hardware_interface.cpp
33+
)
34+
35+
target_include_directories(
36+
${PROJECT_NAME}
37+
PRIVATE
38+
include
39+
)
40+
41+
target_include_directories(${PROJECT_NAME} PUBLIC
42+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
43+
$<INSTALL_INTERFACE:include>)
44+
45+
ament_target_dependencies(
46+
${PROJECT_NAME}
47+
hardware_interface
48+
pluginlib
49+
rclcpp
50+
rclcpp_lifecycle
51+
tl_expected
52+
candle_ros2
53+
sensor_msgs
54+
)
55+
56+
# Causes the visibility macros to use dllexport rather than dllimport,
57+
# which is appropriate when building the dll but not consuming it.
58+
#target_compile_definitions(${PROJECT_NAME} PRIVATE "JOINT_STATE_BROADCASTER_BUILDING_DLL")
59+
60+
# prevent pluginlib from using boost
61+
#target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
62+
63+
pluginlib_export_plugin_description_file(hardware_interface curt_mini_driver.xml)
64+
65+
66+
install(
67+
TARGETS ${PROJECT_NAME}
68+
EXPORT export_${PROJECT_NAME}
69+
ARCHIVE DESTINATION lib
70+
LIBRARY DESTINATION lib
71+
RUNTIME DESTINATION bin
72+
)
73+
74+
# install(
75+
# TARGETS ${PROJECT_NAME}
76+
# DESTINATION lib
77+
# )
78+
79+
install(
80+
DIRECTORY include launch
81+
DESTINATION share/${PROJECT_NAME}
82+
)
83+
84+
# install(
85+
# DIRECTORY include/
86+
# DESTINATION include
87+
# )
88+
89+
# install(TARGETS ${PROJECT_NAME}
90+
# DESTINATION lib/${PROJECT_NAME}
91+
# PUBLIC_HEADER DESTINATION include/${PROJECT_NAME}/
92+
# )
93+
94+
# if(BUILD_TESTING)
95+
# find_package(ament_cmake_gtest REQUIRED)
96+
# endif()
97+
98+
# install(DIRECTORY include/${PROJECT_NAME}/
99+
# DESTINATION include/${PROJECT_NAME}
100+
# FILES_MATCHING PATTERN "*.h"
101+
# PATTERN ".git" EXCLUDE)
102+
103+
## EXPORTS
104+
ament_export_include_directories(
105+
include
106+
)
107+
ament_export_libraries(
108+
${PROJECT_NAME}
109+
)
110+
111+
ament_export_targets(
112+
export_${PROJECT_NAME}
113+
)
114+
115+
# ament_export_dependencies(
116+
# hardware_interface
117+
# pluginlib
118+
# rclcpp
119+
# rclcpp_lifecycle
120+
# tl_expected
121+
# )
122+
ament_package()

‎ipa_ros2_control/README.md‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
# ipa_ros2_control
2+
3+
Drivers of the sensors and actuators.
4+
Setup for ROS2 foxy.

‎ipa_ros2_control/curt_mini_driver.xml‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
<library path="ipa_ros2_control">
2+
<class name="ipa_ros2_control/CurtMiniHardwareInterface" type="ipa_ros2_control::CurtMiniHardwareInterface" base_class_type="hardware_interface::SystemInterface"></class>
3+
</library>

‎ipa_ros2_control/include/ipa_ros2_control/curt_mini_hardware_interface.hpp‎

Lines changed: 160 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,160 @@
1+
// Copyright 2021 ros2_control Development Team
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef CURT_MINI_SYSTEM_HPP_
16+
#define CURT_MINI_SYSTEM_HPP_
17+
18+
#include <chrono>
19+
#include <limits>
20+
#include <memory>
21+
#include <string>
22+
#include <vector>
23+
24+
#include "hardware_interface/handle.hpp"
25+
#include "hardware_interface/hardware_info.hpp"
26+
#include "hardware_interface/system_interface.hpp"
27+
#include "hardware_interface/types/hardware_interface_return_values.hpp"
28+
#include "hardware_interface/types/hardware_interface_type_values.hpp"
29+
#include "rclcpp/clock.hpp"
30+
#include "rclcpp/duration.hpp"
31+
#include "rclcpp/macros.hpp"
32+
#include "rclcpp/rclcpp.hpp"
33+
#include "rclcpp/time.hpp"
34+
#include <sensor_msgs/msg/joint_state.hpp>
35+
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
36+
#include "rclcpp_lifecycle/state.hpp"
37+
38+
#include "candle_ros2/msg/impedance_command.hpp"
39+
#include "candle_ros2/msg/motion_command.hpp"
40+
#include "candle_ros2/msg/position_pid_command.hpp"
41+
#include "candle_ros2/msg/velocity_pid_command.hpp"
42+
#include "candle_ros2/srv/add_md80s.hpp"
43+
#include "candle_ros2/srv/generic_md80_msg.hpp"
44+
#include "candle_ros2/srv/set_limits_md80.hpp"
45+
#include "candle_ros2/srv/set_mode_md80s.hpp"
46+
47+
namespace ipa_ros2_control
48+
{
49+
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
50+
51+
class CurtMiniHardwareInterface : public hardware_interface::SystemInterface
52+
{
53+
public:
54+
55+
CallbackReturn
56+
on_init(const hardware_interface::HardwareInfo &hardware_info) override;
57+
58+
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
59+
60+
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
61+
62+
void jointsCallback(const std::shared_ptr<sensor_msgs::msg::JointState> msg);
63+
64+
CallbackReturn
65+
on_activate(const rclcpp_lifecycle::State &previous_state) override;
66+
CallbackReturn
67+
on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
68+
69+
hardware_interface::return_type read(const rclcpp::Time &time, const rclcpp::Duration &period) override;
70+
71+
hardware_interface::return_type write(const rclcpp::Time &time, const rclcpp::Duration &period) override;
72+
73+
private:
74+
rclcpp::Node::SharedPtr nh_;
75+
// inspired by Jackal github
76+
void writeCommandsToHardware();
77+
void updateJointsFromHardware();
78+
void publishPIDParams(const candle_ros2::msg::Pid& pid_config);
79+
// bool sendGenericRequest(rclcpp::Client<candle_ros2::srv::GenericMd80Msg>::SharedPtr& client);
80+
81+
/**
82+
*
83+
* Wrapper function for templated node's declare_parameter() which checks if
84+
* parameter is already declared.
85+
* (Copied from ControllerInterface base class)
86+
*/
87+
template <typename ParameterT>
88+
auto auto_declare(const std::string& name, const ParameterT& default_value)
89+
{
90+
if (!nh_->has_parameter(name))
91+
{
92+
return nh_->declare_parameter<ParameterT>(name, default_value);
93+
}
94+
else
95+
{
96+
return nh_->get_parameter(name).get_value<ParameterT>();
97+
}
98+
}
99+
100+
template <typename candle_srv_type>
101+
bool sendCandleRequest(
102+
typename rclcpp::Client<candle_srv_type>::SharedPtr client,
103+
typename candle_srv_type::Request::SharedPtr request = std::make_shared<typename candle_srv_type::Request>())
104+
{
105+
RCLCPP_INFO_STREAM(nh_->get_logger(), "Calling " << client->get_service_name());
106+
request->drive_ids = { 102, 100, 103, 101 }; // { br , fr, bl , fl}
107+
auto result = client->async_send_request(request);
108+
if (rclcpp::spin_until_future_complete(nh_, result, std::chrono::seconds{5}) == rclcpp::FutureReturnCode::SUCCESS)
109+
{
110+
RCLCPP_INFO_STREAM(nh_->get_logger(), "Calling ");
111+
auto res = *result.get();
112+
if (!std::all_of(res.drives_success.begin(), res.drives_success.end(),
113+
[](bool b) { return b; }))
114+
{
115+
RCLCPP_ERROR_STREAM(nh_->get_logger(),
116+
"Service " << client->get_service_name() << " was not successfull for all motors! Exiting");
117+
return false;
118+
}
119+
}
120+
else
121+
{
122+
RCLCPP_ERROR_STREAM(nh_->get_logger(), "Calling " << client->get_service_name() << " failed! Exiting");
123+
return false;
124+
}
125+
return true;
126+
}
127+
128+
// Store the command for the robot
129+
std::vector<double> hw_commands_;
130+
std::vector<double> hw_states_position_, hw_states_velocity_;
131+
sensor_msgs::msg::JointState motor_joint_state_;
132+
// ROS services
133+
rclcpp::Client<candle_ros2::srv::AddMd80s>::SharedPtr add_controller_service_client_;
134+
rclcpp::Client<candle_ros2::srv::SetModeMd80s>::SharedPtr set_mode_service_client_;
135+
rclcpp::Client<candle_ros2::srv::GenericMd80Msg>::SharedPtr set_zero_service_client_;
136+
rclcpp::Client<candle_ros2::srv::GenericMd80Msg>::SharedPtr enable_motors_service_client_;
137+
rclcpp::Client<candle_ros2::srv::GenericMd80Msg>::SharedPtr disable_motors_service_client_;
138+
139+
// ROS publishers
140+
rclcpp::Publisher<candle_ros2::msg::MotionCommand>::SharedPtr command_pub_;
141+
rclcpp::Publisher<candle_ros2::msg::VelocityPidCommand>::SharedPtr config_pub_;
142+
143+
// ROS subscribers
144+
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
145+
// controller params
146+
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;
147+
candle_ros2::msg::Pid pid_config_;
148+
double standstill_thresh_{ 0.01 };
149+
bool motors_paused_{ false };
150+
151+
void pauseMotors();
152+
void resumeMotors();
153+
154+
// map of joint names and there index
155+
std::map<std::string, uint8_t> wheel_joints_;
156+
};
157+
158+
} // namespace ipa_ros2_control
159+
160+
#endif // CURT_MINI_SYSTEM_HPP

‎ipa_ros2_control/ipa_ros2_control.repos‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
repositories:
2+
candle_ros2:
3+
type: git
4+
url: [email protected]:ipa323/hardware/candle_ros2.git
5+
version: ipa_main

‎ipa_ros2_control/launch/ros2_control.launch.py‎

Lines changed: 81 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
from email.policy import default
2+
import os
3+
4+
from ament_index_python.packages import get_package_share_directory
5+
from launch import LaunchDescription
6+
from launch.actions import DeclareLaunchArgument
7+
from launch.substitutions import LaunchConfiguration, Command
8+
from launch_ros.actions import Node
9+
from launch.actions import OpaqueFunction
10+
11+
def launch_ros2_control(context, *args, **kwargs):
12+
# initialize arguments
13+
robot = LaunchConfiguration('robot')
14+
gear_ratio = LaunchConfiguration('gear_ratio')
15+
interface = LaunchConfiguration('interface')
16+
17+
robot_dir = get_package_share_directory(robot.perform(context))
18+
19+
ros2_control_yaml_path = os.path.join(robot_dir, 'config', 'ros2_control.yaml')
20+
21+
urdf_path = os.path.join(robot_dir, 'models', robot.perform(context), 'robot.urdf.xacro')
22+
23+
robot_description = Command(['xacro ', urdf_path, ' gear_ratio:=', gear_ratio, ' interface:=', interface])
24+
25+
controller_manager_node = Node(
26+
package='controller_manager',
27+
executable='ros2_control_node',
28+
parameters=[
29+
ros2_control_yaml_path,
30+
],
31+
output={
32+
"stdout": "screen",
33+
"stderr": "screen",
34+
},
35+
)
36+
37+
md80_manager_node = Node(
38+
package='candle_ros2',
39+
executable='candle_ros2_node',
40+
output={
41+
"stdout": "screen",
42+
"stderr": "screen",
43+
},
44+
arguments=["USB", "1M"],
45+
)
46+
return [controller_manager_node,
47+
md80_manager_node]
48+
49+
def generate_launch_description():
50+
51+
declared_arguments = []
52+
53+
# robot argument
54+
declared_arguments.append(
55+
DeclareLaunchArgument(
56+
'robot',
57+
default_value='curt_mini',
58+
description="Set the robot.",
59+
choices=['curt_mvp', 'curt_mini'],
60+
)
61+
)
62+
63+
# set the interface for the ethercat communication
64+
declared_arguments.append(
65+
DeclareLaunchArgument(
66+
'interface',
67+
default_value='enp0s31f6',
68+
description='Ethercat interface to the motor controllers',
69+
)
70+
)
71+
72+
# set the gear_ratio
73+
declared_arguments.append(
74+
DeclareLaunchArgument(
75+
'gear_ratio',
76+
default_value='1',
77+
description='Set the gear ratio of the motors',
78+
)
79+
)
80+
81+
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_ros2_control)])

‎ipa_ros2_control/mvp_driver.xml‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
<library path="ipa_ros2_control">
2+
<class name="ipa_ros2_control/MvpHardwareInterface" type="ipa_ros2_control::MvpHardwareInterface" base_class_type="hardware_interface::SystemInterface"></class>
3+
</library>

0 commit comments

Comments
 (0)