ellensohn/rmp220_middleware
1
0
Fork 0
mirror of https://github.com/bjoernellens1/rmp220_middleware.git synced 2024-11-22 07:43:48 +00:00
Code Issues Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
Björn Ellensohn 2023-08-18 09:20:15 +02:00
parent cc49ca69d3
commit 0cf1825e2d
16 changed files with 129 additions and 338 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
.vscode/c_cpp_properties.json vendored
View File

@ -1,20 +0,0 @@
{
"configurations": [
{
"browse": {
"databaseFilename": "${default}",
"limitSymbolsToIncludedHeaders": false
},
"includePath": [
"/opt/ros/humble/include/**",
"/usr/include/**"
],
"name": "ROS",
"intelliSenseMode": "gcc-x64",
"compilerPath": "/usr/bin/gcc",
"cStandard": "gnu11",
"cppStandard": "c++14"
}
],
"version": 4
}

10
.vscode/settings.json vendored
View File

@ -1,10 +0,0 @@
{
"python.autoComplete.extraPaths": [
"/opt/ros/humble/lib/python3.10/site-packages",
"/opt/ros/humble/local/lib/python3.10/dist-packages"
],
"python.analysis.extraPaths": [
"/opt/ros/humble/lib/python3.10/site-packages",
"/opt/ros/humble/local/lib/python3.10/dist-packages"
]
}

10
CMakeLists.txt
View File

@ -10,6 +10,11 @@ find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(segway_msgs REQUIRED)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
@ -23,4 +28,9 @@ if(BUILD_TESTING)
ament_lint_auto_find_test_dependencies()
endif()
add_executable(rmp220_middleware rmp220_middleware.cpp)
ament_target_dependencies(state_machine_node rclcpp std_msgs geometry_msgs sensor_msgs segway_msgs)
install(TARGETS rmp220_middleware DESTINATION lib/${PROJECT_NAME})
ament_package()

10
package.xml
View File

@ -7,12 +7,12 @@
<maintainer email="bjoern.ellensohn@gmail.com">bjorn</maintainer>
<license>TODO: License declaration</license>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_python</build_type>
<build_type>ament_cmake</build_type>
</export>
</package>

0
resource/rmp220_middleware
View File

0
rmp220_middleware/__init__.py
View File

BIN
rmp220_middleware/__pycache__/__init__.cpython-310.pyc
View File

Binary file not shown.

BIN
rmp220_middleware/__pycache__/rmp220_middleware.cpython-310.pyc
View File

Binary file not shown.

96
rmp220_middleware/rmp220_middleware copy.py
View File

@ -1,96 +0,0 @@
#!/usr/bin/env python3
import rclpy
from enum import Enum
from rclpy.node import Node
from std_msgs.msg import Bool
from geometry_msgs.msg import Twist
#from sensor_msgs.msg import Joy
from segway_msgs.srv import RosSetChassisEnableCmd
class State(Enum):
DISABLED = 0
ENABLED = 1
class StateMachineNode(Node):
def __init__(self):
super().__init__('state_machine_node')
# Initialize state and other variables
self.state = State.DISABLED
self.timeout = 2.0 # Timeout in seconds
# Create publishers, subscribers, timers, and service clients here
self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel_out', 10)
self.cmd_vel_sub = self.create_subscription(Twist, '/cmd_vel_mux', self.cmd_vel_callback, 10)
#self.joy_sub = self.create_subscription(Joy, '/joy', self.joy_callback, 10)
self.timer = self.create_timer(0.1, self.timer_callback)
# Create service clients for chassis enable and disable
self.chassis_enable_client = self.create_client(RosSetChassisEnableCmd, 'set_chassis_enable')
while not self.chassis_enable_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('Service not available, waiting for chassis enable service...')
self.chassis_disable_client = self.create_client(RosSetChassisEnableCmd, 'set_chassis_enable')
while not self.chassis_disable_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('Service not available, waiting for chassis disable service...')
def joy_callback(self, msg):
# Implement logic to detect joystick button presses (start/select) and update state
# ...
if msg.buttons[7] == 1: # Joystick button 'start'
self.state = State.ENABLED
self.get_logger().info("State: ENABLED (Button 'start')")
self.enable_chassis()
if msg.buttons[6] == 1: # Joystick button 'select'
self.state = State.DISABLED
self.get_logger().info("State: DISABLED (Button 'select')")
self.disable_chassis()
def enable_chassis(self):
req = RosSetChassisEnableCmd.Request()
req.ros_set_chassis_enable_cmd = True
self.chassis_enable_client.call_async(req)
def disable_chassis(self):
req = RosSetChassisEnableCmd.Request()
req.ros_set_chassis_enable_cmd = False
self.chassis_disable_client.call_async(req)
def cmd_vel_callback(self, msg):
# Update state to ENABLED upon receiving a command on /cmd_vel_mux
# ...
if self.state == State.ENABLED:
self.cmd_vel_pub.publish(msg)
self.timeout = 2.0 # Reset timeout when receiving commands
def timer_callback(self):
# Republish the cmd_vel_mux command to cmd_vel_out topic
# ...
# Reset the timeout counter
# ...
# Check if the timeout has been exceeded, and if so, switch to DISABLED
# ...
if self.state == State.ENABLED:
if self.timeout <= 0:
self.state = State.DISABLED
self.get_logger().info("State: DISABLED (Timeout)")
self.disable_chassis()
else:
self.timeout -= 0.1
def main(args=None):
rclpy.init(args=args)
node = StateMachineNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

