Compare commits
No commits in common. "humble" and "main" have entirely different histories.
@ -6,7 +6,7 @@
|
||||
// "WORKSPACE": "${containerWorkspaceFolder}"
|
||||
// }
|
||||
// },
|
||||
"image": "delicjusz/ros2aria",
|
||||
"image": "irth7/ros2aria-dev",
|
||||
"remoteUser": "ros",
|
||||
"runArgs": [
|
||||
"--network=host",
|
||||
|
||||
6
.vscode/c_cpp_properties.json
vendored
6
.vscode/c_cpp_properties.json
vendored
@ -10,11 +10,9 @@
|
||||
"/workspaces/ros2aria/src/ros2aria/include/**",
|
||||
"/workspaces/ros2aria/install/ros2aria_msgs/include/**",
|
||||
"/usr/include/**",
|
||||
"/usr/local/Aria/include",
|
||||
"${workspaceFolder}/src/ros2aria/include/**"
|
||||
"/usr/local/Aria/include"
|
||||
],
|
||||
"name": "ROS",
|
||||
"configurationProvider": "ms-vscode.cmake-tools"
|
||||
"name": "ROS"
|
||||
}
|
||||
],
|
||||
"version": 4
|
||||
|
||||
29
.vscode/settings.json
vendored
29
.vscode/settings.json
vendored
@ -61,34 +61,9 @@
|
||||
"thread": "cpp",
|
||||
"cinttypes": "cpp",
|
||||
"typeindex": "cpp",
|
||||
"typeinfo": "cpp",
|
||||
"any": "cpp",
|
||||
"hash_map": "cpp",
|
||||
"hash_set": "cpp",
|
||||
"strstream": "cpp",
|
||||
"bitset": "cpp",
|
||||
"cfenv": "cpp",
|
||||
"compare": "cpp",
|
||||
"complex": "cpp",
|
||||
"concepts": "cpp",
|
||||
"forward_list": "cpp",
|
||||
"unordered_set": "cpp",
|
||||
"source_location": "cpp",
|
||||
"iomanip": "cpp",
|
||||
"numbers": "cpp",
|
||||
"semaphore": "cpp",
|
||||
"stop_token": "cpp",
|
||||
"valarray": "cpp",
|
||||
"variant": "cpp",
|
||||
"regex": "cpp",
|
||||
"ranges": "cpp"
|
||||
"typeinfo": "cpp"
|
||||
},
|
||||
"cSpell.words": [
|
||||
"rclcpp"
|
||||
],
|
||||
"python.analysis.extraPaths": [
|
||||
"/opt/ros/foxy/lib/python3.8/site-packages"
|
||||
],
|
||||
"ros.distro": "humble",
|
||||
"cmake.sourceDirectory": "/home/jdelicat/lab_repos/ros2aria/"
|
||||
]
|
||||
}
|
||||
@ -1,19 +0,0 @@
|
||||
ARG ROS_DISTRO=humble
|
||||
FROM husarnet/ros:humble-ros-core
|
||||
ARG ROS_DISTRO
|
||||
|
||||
SHELL ["/bin/bash", "-c"]
|
||||
|
||||
RUN mkdir -p /ros2_ws/src && cd /ros2_ws/src
|
||||
WORKDIR /ros2_ws
|
||||
|
||||
RUN apt-get update && apt-get install -y \
|
||||
build-essential
|
||||
|
||||
COPY ./src/AriaCoda /usr/local/Aria
|
||||
RUN cd /usr/local/Aria && make -j$(nproc) && export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/Aria
|
||||
# clear ubuntu packages
|
||||
RUN apt-get clean && \
|
||||
apt-get remove -y \
|
||||
build-essential && \
|
||||
rm -rf /var/lib/apt/lists/*
|
||||
@ -1,7 +0,0 @@
|
||||
ARG ROS_DISTRO=humble
|
||||
FROM husarnet/ros:humble-ros-core
|
||||
SHELL ["/bin/bash", "-c"]
|
||||
RUN apt-get update && apt-get install -y \
|
||||
ros-$ROS_DISTRO-urg-node
|
||||
|
||||
RUN echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc
|
||||
25
Makefile
25
Makefile
@ -6,34 +6,17 @@ HEADERS := $(wildcard src/ros2aria/src/*.hpp)
|
||||
build/ros2aria/ros2aria: $(SOURCES) $(HEADERS)
|
||||
colcon build
|
||||
|
||||
build:
|
||||
docker build -f Dockerfile -t delicjusz/pioneer .
|
||||
build: build/ros2aria/ros2aria
|
||||
|
||||
# .uploaded: build/ros2aria/ros2aria
|
||||
upload:
|
||||
rsync -r . lab1_5@pionier2:~/ros2aria
|
||||
rsync -r . lab1_5@pionier6:~/ros2aria
|
||||
touch .uploaded
|
||||
|
||||
# upload: .uploaded
|
||||
|
||||
run: upload
|
||||
ssh lab1_5@pionier2 -t -- docker run --rm --network=host -it --device /dev/ttyS0 delicjusz/ros2aria /bin/bash /ros2_ws/ros2aria/run.sh
|
||||
# export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/Aria/lib ros2 run ros2aria ros2aria --ros-args -p pioneer_id:=2
|
||||
ssh lab1_5@pionier6 -t -- docker run --rm --network=host -it --device /dev/ttyS0 -v /home/lab1_5:/ws irth7/ros2aria-dev /bin/bash /ws/ros2aria/run.sh
|
||||
|
||||
legacy:
|
||||
ssh lab1_5@pionier2 -t -- ./run.sh
|
||||
|
||||
push_ros2aria:
|
||||
./build_devcontainer.sh
|
||||
docker push delicjusz/ros2aria
|
||||
|
||||
push:
|
||||
docker build -f Dockerfile -t delicjusz/pioneer .
|
||||
docker push delicjusz/pioneer
|
||||
docker build -f Dockerfile.hokuyo -t delicjusz/hokuyo .
|
||||
docker push delicjusz/hokuyo
|
||||
pull:
|
||||
docker pull delicjusz/ros2aria
|
||||
rosbridge:
|
||||
docker build -f Dockerfile.rosbridge -t delicjusz/rosbridge .
|
||||
docker push delicjusz/rosbridge
|
||||
ssh lab1_5@pionier6 -t -- ./run.sh
|
||||
|
||||
21
README.md
21
README.md
@ -1,21 +0,0 @@
|
||||
# Instruction on station
|
||||
Export discovery server configuration file:
|
||||
```bash
|
||||
export FASTRTPS_DEFAULT_PROFILES_FILE=$(pwd)/fastdds.xml
|
||||
```
|
||||
|
||||
Export discovery server ip:
|
||||
```
|
||||
export ROS_DISCOVERY_SERVER=10.104.16.240:11811
|
||||
```
|
||||
|
||||
Export fastdds ros middleware implementation:
|
||||
```
|
||||
export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
|
||||
```
|
||||
|
||||
Restart ROS 2 daemon:
|
||||
```
|
||||
ros2 daemon stop
|
||||
ros2 daemon start
|
||||
```
|
||||
@ -1,3 +1,3 @@
|
||||
#!/bin/sh
|
||||
docker build -f Dockerfile.aria -t delicjusz/ros2aria .
|
||||
docker build --build-arg WORKSPACE=/ws -f Dockerfile.devcontainer -t irth7/ros2aria-dev .
|
||||
|
||||
|
||||
@ -1,14 +0,0 @@
|
||||
# docker compose -f compose.decawave.yaml up
|
||||
services:
|
||||
decawave:
|
||||
image: delicjusz/decawave
|
||||
network_mode: host
|
||||
ipc: host
|
||||
environment:
|
||||
- RMW_IMPLEMENTATION=rmw_fastrtps_cpp
|
||||
- FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml
|
||||
volumes:
|
||||
- ./fastdds.xml:/fastdds.xml
|
||||
devices:
|
||||
- /dev/decawave
|
||||
command: ros2 launch decawave_driver decawave.launch.py namespace:=pioneer${PIONEER_ID}/
|
||||
@ -1,18 +0,0 @@
|
||||
# docker compose -f compose.hokuyo.yaml up
|
||||
services:
|
||||
hokuyo:
|
||||
image: delicjusz/hokuyo
|
||||
network_mode: host
|
||||
ipc: host
|
||||
environment:
|
||||
- RMW_IMPLEMENTATION=rmw_fastrtps_cpp
|
||||
- FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml
|
||||
volumes:
|
||||
- ./fastdds.xml:/fastdds.xml
|
||||
devices:
|
||||
- /dev/hokuyo
|
||||
command: >
|
||||
ros2 run urg_node urg_node_driver --ros-args -r
|
||||
__ns:=/pioneer${PIONEER_ID} -p
|
||||
laser_frame_id:=pioneer${PIONEER_ID}/laser -p
|
||||
serial_port:=/dev/hokuyo
|
||||
@ -1,35 +0,0 @@
|
||||
# docker compose -f compose.ros2aria.yaml up
|
||||
services:
|
||||
ros2aria-dev:
|
||||
image: delicjusz/pioneer
|
||||
network_mode: host
|
||||
ipc: host
|
||||
environment:
|
||||
- RMW_IMPLEMENTATION=rmw_fastrtps_cpp
|
||||
- LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/Aria/lib
|
||||
- FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml
|
||||
- ROS_DISCOVERY_SERVER=10.104.16.240:11811
|
||||
volumes:
|
||||
- ./fastdds.xml:/fastdds.xml
|
||||
devices:
|
||||
- /dev/ttyS0
|
||||
command: ros2 launch pioneer_bringup robot.launch.py namespace:=pioneer${PIONEER_ID}/
|
||||
|
||||
hokuyo:
|
||||
image: delicjusz/hokuyo
|
||||
network_mode: host
|
||||
ipc: host
|
||||
environment:
|
||||
- RMW_IMPLEMENTATION=rmw_fastrtps_cpp
|
||||
- FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml
|
||||
- ROS_DISCOVERY_SERVER=10.104.16.240:11811
|
||||
volumes:
|
||||
- ./fastdds.xml:/fastdds.xml
|
||||
devices:
|
||||
# - /dev/hokuyo
|
||||
- /dev/ttyACM0
|
||||
command: >
|
||||
ros2 run urg_node urg_node_driver --ros-args -r
|
||||
__ns:=/pioneer${PIONEER_ID} -p
|
||||
laser_frame_id:=pioneer${PIONEER_ID}/laser -p
|
||||
serial_port:=/dev/ttyACM0
|
||||
@ -1,15 +0,0 @@
|
||||
# docker compose -f compose.ros2aria.yaml up
|
||||
services:
|
||||
ros2aria-dev:
|
||||
image: delicjusz/pioneer:humble-18012024
|
||||
network_mode: host
|
||||
ipc: host
|
||||
environment:
|
||||
- RMW_IMPLEMENTATION=rmw_fastrtps_cpp
|
||||
- LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/Aria/lib
|
||||
- FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml
|
||||
volumes:
|
||||
- ./fastdds.xml:/fastdds.xml
|
||||
devices:
|
||||
- /dev/ttyS0
|
||||
command: ros2 launch pioneer_bringup robot.launch.py namespace:=pioneer${PIONEER_ID}/
|
||||
@ -1,14 +0,0 @@
|
||||
[Unit]
|
||||
Description=%i service with docker compose
|
||||
PartOf=docker.service
|
||||
After=docker.service
|
||||
|
||||
[Service]
|
||||
Type=oneshot
|
||||
RemainAfterExit=true
|
||||
WorkingDirectory=/etc/docker/compose/%i
|
||||
ExecStart=/usr/bin/docker compose up -d --remove-orphans
|
||||
ExecStop=/usr/bin/docker compose down
|
||||
|
||||
[Install]
|
||||
WantedBy=multi-user.target
|
||||
25
fastdds.xml
25
fastdds.xml
@ -1,25 +0,0 @@
|
||||
<dds>
|
||||
<profiles>
|
||||
<participant profile_name="super_client_profile" is_default_profile="true">
|
||||
<rtps>
|
||||
<builtin>
|
||||
<discovery_config>
|
||||
<discoveryProtocol>SUPER_CLIENT</discoveryProtocol>
|
||||
<discoveryServersList>
|
||||
<RemoteServer prefix="44.53.00.5f.45.50.52.4f.53.49.4d.41">
|
||||
<metatrafficUnicastLocatorList>
|
||||
<locator>
|
||||
<udpv4>
|
||||
<address>10.104.16.240</address>
|
||||
<port>11811</port>
|
||||
</udpv4>
|
||||
</locator>
|
||||
</metatrafficUnicastLocatorList>
|
||||
</RemoteServer>
|
||||
</discoveryServersList>
|
||||
</discovery_config>
|
||||
</builtin>
|
||||
</rtps>
|
||||
</participant>
|
||||
</profiles>
|
||||
</dds>
|
||||
61
node_info_ros1.txt
Normal file
61
node_info_ros1.txt
Normal file
@ -0,0 +1,61 @@
|
||||
--------------------------------------------------------------------------------
|
||||
Node [/PIONIER6/RosAria]
|
||||
Publications:
|
||||
* /PIONIER6/RosAria/battery_recharge_state [std_msgs/Int8]
|
||||
* /PIONIER6/RosAria/battery_state_of_charge [std_msgs/Float32]
|
||||
* /PIONIER6/RosAria/bumper_state [rosaria/BumperState]
|
||||
* /PIONIER6/RosAria/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
|
||||
* /PIONIER6/RosAria/parameter_updates [dynamic_reconfigure/Config]
|
||||
* /PIONIER6/RosAria/pose [nav_msgs/Odometry]
|
||||
* /PIONIER6/RosAria/robot_info [rosaria_msgs/RobotInfoMsg]
|
||||
* /PIONIER6/RosAria/sonar [sensor_msgs/PointCloud]
|
||||
* /PIONIER6/RosAria/sonar_pointcloud2 [sensor_msgs/PointCloud2]
|
||||
* /PIONIER6/RosAria/wheels [sensor_msgs/JointState]
|
||||
* /rosout [rosgraph_msgs/Log]
|
||||
* /tf [tf2_msgs/TFMessage]
|
||||
|
||||
Subscriptions:
|
||||
* /PIONIER/master_stop [std_msgs/Bool]
|
||||
* /PIONIER/restrictions [rosaria_msgs/RestrictionsMsg]
|
||||
* /PIONIER6/RosAria/clutch [std_msgs/Bool]
|
||||
* /PIONIER6/RosAria/cmd_vel [unknown type]
|
||||
* /PIONIER6/RosAria/user_stop [std_msgs/Bool]
|
||||
|
||||
Services:
|
||||
* /PIONIER6/RosAria/get_loggers
|
||||
* /PIONIER6/RosAria/gripper_close
|
||||
* /PIONIER6/RosAria/gripper_down
|
||||
* /PIONIER6/RosAria/gripper_open
|
||||
* /PIONIER6/RosAria/gripper_up
|
||||
* /PIONIER6/RosAria/set_logger_level
|
||||
* /PIONIER6/RosAria/set_parameters
|
||||
|
||||
|
||||
contacting node http://10.104.16.45:35307/ ...
|
||||
Pid: 52
|
||||
Connections:
|
||||
* topic: /rosout
|
||||
* to: /rosout
|
||||
* direction: outbound
|
||||
* transport: TCPROS
|
||||
* topic: /PIONIER6/RosAria/robot_info
|
||||
* to: /master_plugin_lab15_19_12824_1114070233125035517
|
||||
* direction: outbound
|
||||
* transport: TCPROS
|
||||
* topic: /PIONIER/restrictions
|
||||
* to: /master_plugin_lab15_19_12824_1114070233125035517 (http://10.104.16.29:35537/)
|
||||
* direction: inbound
|
||||
* transport: TCPROS
|
||||
* topic: /PIONIER6/RosAria/user_stop
|
||||
* to: /user_plugin_lab15_19_15229_5138248156866338464 (http://10.104.16.29:43275/)
|
||||
* direction: inbound
|
||||
* transport: TCPROS
|
||||
* topic: /PIONIER/master_stop
|
||||
* to: /master_plugin_lab15_19_12824_1114070233125035517 (http://10.104.16.29:35537/)
|
||||
* direction: inbound
|
||||
* transport: TCPROS
|
||||
* topic: /PIONIER6/RosAria/clutch
|
||||
* to: /user_plugin_lab15_19_15229_5138248156866338464 (http://10.104.16.29:43275/)
|
||||
* direction: inbound
|
||||
* transport: TCPROS
|
||||
|
||||
2
run.sh
2
run.sh
@ -1,6 +1,6 @@
|
||||
#!/bin/bash
|
||||
export LD_LIBRARY_PATH=/usr/local/Aria/lib
|
||||
source /opt/ros/foxy/setup.bash
|
||||
source /ros2_ws/ros2aria/install/setup.bash
|
||||
source /ws/ros2aria/install/setup.bash
|
||||
ros2 run ros2aria ros2aria
|
||||
|
||||
|
||||
@ -1,11 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(pioneer_bringup)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(DIRECTORY
|
||||
launch
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@ -1,53 +0,0 @@
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch_ros.actions import Node
|
||||
from launch.substitutions import Command, LaunchConfiguration, PythonExpression
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||
import launch
|
||||
from launch.substitutions import Command, LaunchConfiguration
|
||||
import launch_ros
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.conditions import IfCondition
|
||||
from launch_ros.actions import Node, PushRosNamespace
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
pioneer_description = get_package_share_directory('pioneer_description')
|
||||
default_model_path = os.path.join(pioneer_description, 'urdf/pioneer3dx.urdf.xacro')
|
||||
|
||||
namespace = LaunchConfiguration('namespace')
|
||||
namespace_arg = DeclareLaunchArgument(
|
||||
'namespace',
|
||||
default_value='pioneer0/'
|
||||
)
|
||||
|
||||
robot_state_publisher_node = Node(
|
||||
namespace=namespace,
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
parameters=[
|
||||
{'robot_description': Command(['xacro ', default_model_path])},
|
||||
{'frame_prefix': namespace}
|
||||
]
|
||||
)
|
||||
|
||||
ros2aria = Node(
|
||||
namespace=namespace,
|
||||
package='ros2aria',
|
||||
executable='ros2aria',
|
||||
parameters=[
|
||||
{'use_sonar': False}
|
||||
]
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
namespace_arg,
|
||||
robot_state_publisher_node,
|
||||
ros2aria
|
||||
])
|
||||
@ -1,21 +0,0 @@
|
||||
<?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>pioneer_bringup</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="jakub.delicat@husarion.com">deli</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>xacro</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>robot_state_publisher</depend>
|
||||
|
||||
<depend>pioneer_description</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -1,14 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(pioneer_description)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(
|
||||
DIRECTORY
|
||||
meshes
|
||||
urdf
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
|
||||
ament_package()
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -1,27 +0,0 @@
|
||||
<package>
|
||||
<name>pioneer_description</name>
|
||||
<version>1.1.0</version>
|
||||
<description>URDF file descriptions for various Adept MobileRobots/ActivMedia robots</description>
|
||||
<maintainer email="reed.hedges@adept.com">Reed Hedges</maintainer>
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
<url type="website">http://wiki.ros.org/Robots/Pioneer</url>
|
||||
|
||||
<author email="hunter.allen@Vanderbilt.edu">Hunter Allen</author>
|
||||
<author email="dfseifer@usc.edu">David Feil-Seifer</author>
|
||||
<author email="reed.hedges@adept.com">Reed Hedges</author>
|
||||
<author>Brian Gerkey</author>
|
||||
<author>Kasper Stoy</author>
|
||||
<author>Richard Vaughan</author>
|
||||
<author>Andrew Howard</author>
|
||||
<author>Tucker Hermans</author>
|
||||
<author>ActivMedia Robotics LLC</author>
|
||||
<author>MobileRobots Inc.</author>
|
||||
<author email="jakub.delicat@pwr.edu.pl">Jakub Delicat</author>
|
||||
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -1,393 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<robot name="pioneer3dx"
|
||||
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
|
||||
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
|
||||
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
|
||||
xmlns:xacro="http://ros.org/wiki/xacro">
|
||||
|
||||
<!--<include filename="$(find amr_robots_description)/urdf/materials.xacro"/>-->
|
||||
<xacro:include filename="$(find pioneer_description)/urdf/pioneer3dx_wheel.xacro"/>
|
||||
|
||||
<!-- Chassis -->
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<mass value="3.5"/>
|
||||
<!--<origin xyz="-0.025 0 -0.223"/>-->
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="1" ixy="0" ixz="0"
|
||||
iyy="1" iyz="0"
|
||||
izz="1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="-0.045 0 0.148" rpy="0 0 0"/>
|
||||
<geometry name="pioneer_geom">
|
||||
<mesh filename="$(find pioneer_description)/meshes/chassis.stl"/>
|
||||
</geometry>
|
||||
<material name="ChassisRed">
|
||||
<color rgba="0.851 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.01 0.01 0.01"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<link name="top_plate">
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="1" ixy="0" ixz="0"
|
||||
iyy="1" iyz="0"
|
||||
izz="1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry name="pioneer_geom">
|
||||
<mesh filename="$(find pioneer_description)/meshes/top.stl"/>
|
||||
</geometry>
|
||||
<material name="TopBlack">
|
||||
<color rgba="0.038 0.038 0.038 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<gazebo reference="top_plate">
|
||||
<!-- material value="Gazebo/Black"/ -->
|
||||
<material>Gazebo/Black</material>
|
||||
</gazebo>
|
||||
|
||||
<joint name="base_top_joint" type="fixed">
|
||||
<origin xyz="-0.045 0 0.234" rpy="0 0 0"/>
|
||||
<axis xzy="0 0 1"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="top_plate"/>
|
||||
</joint>
|
||||
|
||||
<link name="front_sonar">
|
||||
<inertial>
|
||||
<mass value="0.0001"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="1" ixy="0" ixz="0"
|
||||
iyy="1" iyz="0" izz="1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry name="pioneer_geom">
|
||||
<mesh filename="$(find pioneer_description)/meshes/front_sonar.stl"/>
|
||||
</geometry>
|
||||
<material name="SonarYellow">
|
||||
<color rgba="0.715 0.583 0.210 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<gazebo reference="front_sonar">
|
||||
<material value="Gazebo/Yellow"/>
|
||||
</gazebo>
|
||||
|
||||
<joint name="base_front_joint" type="fixed">
|
||||
<origin xyz="-0.198 0 0.208" rpy="0 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="front_sonar"/>
|
||||
</joint>
|
||||
|
||||
<joint name="base_back_joint" type="fixed">
|
||||
<origin xyz="0.109 0 0.209" rpy="0 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="back_sonar"/>
|
||||
</joint>
|
||||
<link name="back_sonar">
|
||||
<inertial>
|
||||
<mass value="0"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="1" ixy="0" ixz="0"
|
||||
iyy="1" iyz="0" izz="1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry name="pioneer_geom">
|
||||
<mesh filename="$(find pioneer_description)/meshes/back_sonar.stl"/>
|
||||
</geometry>
|
||||
<material name="SonarYellow">
|
||||
<color rgba="0.715 0.583 0.210 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<!-- Caster -->
|
||||
<joint name="base_caster_swivel_joint" type="continuous">
|
||||
<origin xyz="-0.185 0 0.055" rpy="0 0 0"/>
|
||||
<anchor xyz="0 0 0"/>
|
||||
<limit effort="100" velocity="100" k_velocity="0" />
|
||||
<joint_properties damping="0.0" friction="0.0" />
|
||||
<parent link="base_link"/>
|
||||
<child link="caster_swivel"/>
|
||||
</joint>
|
||||
<link name="caster_swivel">
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0"
|
||||
iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry name="pioneer_geom">
|
||||
<mesh filename="$(find pioneer_description)/meshes/caster_swivel.stl"/>
|
||||
</geometry>
|
||||
<material name="caster_swivel">
|
||||
<color rgba="0.5 0.5 0.5 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<gazebo reference="caster_swivel">
|
||||
<material value="Gazebo/Grey"/>
|
||||
</gazebo>
|
||||
|
||||
<!-- Center Wheel + Hubcap -->
|
||||
<link name="caster_hubcap">
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983"
|
||||
iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry name="pioneer_geom">
|
||||
<mesh filename="$(find pioneer_description)/meshes/caster_hubcap.stl"/>
|
||||
</geometry>
|
||||
<material name="caster_swivel">
|
||||
<color rgba="0.5 0.5 0.5 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.05 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo reference="caster_hubcap">
|
||||
<material value="Gazebo/Grey"/>
|
||||
</gazebo>
|
||||
|
||||
<joint name="caster_swivel_hubcap_joint" type="continuous">
|
||||
<origin xyz="-0.026 0 -0.016" rpy="0 0 0"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<anchor xyz="0 0 0"/>
|
||||
<limit effort="100" velocity="100" k_velocity="0" />
|
||||
<joint_properties damping="0.0" friction="0.0" />
|
||||
<parent link="caster_swivel"/>
|
||||
<child link="caster_wheel"/>
|
||||
</joint>
|
||||
|
||||
<link name="caster_wheel">
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983"
|
||||
iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry name="pioneer_geom">
|
||||
<mesh filename="$(find pioneer_description)/meshes/caster_wheel.stl"/>
|
||||
</geometry>
|
||||
<material name="WheelBlack">
|
||||
<color rgba="0.117 0.117 0.117 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="${-3.1415927/2.0} 0 0"/>
|
||||
<geometry>
|
||||
<!--<mesh filename="$(find pioneer_description)/meshes/caster_wheel.stl"/>-->
|
||||
<cylinder radius="0.0375" length="0.01"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo reference="left_hub">
|
||||
<material value="Gazebo/Yellow"/>
|
||||
</gazebo>
|
||||
|
||||
<joint name="caster_wheel_joint" type="fixed">
|
||||
<origin xyz="-0.0035 0 -0.001" rpy="0 0 0"/>
|
||||
<parent link="caster_wheel"/>
|
||||
<child link="caster_hubcap"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_hub">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="$(find pioneer_description)/meshes/left_hubcap.stl"/>
|
||||
</geometry>
|
||||
<material name="HubcapYellow">
|
||||
<color rgba="1.0 0.811 0.151 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="${-3.1415927/2.0} 0 0"/>
|
||||
<geometry>
|
||||
<!--<mesh filename="$(find pioneer_description)/meshes/caster_wheel.stl"/>-->
|
||||
<cylinder radius="0.09" length="0.01"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="left_hub_joint" type="fixed">
|
||||
<origin xyz="0 0.15 0.08" rpy="0 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="left_hub"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_wheel">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="$(find pioneer_description)/meshes/left_wheel.stl"/>
|
||||
</geometry>
|
||||
<material name="WheelBlack">
|
||||
<color rgba="0.117 0.117 0.117 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="left_wheel_joint" type="continuous"> <!-- type="continuous" -->
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<parent link="left_hub"/>
|
||||
<child link="left_wheel"/>
|
||||
</joint>
|
||||
|
||||
<gazebo reference="left_wheel_joint">
|
||||
<material value="Gazebo/Black"/>
|
||||
</gazebo>
|
||||
|
||||
<link name="right_hub">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="$(find pioneer_description)/meshes/right_hubcap.stl"/>
|
||||
</geometry>
|
||||
<material name="HubcapYellow">
|
||||
<color rgba="1.0 0.811 0.151 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="${-3.1415927/2.0} 0 0"/>
|
||||
<geometry>
|
||||
<!--<mesh filename="$(find pioneer_description)/meshes/caster_wheel.stl"/>-->
|
||||
<cylinder radius="0.09" length="0.01"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo reference="right_hub">
|
||||
<material value="Gazebo/Yellow"/>
|
||||
</gazebo>
|
||||
|
||||
<joint name="right_hub_joint" type="fixed">
|
||||
<origin xyz="0 -0.15 0.08" rpy="0 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="right_hub"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_wheel">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="$(find pioneer_description)/meshes/right_wheel.stl"/>
|
||||
</geometry>
|
||||
<material name="WheelBlack">
|
||||
<color rgba="0.117 0.117 0.117 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="right_wheel_joint" type="continuous"> <!-- type="continuous" -->
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<parent link="right_hub"/>
|
||||
<child link="right_wheel"/>
|
||||
</joint>
|
||||
<gazebo reference="right_wheel_joint">
|
||||
<material value="Gazebo/Black"/>
|
||||
</gazebo>
|
||||
|
||||
<link name="laser">
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.1 0.1 0.1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://amr_robots_description/meshes/p3dx_meshes/hokuyo_convex.stl"/>
|
||||
</geometry>
|
||||
<material name="Hokuyo">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<inertial>
|
||||
<mass value="1e-5" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="hokuyo_joint" type="fixed">
|
||||
<axis xyz="0 0 0" />
|
||||
<origin xyz="0.040 0 0.0625" rpy="0 0 0"/>
|
||||
<parent link="front_sonar"/>
|
||||
<child link="laser"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="camera_frame">
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.095 0.03 0.03"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="camera_joint" type="fixed">
|
||||
<axis xyz="0 0 0" />
|
||||
<origin xyz="0.040 0 0.1125" rpy="0 0 0"/>
|
||||
<parent link="front_sonar"/>
|
||||
<child link="camera_frame"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<create>
|
||||
<back_sonar parent="base_link"/>
|
||||
<top_plate parent="base_link"/>
|
||||
</create>
|
||||
|
||||
<gazebo>
|
||||
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
|
||||
<alwaysOn>true</alwaysOn>
|
||||
<updateRate>100</updateRate>
|
||||
<leftJoint>left_wheel_joint</leftJoint>
|
||||
<rightJoint>right_wheel_joint</rightJoint>
|
||||
<wheelSeparation>0.158</wheelSeparation>
|
||||
<wheelDiameter>0.12</wheelDiameter>
|
||||
<torque>5</torque>
|
||||
<!-- interface:position name="position_iface_0"/ -->
|
||||
<commandTopic>cmd_vel</commandTopic>
|
||||
<robotBaseFrame>base_link</robotBaseFrame>
|
||||
<odometryTopic>odom</odometryTopic>
|
||||
<odometryFrame>odom</odometryFrame>
|
||||
</plugin>
|
||||
|
||||
<!-- TODO include P3D (ground truth) plugin -->
|
||||
|
||||
|
||||
<!-- XXX old urdf included a gazebo_ros_controller_manager plugin with a 1 second update rate -->
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
@ -1,108 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<robot
|
||||
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
|
||||
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
|
||||
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
|
||||
xmlns:xacro="http://ros.org/wiki/xacro"
|
||||
>
|
||||
|
||||
<!--<include filename="$(find amr_robots_description)/urdf/materials.xacro"/>-->
|
||||
|
||||
<!-- Properties (Constants) -->
|
||||
<property name="M_PI" value="3.14159"/>
|
||||
|
||||
<!-- Right/Left Hubcap + Wheel -->
|
||||
<xacro:macro name="p3dx_wheel" params="suffix reflect">
|
||||
<link name="p3dx_${suffix}_wheel">
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.012411765597" ixy="0" ixz="0"
|
||||
iyy="0.015218160428" iyz="0" izz="0.011763977943"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry name="pioneer_geom">
|
||||
<mesh filename="$(find pioneer_description)/meshes/meshes/p3dx_meshes/${suffix}_wheel.stl"/>
|
||||
</geometry>
|
||||
<material name="WheelBlack">
|
||||
<color rgba="0.117 0.117 0.117 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="${-3.1415927/2} 0 0"/>
|
||||
<geometry>
|
||||
<!--<mesh filename="$(find pioneer_description)/meshes/meshes/p3dx_meshes/${suffix}_wheel.stl"/>-->
|
||||
<cylinder radius="0.09" length="0.01"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="base_${suffix}_hubcap_joint" type="fixed">
|
||||
<!--<origin xyz="0 ${reflect*0.158} 0.091" rpy="0 0 0"/>-->
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
|
||||
<parent link="p3dx_${suffix}_wheel"/>
|
||||
<child link="p3dx_${suffix}_hubcap"/>
|
||||
</joint>
|
||||
|
||||
<link name="p3dx_${suffix}_hubcap">
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="0.012411765597" ixy="0" ixz="0"
|
||||
iyy="0.015218160428" iyz="0" izz="0.011763977943"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry name="pioneer_geom">
|
||||
<mesh filename="$(find pioneer_description)/meshes/meshes/p3dx_meshes/${suffix}_hubcap.stl"/>
|
||||
</geometry>
|
||||
<material name="HubcapYellow">
|
||||
<color rgba="1.0 0.811 0.151 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0 0 0"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<transmission type="pr2_mechanism_model/SimpleTransmission" name="${suffix}_wheel_trans">
|
||||
<actuator name="base_${suffix}_wheel_motor" />
|
||||
<joint name="base_${suffix}_wheel_joint" />
|
||||
<mechanicalReduction>${reflect * 624/35 * 80/19}</mechanicalReduction>
|
||||
</transmission>
|
||||
|
||||
<joint name="base_${suffix}_wheel_joint" type="continuous">
|
||||
<axis xyz="0 1 0"/>
|
||||
<anchor xyz="0 0 0"/>
|
||||
<limit effort="100" velocity="100" />
|
||||
<joint_properties damping="0.0" friction="0.0" />
|
||||
<origin xyz="0 ${reflect*0.158} 0.091" rpy="0 0 0"/>
|
||||
<parent link="p3dx_${suffix}_hubcap"/>
|
||||
<child link="p3dx_${suffix}_wheel"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<gazebo reference="p3dx_${suffix}_hubcap">
|
||||
<material value="Gazebo/Yellow"/>
|
||||
</gazebo>
|
||||
|
||||
|
||||
<gazebo reference="p3dx_${suffix}_wheel">
|
||||
<material value="Gazebo/Black"/>
|
||||
<elem key="mu1" value="0.5" />
|
||||
<elem key="mu2" value="50.0" />
|
||||
<elem key="kp" value="100000000.0" />
|
||||
<elem key="kd" value="1.0" />
|
||||
</gazebo>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
|
||||
</robot>
|
||||
|
||||
@ -23,9 +23,10 @@ find_package(geometry_msgs REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(tf2 REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(tf2_geometry_msgs REQUIRED)
|
||||
find_package(ros2aria_msgs REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
|
||||
|
||||
add_executable(ros2aria
|
||||
@ -39,10 +40,7 @@ add_executable(ros2aria
|
||||
src/gripper.cpp
|
||||
src/wheels.cpp
|
||||
src/clutch.cpp
|
||||
src/state.cpp
|
||||
src/restrictions.cpp
|
||||
src/scan.cpp
|
||||
)
|
||||
src/state.cpp)
|
||||
|
||||
ament_target_dependencies(ros2aria rclcpp)
|
||||
ament_target_dependencies(ros2aria std_srvs)
|
||||
@ -50,8 +48,6 @@ ament_target_dependencies(ros2aria geometry_msgs)
|
||||
ament_target_dependencies(ros2aria sensor_msgs)
|
||||
ament_target_dependencies(ros2aria nav_msgs)
|
||||
ament_target_dependencies(ros2aria tf2)
|
||||
ament_target_dependencies(ros2aria tf2_ros)
|
||||
ament_target_dependencies(ros2aria tf2_geometry_msgs)
|
||||
ament_target_dependencies(ros2aria ros2aria_msgs)
|
||||
|
||||
target_include_directories(ros2aria PUBLIC
|
||||
|
||||
@ -1,36 +0,0 @@
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "Aria/Aria.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
|
||||
class RAIIBot {
|
||||
public:
|
||||
typedef std::shared_ptr<RAIIBot> SharedPtr;
|
||||
|
||||
RAIIBot(rclcpp::Node *node, std::string port);
|
||||
~RAIIBot();
|
||||
ArRobot *getRobot();
|
||||
ArGripper *getGripper();
|
||||
rclcpp::Clock::SharedPtr getClock();
|
||||
void lock();
|
||||
void unlock();
|
||||
void pokeWatchdog();
|
||||
|
||||
private:
|
||||
ArRobot *robot = nullptr;
|
||||
ArRobotConnector *robotConn = nullptr;
|
||||
ArArgumentBuilder *args = nullptr;
|
||||
ArArgumentParser *argparser = nullptr;
|
||||
ArGripper *gripper = nullptr;
|
||||
rclcpp::Node *node = nullptr;
|
||||
rclcpp::TimerBase::SharedPtr watchdogTimer;
|
||||
rclcpp::Time lastWatchdogPoke;
|
||||
rclcpp::Clock::SharedPtr clock;
|
||||
|
||||
rclcpp::Logger get_logger();
|
||||
void startWatchdog();
|
||||
void watchdog_cb();
|
||||
};
|
||||
@ -15,7 +15,6 @@
|
||||
<depend>rclcpp</depend>
|
||||
<depend>std_srvs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>tf2_geometry_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>ros2aria_msgs</depend>
|
||||
|
||||
@ -1,6 +1,7 @@
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include "ros2aria.hpp"
|
||||
|
||||
void Ros2Aria::clutch_callback(const std_msgs::msg::Bool::SharedPtr msg) {
|
||||
void Ros2Aria::clutch_callback(const std_msgs::msg::Bool::SharedPtr msg)
|
||||
{
|
||||
std::lock_guard<RAIIBot> lock(*robot.get());
|
||||
|
||||
auto r = robot->getRobot();
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include <cmath>
|
||||
#include "ros2aria.hpp"
|
||||
|
||||
void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) {
|
||||
void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
|
||||
{
|
||||
float x, y, z;
|
||||
x = msg->linear.x;
|
||||
y = msg->linear.y;
|
||||
@ -13,23 +13,8 @@ void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
|
||||
this->robot->pokeWatchdog();
|
||||
|
||||
auto r = robot->getRobot();
|
||||
|
||||
if (master_stop or obstacle_too_close or user_stop){
|
||||
x = 0.0;
|
||||
y = 0.0;
|
||||
z = 0.0;
|
||||
}
|
||||
else{
|
||||
// apply limits
|
||||
x = std::abs(x) > restrictions.linear_velocity.data ? std::abs(x)/x * restrictions.linear_velocity.data : x;
|
||||
y = std::abs(y) > restrictions.linear_velocity.data ? std::abs(y)/y * restrictions.linear_velocity.data : y;
|
||||
z = std::abs(z) > restrictions.angular_velocity.data ? std::abs(z)/z * restrictions.angular_velocity.data : z;
|
||||
}
|
||||
x *= 1e3;
|
||||
y *= 1e3;
|
||||
z *= 180 / M_PI;
|
||||
r->setVel(x);
|
||||
r->setVel(x * 1e3);
|
||||
if (r->hasLatVel())
|
||||
r->setLatVel(y);
|
||||
r->setRotVel(z);
|
||||
r->setLatVel(y * 1e3);
|
||||
r->setRotVel(z * 180 / M_PI);
|
||||
}
|
||||
|
||||
@ -1,30 +1,31 @@
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include "./ros2aria.hpp"
|
||||
|
||||
void Ros2Aria::gripper_open_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const {
|
||||
void Ros2Aria::gripper_open_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const
|
||||
{
|
||||
UNUSED(request);
|
||||
UNUSED(response);
|
||||
std::lock_guard<RAIIBot> lock(*robot.get());
|
||||
auto g = robot->getGripper();
|
||||
g->gripOpen();
|
||||
}
|
||||
|
||||
void Ros2Aria::gripper_close_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const {
|
||||
void Ros2Aria::gripper_close_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const
|
||||
{
|
||||
UNUSED(request);
|
||||
UNUSED(response);
|
||||
std::lock_guard<RAIIBot> lock(*robot.get());
|
||||
auto g = robot->getGripper();
|
||||
g->gripClose();
|
||||
}
|
||||
|
||||
void Ros2Aria::gripper_up_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const {
|
||||
void Ros2Aria::gripper_up_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const
|
||||
{
|
||||
UNUSED(request);
|
||||
UNUSED(response);
|
||||
std::lock_guard<RAIIBot> lock(*robot.get());
|
||||
auto g = robot->getGripper();
|
||||
g->liftUp();
|
||||
}
|
||||
|
||||
void Ros2Aria::gripper_down_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const {
|
||||
void Ros2Aria::gripper_down_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const
|
||||
{
|
||||
UNUSED(request);
|
||||
UNUSED(response);
|
||||
std::lock_guard<RAIIBot> lock(*robot.get());
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
#include <cstdio>
|
||||
#include <memory>
|
||||
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include "ros2aria.hpp"
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
|
||||
@ -1,47 +1,46 @@
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include "ros2aria.hpp"
|
||||
#include "tf2/utils.h"
|
||||
#include "tf2/transform_datatypes.h"
|
||||
|
||||
std::pair<nav_msgs::msg::Odometry, geometry_msgs::msg::TransformStamped> Ros2Aria::handlePose(rclcpp::Time stamp) {
|
||||
nav_msgs::msg::Odometry odom_msg;
|
||||
inline void convert_transform_to_pose(const tf2::Transform &trans, geometry_msgs::msg::Pose &pose)
|
||||
{
|
||||
geometry_msgs::msg::Transform msg = tf2::toMsg(trans);
|
||||
pose.orientation = msg.rotation;
|
||||
pose.position.x = msg.translation.x;
|
||||
pose.position.y = msg.translation.y;
|
||||
pose.position.z = msg.translation.z;
|
||||
}
|
||||
|
||||
nav_msgs::msg::Odometry Ros2Aria::handlePose(rclcpp::Time stamp)
|
||||
{
|
||||
nav_msgs::msg::Odometry pose;
|
||||
|
||||
auto r = robot->getRobot();
|
||||
ArPose p = r->getPose();
|
||||
|
||||
tf2::Quaternion rotation;
|
||||
auto position = tf2::Vector3(p.getX() / 1000, p.getY() / 1000, 0);
|
||||
rotation.setRPY(0, 0, p.getTh() * M_PI / 180);
|
||||
|
||||
odom_msg.pose.pose.orientation = tf2::toMsg(rotation);
|
||||
odom_msg.pose.pose.position.x = position.getX();
|
||||
odom_msg.pose.pose.position.y = position.getY();
|
||||
odom_msg.pose.pose.position.z = position.getZ();
|
||||
tf2::Vector3 position = tf2::Vector3(p.getX() / 1000, p.getY() / 1000, 0);
|
||||
|
||||
odom_msg.twist.twist.linear.x = r->getVel() / 1000;
|
||||
odom_msg.twist.twist.linear.y = r->getLatVel() / 1000;
|
||||
odom_msg.twist.twist.angular.z = r->getRotVel() * M_PI / 180;
|
||||
convert_transform_to_pose(tf2::Transform(rotation, position),
|
||||
pose.pose.pose);
|
||||
|
||||
odom_msg.header.frame_id = std::string(get_namespace()) + "/odom";
|
||||
odom_msg.child_frame_id = std::string(get_namespace()) + "/base_link";
|
||||
odom_msg.header.stamp = stamp;
|
||||
pose.twist.twist.linear.x = r->getVel() / 1000;
|
||||
pose.twist.twist.linear.y = r->getLatVel() / 1000;
|
||||
pose.twist.twist.angular.z = r->getRotVel() * M_PI / 180;
|
||||
|
||||
geometry_msgs::msg::TransformStamped transform;
|
||||
transform.header = odom_msg.header;
|
||||
transform.child_frame_id= odom_msg.child_frame_id;
|
||||
transform.transform.translation.x = odom_msg.pose.pose.position.x;
|
||||
transform.transform.translation.y = odom_msg.pose.pose.position.y;
|
||||
transform.transform.translation.z = odom_msg.pose.pose.position.z;
|
||||
transform.transform.rotation = odom_msg.pose.pose.orientation;
|
||||
pose.header.frame_id = "header.frame_id"; // TODO: use correct frame_id
|
||||
pose.child_frame_id = "child_frame_id"; // TODO: use correct child_frame_id
|
||||
pose.header.stamp = stamp;
|
||||
|
||||
std::pair<nav_msgs::msg::Odometry, geometry_msgs::msg::TransformStamped> pair_msgs = {odom_msg, transform};
|
||||
return pair_msgs;
|
||||
return pose;
|
||||
}
|
||||
|
||||
void Ros2Aria::publishPose(nav_msgs::msg::Odometry pose) {
|
||||
void Ros2Aria::publishPose(nav_msgs::msg::Odometry pose)
|
||||
{
|
||||
if (this->pose_pub_->get_subscription_count() == 0)
|
||||
return;
|
||||
|
||||
this->pose_pub_->publish(pose);
|
||||
}
|
||||
|
||||
void Ros2Aria::publishTf(geometry_msgs::msg::TransformStamped tf){
|
||||
odom_tf_broadcaster_->sendTransform(tf);
|
||||
}
|
||||
@ -1,21 +1,19 @@
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include "ros2aria.hpp"
|
||||
|
||||
void Ros2Aria::publish()
|
||||
{
|
||||
// RCLCPP_INFO(this->get_logger(), "publish");
|
||||
rclcpp::Time t = robot->getClock()->now();
|
||||
|
||||
if(use_sonar){
|
||||
sensor_msgs::msg::PointCloud sonarData = handleSonar(t);
|
||||
publishSonar(sonarData);
|
||||
publishSonarPointCloud2(sonarData);
|
||||
}
|
||||
|
||||
auto pose = handlePose(t);
|
||||
publishPose(pose.first);
|
||||
publishTf(pose.second);
|
||||
nav_msgs::msg::Odometry pose = handlePose(t);
|
||||
publishPose(pose);
|
||||
|
||||
sensor_msgs::msg::JointState wheels = handleWheels(t);
|
||||
publishWheels(wheels);
|
||||
|
||||
publishState();
|
||||
publishState(t);
|
||||
}
|
||||
@ -1,7 +1,19 @@
|
||||
#include "ros2aria/raiibot.hpp"
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <iostream>
|
||||
|
||||
#include "Aria/Aria.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
RAIIBot::RAIIBot(rclcpp::Node *node, std::string port) {
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
class RAIIBot
|
||||
{
|
||||
public:
|
||||
typedef std::shared_ptr<RAIIBot> SharedPtr;
|
||||
|
||||
RAIIBot(rclcpp::Node *node, std::string port)
|
||||
{
|
||||
this->robot = new ArRobot();
|
||||
this->gripper = new ArGripper(this->robot);
|
||||
this->node = node;
|
||||
@ -10,15 +22,16 @@ RAIIBot::RAIIBot(rclcpp::Node *node, std::string port) {
|
||||
args = new ArArgumentBuilder();
|
||||
this->argparser = new ArArgumentParser(args);
|
||||
argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args. Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
|
||||
std::string port_arg = "-robotPort " + port;
|
||||
args->add(port_arg.c_str()); // pass robot's serial port to Aria // TODO: use `port` variable.
|
||||
args->add("-robotPort /dev/ttyS0"); // pass robot's serial port to Aria // TODO: use `port` variable.
|
||||
// args->add("-robotLogPacketsReceived"); // log received packets
|
||||
// args->add("-robotLogPacketsSent"); // log sent packets
|
||||
// args->add("-robotLogVelocitiesReceived"); // log received velocities
|
||||
// args->add("-robotLogMovementSent");
|
||||
// args->add("-robotLogMovementReceived");
|
||||
this->robotConn = new ArRobotConnector(argparser, robot);
|
||||
if (!this->robotConn->connectRobot()) {
|
||||
if (!this->robotConn->connectRobot())
|
||||
{
|
||||
|
||||
// RCLCPP_INFO(this->get_logger(), "RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device, or any errors reported above)");
|
||||
// rclcpp::shutdown();
|
||||
}
|
||||
@ -29,18 +42,21 @@ RAIIBot::RAIIBot(rclcpp::Node *node, std::string port) {
|
||||
this->startWatchdog();
|
||||
}
|
||||
|
||||
RAIIBot ::~RAIIBot() {
|
||||
~RAIIBot()
|
||||
{
|
||||
std::cout << std::endl
|
||||
<< "RAIIBOT DESTRUCTOR RUNNING!" << std::endl;
|
||||
|
||||
if (this->robot != nullptr) {
|
||||
if (this->robot != nullptr)
|
||||
{
|
||||
this->robot->lock();
|
||||
std::cout << "disabling motors" << std::endl;
|
||||
this->robot->disableMotors();
|
||||
std::cout << "disabled motors" << std::endl;
|
||||
Aria::shutdown();
|
||||
}
|
||||
if (this->robotConn != nullptr) {
|
||||
if (this->robotConn != nullptr)
|
||||
{
|
||||
std::cout << "disconnecting" << std::endl;
|
||||
this->robotConn->disconnectAll();
|
||||
std::cout << "disconnected" << std::endl;
|
||||
@ -56,48 +72,66 @@ RAIIBot ::~RAIIBot() {
|
||||
delete this->robot;
|
||||
}
|
||||
|
||||
ArRobot *RAIIBot::getRobot() {
|
||||
ArRobot *getRobot()
|
||||
{
|
||||
return this->robot;
|
||||
}
|
||||
|
||||
ArGripper *RAIIBot::getGripper() {
|
||||
ArGripper *getGripper()
|
||||
{
|
||||
return this->gripper;
|
||||
}
|
||||
|
||||
rclcpp::Clock::SharedPtr RAIIBot::getClock() {
|
||||
rclcpp::Clock::SharedPtr getClock()
|
||||
{
|
||||
return this->clock;
|
||||
}
|
||||
|
||||
void RAIIBot::lock() {
|
||||
void lock()
|
||||
{
|
||||
if (this->robot != nullptr)
|
||||
this->robot->lock();
|
||||
}
|
||||
|
||||
void RAIIBot::unlock() {
|
||||
void unlock()
|
||||
{
|
||||
if (this->robot != nullptr)
|
||||
this->robot->unlock();
|
||||
}
|
||||
|
||||
void RAIIBot::pokeWatchdog() {
|
||||
void pokeWatchdog()
|
||||
{
|
||||
// this should probably be made thread safe, or something?
|
||||
this->lastWatchdogPoke = this->clock->now();
|
||||
}
|
||||
|
||||
rclcpp::Logger RAIIBot::get_logger() {
|
||||
private:
|
||||
ArRobot *robot = nullptr;
|
||||
ArRobotConnector *robotConn = nullptr;
|
||||
ArArgumentBuilder *args = nullptr;
|
||||
ArArgumentParser *argparser = nullptr;
|
||||
ArGripper *gripper = nullptr;
|
||||
rclcpp::Node *node = nullptr;
|
||||
rclcpp::TimerBase::SharedPtr watchdogTimer;
|
||||
rclcpp::Time lastWatchdogPoke;
|
||||
rclcpp::Clock::SharedPtr clock;
|
||||
|
||||
rclcpp::Logger get_logger()
|
||||
{
|
||||
return this->node->get_logger();
|
||||
}
|
||||
|
||||
void RAIIBot::startWatchdog() {
|
||||
using namespace std::chrono_literals;
|
||||
void startWatchdog()
|
||||
{
|
||||
this->lastWatchdogPoke = clock->now();
|
||||
watchdogTimer = this->node->create_wall_timer(0.1s, std::bind(&RAIIBot::watchdog_cb, this));
|
||||
}
|
||||
|
||||
void RAIIBot::watchdog_cb() {
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
void watchdog_cb()
|
||||
{
|
||||
auto now = this->clock->now();
|
||||
if (now - this->lastWatchdogPoke > 1s) {
|
||||
if (now - this->lastWatchdogPoke > 1s)
|
||||
{
|
||||
std::lock_guard<RAIIBot> lock(*this);
|
||||
auto r = this->getRobot();
|
||||
r->setVel(0);
|
||||
@ -106,3 +140,4 @@ void RAIIBot::watchdog_cb() {
|
||||
r->setLatVel(0);
|
||||
}
|
||||
}
|
||||
};
|
||||
@ -1,21 +0,0 @@
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include <limits>
|
||||
void Ros2Aria::restrictions_callback(const ros2aria_msgs::msg::RestrictionsMsg::SharedPtr msg){
|
||||
restrictions = *msg;
|
||||
RCLCPP_INFO(this->get_logger(), "restrictions: x:%f y:%f z:%f", restrictions.linear_velocity.data, restrictions.angular_velocity.data,
|
||||
restrictions.distance.data);
|
||||
}
|
||||
|
||||
void Ros2Aria::init_restrictions(){
|
||||
restrictions.linear_velocity.data = 0.0;
|
||||
restrictions.angular_velocity.data = 0.0;
|
||||
restrictions.distance.data = std::numeric_limits<float>::max();
|
||||
}
|
||||
|
||||
void Ros2Aria::user_stop_callback(const std_msgs::msg::Bool::SharedPtr msg){
|
||||
user_stop = msg->data;
|
||||
}
|
||||
|
||||
void Ros2Aria::master_stop_callback(const std_msgs::msg::Bool::SharedPtr msg){
|
||||
master_stop = msg->data;
|
||||
}
|
||||
@ -1,7 +1,5 @@
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <sstream>
|
||||
#include "ros2aria.hpp"
|
||||
|
||||
using std::placeholders::_1;
|
||||
using std::placeholders::_2;
|
||||
|
||||
@ -9,42 +7,20 @@ Ros2Aria::Ros2Aria()
|
||||
: Node("ros2aria"),
|
||||
sensorCb(this, &Ros2Aria::publish)
|
||||
{
|
||||
id = extract_number_from_string(std::string(get_namespace()));
|
||||
|
||||
this->robot = std::make_shared<RAIIBot>(this, "/dev/ttyS0");
|
||||
handle_parameters();
|
||||
this->robot = std::make_shared<RAIIBot>(this, "/dev/ttyS0"); // TODO: use args for port
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "starting subscribers and services");
|
||||
|
||||
restrictions_sub_ = this->create_subscription<ros2aria_msgs::msg::RestrictionsMsg>(
|
||||
"/pioneers/restrictions", 10, std::bind(&Ros2Aria::restrictions_callback, this, _1));
|
||||
init_restrictions();
|
||||
|
||||
master_stop_sub_ = this->create_subscription<std_msgs::msg::Bool>(
|
||||
"/pioneers/master_stop", 10, std::bind(&Ros2Aria::master_stop_callback, this, _1));
|
||||
user_stop_sub_ = this->create_subscription<std_msgs::msg::Bool>(
|
||||
"user_stop", 10, std::bind(&Ros2Aria::user_stop_callback, this, _1));
|
||||
cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
|
||||
"cmd_vel", 10, std::bind(&Ros2Aria::cmd_vel_callback, this, _1));
|
||||
clutch_sub_ = this->create_subscription<std_msgs::msg::Bool>(
|
||||
"clutch", 10, std::bind(&Ros2Aria::clutch_callback, this, _1));
|
||||
scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
|
||||
"scan", 10, std::bind(&Ros2Aria::scan_callback, this, _1));
|
||||
|
||||
auto r = robot->getRobot();
|
||||
if(use_sonar){
|
||||
sonar_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud>("sonar", 10);
|
||||
sonar_pointcloud2_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("sonar_pointcloud2", 10);
|
||||
}
|
||||
else{
|
||||
r->disableSonar();
|
||||
}
|
||||
r->disableMotors();
|
||||
|
||||
pose_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("odom", 1000);
|
||||
odom_tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
|
||||
pose_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("pose", 1000);
|
||||
|
||||
wheels_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", 1000);
|
||||
wheels_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("wheels", 1000);
|
||||
|
||||
battery_recharge_state_pub_ = this->create_publisher<std_msgs::msg::Int8>("battery_recharge_state", 10);
|
||||
battery_state_of_charge_pub_ = this->create_publisher<std_msgs::msg::Float32>("battery_state_of_charge", 10);
|
||||
@ -57,35 +33,11 @@ Ros2Aria::Ros2Aria()
|
||||
gripper_up_service_ = this->create_service<std_srvs::srv::Empty>("gripper_up", std::bind(&Ros2Aria::gripper_up_callback, this, _1, _2));
|
||||
gripper_down_service_ = this->create_service<std_srvs::srv::Empty>("gripper_down", std::bind(&Ros2Aria::gripper_down_callback, this, _1, _2));
|
||||
|
||||
// listen to sensors
|
||||
auto r = robot->getRobot();
|
||||
r->addSensorInterpTask("ROSPublishingTask", 100, &this->sensorCb);
|
||||
|
||||
RCLCPP_INFO(get_logger(), "NAMESPACE = %s", get_namespace());
|
||||
RCLCPP_INFO(get_logger(), "ID = %d", id);
|
||||
|
||||
}
|
||||
|
||||
std::size_t Ros2Aria::extract_number_from_string(const std::string& str) {
|
||||
std::string numericPart;
|
||||
for (char c : str) {
|
||||
if (isdigit(c)) {
|
||||
numericPart += c;
|
||||
} else if (!numericPart.empty()) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
int number;
|
||||
std::istringstream(numericPart) >> number;
|
||||
return number;
|
||||
}
|
||||
|
||||
void Ros2Aria::handle_parameters(){
|
||||
declare_parameter("use_sonar", true);
|
||||
declare_parameter("num_of_sonars", 0);
|
||||
use_sonar = get_parameter("use_sonar").as_bool();
|
||||
num_of_sonars = get_parameter("num_of_sonars").as_int();
|
||||
|
||||
RCLCPP_INFO(get_logger(), "use_sonar = %d", use_sonar);
|
||||
RCLCPP_INFO(get_logger(), "num_of_sonars = %d", num_of_sonars);
|
||||
sonar_frame_id = "pionier6"; // TODO: use args
|
||||
}
|
||||
|
||||
Ros2Aria::~Ros2Aria()
|
||||
|
||||
@ -1,39 +1,21 @@
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
|
||||
#include <utility>
|
||||
#include<string>
|
||||
|
||||
#include "ros2aria/raiibot.hpp"
|
||||
#include <Aria/Aria.h>
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "nav_msgs/msg/odometry.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "ros2aria_msgs/msg/robot_info_msg.hpp"
|
||||
#include "ros2aria_msgs/msg/restrictions_msg.hpp"
|
||||
#include "sensor_msgs/msg/joint_state.hpp"
|
||||
#include "Aria/Aria.h"
|
||||
|
||||
#include "sensor_msgs/msg/point_cloud.hpp"
|
||||
#include "sensor_msgs/msg/point_cloud2.hpp"
|
||||
#include "sensor_msgs/msg/laser_scan.hpp"
|
||||
#include "std_msgs/msg/bool.hpp"
|
||||
#include "std_msgs/msg/float32.hpp"
|
||||
#include "std_msgs/msg/int8.hpp"
|
||||
#include "sensor_msgs/msg/joint_state.hpp"
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "nav_msgs/msg/odometry.hpp"
|
||||
#include "std_srvs/srv/empty.hpp"
|
||||
#include "tf2_ros/transform_broadcaster.h"
|
||||
#include "tf2/LinearMath/Quaternion.h"
|
||||
#include "std_msgs/msg/bool.hpp"
|
||||
#include "std_msgs/msg/int8.hpp"
|
||||
#include "std_msgs/msg/float32.hpp"
|
||||
|
||||
#include "ros2aria_msgs/msg/robot_info_msg.hpp"
|
||||
#include "./raiibot.cpp"
|
||||
|
||||
#define UNUSED(x) (void)(x)
|
||||
|
||||
constexpr double mm_per_sec_to_rad_per_sec(double mm_per_sec){
|
||||
return mm_per_sec / 195.0 / 2.0; /*diameter in mm*/
|
||||
}
|
||||
|
||||
constexpr double encoder_to_rad(long int encoder){
|
||||
return static_cast<double>(encoder) * M_PI / 32768.0;
|
||||
}
|
||||
|
||||
constexpr float distance(float a, float b){
|
||||
return std::sqrt(a*a + b*b);
|
||||
}
|
||||
|
||||
class Ros2Aria : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
@ -41,20 +23,14 @@ public:
|
||||
~Ros2Aria();
|
||||
|
||||
private:
|
||||
std::size_t id;
|
||||
// robot
|
||||
RAIIBot::SharedPtr robot;
|
||||
ArFunctorC<Ros2Aria> sensorCb;
|
||||
ros2aria_msgs::msg::RestrictionsMsg restrictions;
|
||||
float minimal_distance = 0.0;
|
||||
bool obstacle_too_close = true;
|
||||
bool master_stop = true;
|
||||
bool user_stop = true;
|
||||
bool use_sonar = true;
|
||||
uint8_t num_of_sonars = 0;
|
||||
|
||||
std::size_t extract_number_from_string(const std::string& str);
|
||||
// config
|
||||
std::string sonar_frame_id;
|
||||
|
||||
void handle_parameters();
|
||||
// publishers
|
||||
void publish();
|
||||
|
||||
sensor_msgs::msg::PointCloud handleSonar(rclcpp::Time stamp);
|
||||
@ -63,11 +39,9 @@ private:
|
||||
void publishSonar(sensor_msgs::msg::PointCloud cloud);
|
||||
void publishSonarPointCloud2(sensor_msgs::msg::PointCloud cloud);
|
||||
|
||||
std::pair<nav_msgs::msg::Odometry, geometry_msgs::msg::TransformStamped> handlePose(rclcpp::Time stamp);
|
||||
std::unique_ptr<tf2_ros::TransformBroadcaster> odom_tf_broadcaster_;
|
||||
nav_msgs::msg::Odometry handlePose(rclcpp::Time stamp);
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pose_pub_;
|
||||
void publishPose(nav_msgs::msg::Odometry pose);
|
||||
void publishTf(geometry_msgs::msg::TransformStamped tf);
|
||||
|
||||
sensor_msgs::msg::JointState handleWheels(rclcpp::Time stamp);
|
||||
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr wheels_pub_;
|
||||
@ -76,28 +50,15 @@ private:
|
||||
rclcpp::Publisher<std_msgs::msg::Int8>::SharedPtr battery_recharge_state_pub_;
|
||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr battery_state_of_charge_pub_;
|
||||
rclcpp::Publisher<ros2aria_msgs::msg::RobotInfoMsg>::SharedPtr robot_info_pub_;
|
||||
void publishState();
|
||||
void publishState(rclcpp::Time stamp);
|
||||
|
||||
// subscribers
|
||||
rclcpp::Subscription<ros2aria_msgs::msg::RestrictionsMsg>::SharedPtr restrictions_sub_;
|
||||
void restrictions_callback(const ros2aria_msgs::msg::RestrictionsMsg::SharedPtr msg);
|
||||
void init_restrictions();
|
||||
|
||||
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr user_stop_sub_;
|
||||
void user_stop_callback(const std_msgs::msg::Bool::SharedPtr msg);
|
||||
|
||||
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr master_stop_sub_;
|
||||
void master_stop_callback(const std_msgs::msg::Bool::SharedPtr msg);
|
||||
|
||||
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
|
||||
void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
|
||||
|
||||
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr clutch_sub_;
|
||||
void clutch_callback(const std_msgs::msg::Bool::SharedPtr msg);
|
||||
|
||||
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
|
||||
void scan_callback(const sensor_msgs::msg::LaserScan msg);
|
||||
|
||||
// services
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr stop_service_;
|
||||
void stop(const std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response) const;
|
||||
@ -1,19 +0,0 @@
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
|
||||
void Ros2Aria::scan_callback(const sensor_msgs::msg::LaserScan msg){
|
||||
float actual_minimal_distance = std::numeric_limits<float>::max();
|
||||
for(const auto &range: msg.ranges){
|
||||
if(not std::isnan(range) and range > msg.range_min and range < msg.range_max){
|
||||
actual_minimal_distance = std::min(actual_minimal_distance, range);
|
||||
}
|
||||
}
|
||||
minimal_distance = actual_minimal_distance;
|
||||
if(actual_minimal_distance < restrictions.distance.data){
|
||||
RCLCPP_INFO(this->get_logger(), "obstacle detected, minimal_distance: %f", actual_minimal_distance);
|
||||
obstacle_too_close = true;
|
||||
}else{
|
||||
obstacle_too_close = false;
|
||||
}
|
||||
}
|
||||
@ -1,16 +1,17 @@
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include "ros2aria.hpp"
|
||||
|
||||
#include "sensor_msgs/point_cloud_conversion.hpp"
|
||||
|
||||
sensor_msgs::msg::PointCloud Ros2Aria::handleSonar(rclcpp::Time stamp)
|
||||
{
|
||||
sensor_msgs::msg::PointCloud cloud;
|
||||
sensor_msgs::msg::PointCloud cloud; //sonar readings.
|
||||
cloud.header.stamp = stamp;
|
||||
cloud.header.frame_id = std::string(get_namespace()) + "/front_sonar";
|
||||
// sonar sensors relative to base_link
|
||||
cloud.header.frame_id = this->sonar_frame_id;
|
||||
|
||||
auto r = robot->getRobot();
|
||||
|
||||
auto lenght = r->getNumSonar()/2;
|
||||
for (int i = 0; i < num_of_sonars; i++)
|
||||
for (int i = 0; i < r->getNumSonar(); i++)
|
||||
{
|
||||
ArSensorReading *reading = NULL;
|
||||
reading = r->getSonarReading(i);
|
||||
@ -37,10 +38,6 @@ sensor_msgs::msg::PointCloud Ros2Aria::handleSonar(rclcpp::Time stamp)
|
||||
p.y = reading->getLocalY() / 1000.0;
|
||||
p.z = 0.0;
|
||||
cloud.points.push_back(p);
|
||||
|
||||
if(distance(p.x, p.y) < minimal_distance){
|
||||
obstacle_too_close = true;
|
||||
}
|
||||
}
|
||||
return cloud;
|
||||
}
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include "ros2aria.hpp"
|
||||
|
||||
void Ros2Aria::publishState()
|
||||
void Ros2Aria::publishState(rclcpp::Time stamp)
|
||||
{
|
||||
auto r = this->robot->getRobot();
|
||||
|
||||
@ -13,21 +13,32 @@ void Ros2Aria::publishState()
|
||||
this->battery_recharge_state_pub_->publish(recharge_state);
|
||||
}
|
||||
|
||||
// TODO: our robots don't have this, cannot test
|
||||
if (r->haveStateOfCharge() && this->battery_state_of_charge_pub_->get_subscription_count() > 0)
|
||||
{
|
||||
std_msgs::msg::Float32 soc;
|
||||
soc.data = r->getStateOfCharge() / 100.0;
|
||||
this->battery_state_of_charge_pub_->publish(soc);
|
||||
}
|
||||
|
||||
if (robot_info_pub_->get_subscription_count() > 0)
|
||||
if (this->robot_info_pub_->get_subscription_count() > 0)
|
||||
{
|
||||
ros2aria_msgs::msg::RobotInfoMsg robot_info_msg;
|
||||
|
||||
robot_info_msg.robot_id.data = id;
|
||||
// TODO: allow setting the robot_id
|
||||
robot_info_msg.robot_id.data = 6;
|
||||
robot_info_msg.battery_voltage.data = r->getRealBatteryVoltageNow();
|
||||
robot_info_msg.minimal_distance.data = minimal_distance;
|
||||
robot_info_msg.twist.linear.x = r->getVel() / 1000;
|
||||
robot_info_msg.twist.linear.y = r->getLatVel() / 1000.0;
|
||||
robot_info_msg.twist.angular.z = r->getRotVel() * M_PI / 180;
|
||||
robot_info_msg.state.data = not user_stop;
|
||||
robot_info_msg.clutch.data = r->areMotorsEnabled();
|
||||
robot_info_msg.obstacle_detected.data = obstacle_too_close;
|
||||
|
||||
robot_info_pub_->publish(robot_info_msg);
|
||||
// TODO: actually keep track of robot state (true -> unlocked, false ->
|
||||
// locked). This requires safety plugin to be implemented
|
||||
robot_info_msg.state.data = true;
|
||||
robot_info_msg.clutch.data = r->areMotorsEnabled();
|
||||
// TODO: actually look for obstacles
|
||||
robot_info_msg.obstacle_detected.data = false;
|
||||
|
||||
this->robot_info_pub_->publish(robot_info_msg);
|
||||
}
|
||||
}
|
||||
@ -1,5 +1,5 @@
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include <cmath>
|
||||
#include "ros2aria.hpp"
|
||||
|
||||
sensor_msgs::msg::JointState Ros2Aria::handleWheels(rclcpp::Time stamp)
|
||||
{
|
||||
sensor_msgs::msg::JointState wheels;
|
||||
@ -13,14 +13,13 @@ sensor_msgs::msg::JointState Ros2Aria::handleWheels(rclcpp::Time stamp)
|
||||
wheels.velocity.resize(2);
|
||||
wheels.effort.resize(0);
|
||||
|
||||
// robot_state_publisher gives namespace
|
||||
wheels.name[0] = "left_wheel_joint";
|
||||
wheels.name[1] = "right_wheel_joint";
|
||||
|
||||
wheels.name[0] = "Wheel_L";
|
||||
wheels.name[1] = "Wheel_R";
|
||||
wheels.position[0] = r->getLeftEncoder();
|
||||
wheels.position[1] = r->getRightEncoder();
|
||||
wheels.velocity[0] = r->getLeftVel();
|
||||
wheels.velocity[1] = r->getRightVel();
|
||||
|
||||
return wheels;
|
||||
}
|
||||
|
||||
|
||||
@ -4,4 +4,3 @@ geometry_msgs/Twist twist
|
||||
std_msgs/Bool state
|
||||
std_msgs/Bool clutch
|
||||
std_msgs/Bool obstacle_detected
|
||||
std_msgs/Float64 minimal_distance
|
||||
|
||||
@ -1,6 +0,0 @@
|
||||
Student:
|
||||
- odblokowanie robota
|
||||
- odblokowanie
|
||||
|
||||
|
||||
- stan przejściowy na przyciskach
|
||||
Loading…
Reference in New Issue
Block a user