104
rmp220_middleware/rmp220_middleware.py
View File

@ -1,104 +0,0 @@
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import Bool
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Joy
from enum import Enum
from segway_msgs.srv import RosSetChassisEnableCmd
import atexit
import signal
import sys
class State(Enum):
DISABLED = 0
ENABLED = 1
class StateMachineNode(Node):
def __init__(self):
super().__init__('state_machine_node')
# Initialize state and other variables
self.state = State.DISABLED
self.timeout = 20.0 # Timeout in seconds
#self.limit = 0.5 # Limit for linear and angular velocity
# Create publishers, subscribers, timers, and service clients
self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel_out', 10)
self.cmd_vel_sub = self.create_subscription(Twist, '/cmd_vel_mux', self.cmd_vel_callback, 10)
self.joy_sub = self.create_subscription(Joy, '/joy', self.joy_callback, 10)
self.timer = self.create_timer(0.01, self.timer_callback)
# Create twist class for publishing velocities
self.twist = Twist()
self.latest_cmd_vel = Twist()
# Create service clients for chassis enable and disable
self.chassis_enable_client = self.create_client(RosSetChassisEnableCmd, 'set_chassis_enable')
while not self.chassis_enable_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('Service not available, waiting for chassis enable service...')
self.get_logger().info('Chassis enable service available.')
def enable_chassis(self):
req = RosSetChassisEnableCmd.Request()
req.ros_set_chassis_enable_cmd = True
self.chassis_enable_client.call_async(req)
self.get_logger().info('Enabling chassis...')
def disable_chassis(self):
req = RosSetChassisEnableCmd.Request()
req.ros_set_chassis_enable_cmd = False
self.chassis_enable_client.call_async(req)
self.get_logger().info('Disabling chassis...')
def joy_callback(self, msg):
# if self.state == State.DISABLED and msg.buttons[7] == 1: # Joystick button 'start'
if msg.buttons[7] == 1: # Joystick button 'start'
self.state = State.ENABLED
self.get_logger().info("State: ENABLED (Button 'start')")
self.enable_chassis()
# if self.state == State.ENABLED and msg.buttons[6] == 1: # Joystick button 'select'
if msg.buttons[6] == 1: # Joystick button 'select'
self.state = State.DISABLED
self.get_logger().info("State: DISABLED (Button 'select')")
self.disable_chassis()
def cmd_vel_callback(self, msg):
# This method shall only update the latest_cmd_vel attribute so it can be republished by the timer_callback with 100 HZ. Should have a look at performance though.
self.latest_cmd_vel = msg
self.timeout = 20.0 # Reset timeout when receiving commands
def timer_callback(self):
if self.state == State.ENABLED:
if self.timeout <= 0:
self.state = State.DISABLED
self.get_logger().info("State: DISABLED (Timeout)")
self.disable_chassis()
else:
self.timeout -= 0.01
self.cmd_vel_pub.publish(self.latest_cmd_vel)
if self.state == State.DISABLED and (abs(self.latest_cmd_vel.linear) > 0.1 or abs(self.latest_cmd_vel.angular > 0.1)): # This is a hack to enable the chassis when receiving commands e.g. from Nav2
self.state = State.ENABLED
self.get_logger().info("State: ENABLED (cmd_vel)")
self.enable_chassis()
else:
self.cmd_vel_pub.publish(self.twist)
def main(args=None):
rclpy.init(args=args)
node = StateMachineNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.disable_chassis()
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

4
setup.cfg
View File

@ -1,4 +0,0 @@
[develop]
script_dir=$base/lib/rmp220_middleware
[install]
install_scripts=$base/lib/rmp220_middleware

26
setup.py
View File

@ -1,26 +0,0 @@
from setuptools import setup
package_name = 'rmp220_middleware'
setup(
name=package_name,
version='0.0.0',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='bjorn',
maintainer_email='bjoern.ellensohn@gmail.com',
description='TODO: Package description',
license='TODO: License declaration',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'rmp220_middleware = rmp220_middleware.rmp220_middleware:main'
],
},
)

114
src/rmp220_middleware.cpp Normal file
View File

@ -0,0 +1,114 @@
#include <memory>
#include <chrono>
#include <cmath>
#include <signal.h>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/bool.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <sensor_msgs/msg/joy.hpp>
#include <segway_msgs/srv/ros_set_chassis_enable_cmd.hpp>
using namespace std::chrono_literals;
enum class State {
DISABLED,
ENABLED
};
class StateMachineNode : public rclcpp::Node {
public:
StateMachineNode() : Node("state_machine_node") {
state = State::DISABLED;
timeout = 20.0;
cmd_vel_pub = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel_out", 10);
cmd_vel_sub = create_subscription<geometry_msgs::msg::Twist>(
"/cmd_vel_mux", 10,
std::bind(&StateMachineNode::cmd_vel_callback, this, std::placeholders::_1)
);
joy_sub = create_subscription<sensor_msgs::msg::Joy>(
"/joy", 10,
std::bind(&StateMachineNode::joy_callback, this, std::placeholders::_1)
);
timer = create_wall_timer(10ms, std::bind(&StateMachineNode::timer_callback, this));
chassis_enable_client = create_client<segway_msgs::srv::RosSetChassisEnableCmd>("set_chassis_enable");
while (!chassis_enable_client->wait_for_service(1s)) {
RCLCPP_INFO(get_logger(), "Service not available, waiting for chassis enable service...");
}
RCLCPP_INFO(get_logger(), "Chassis enable service available.");
}
private:
State state;
double timeout;
geometry_msgs::msg::Twist twist;
geometry_msgs::msg::Twist latest_cmd_vel;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub;
rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub;
rclcpp::TimerBase::SharedPtr timer;
rclcpp::Client<segway_msgs::srv::RosSetChassisEnableCmd>::SharedPtr chassis_enable_client;
void enable_chassis() {
auto request = std::make_shared<segway_msgs::srv::RosSetChassisEnableCmd::Request>();
request->ros_set_chassis_enable_cmd = true;
auto future = chassis_enable_client->async_send_request(request);
RCLCPP_INFO(get_logger(), "Enabling chassis...");
}
void disable_chassis() {
auto request = std::make_shared<segway_msgs::srv::RosSetChassisEnableCmd::Request>();
request->ros_set_chassis_enable_cmd = false;
auto future = chassis_enable_client->async_send_request(request);
RCLCPP_INFO(get_logger(), "Disabling chassis...");
}
void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg) {
if (msg->buttons[7] == 1) { // Joystick button 'start'
state = State::ENABLED;
RCLCPP_INFO(get_logger(), "State: ENABLED (Button 'start')");
enable_chassis();
}
if (msg->buttons[6] == 1) { // Joystick button 'select'
state = State::DISABLED;
RCLCPP_INFO(get_logger(), "State: DISABLED (Button 'select')");
disable_chassis();
}
}
void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) {
latest_cmd_vel = *msg;
timeout = 20.0;
}
void timer_callback() {
if (state == State::ENABLED) {
if (timeout <= 0) {
state = State::DISABLED;
RCLCPP_INFO(get_logger(), "State: DISABLED (Timeout)");
disable_chassis();
} else {
timeout -= 0.01;
cmd_vel_pub->publish(latest_cmd_vel);
}
}
if (state == State::DISABLED &&
(std::abs(latest_cmd_vel.linear.x) > 0.1 || std::abs(latest_cmd_vel.angular.z) > 0.1)) {
state = State::ENABLED;
RCLCPP_INFO(get_logger(), "State: ENABLED (cmd_vel)");
enable_chassis();
} else {
cmd_vel_pub->publish(twist);
}
}
};
int main(int argc, char * argv[]) {
rclcpp::init(argc, argv);
auto node = std::make_shared<StateMachineNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

25
test/test_copyright.py
View File

@ -1,25 +0,0 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_copyright.main import main
import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found errors'

25
test/test_flake8.py
View File

@ -1,25 +0,0 @@
# Copyright 2017 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_flake8.main import main_with_errors
import pytest
@pytest.mark.flake8
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
assert rc == 0, \
'Found %d code style errors / warnings:\n' % len(errors) + \
'\n'.join(errors)

23
test/test_pep257.py
View File

@ -1,23 +0,0 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_pep257.main import main
import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found code style errors / warnings'