L1.5/ros2aria
L1.5/ros2aria
7
0
Fork 0
Code Issues 5 Pull Requests Projects 1 Releases Wiki Activity

update

Browse Source
This commit is contained in:
Jakub Delicat 2023-07-12 10:55:59 +02:00
parent 3c2f23dbf8
commit 93a170d2bf
36 changed files with 2482 additions and 670 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
.vscode/c_cpp_properties.json vendored
View File

@ -10,9 +10,11 @@
"/workspaces/ros2aria/src/ros2aria/include/**",
"/workspaces/ros2aria/install/ros2aria_msgs/include/**",
"/usr/include/**",
"/usr/local/Aria/include"
"/usr/local/Aria/include",
"${workspaceFolder}/src/ros2aria/include/**"
],
"name": "ROS"
"name": "ROS",
"configurationProvider": "ms-vscode.cmake-tools"
}
],
"version": 4

3
.vscode/settings.json vendored
View File

@ -88,5 +88,6 @@
"python.analysis.extraPaths": [
"/opt/ros/foxy/lib/python3.8/site-packages"
],
"ros.distro": "humble"
"ros.distro": "humble",
"cmake.sourceDirectory": "/home/jdelicat/lab_repos/ros2aria/"
}

7
Dockerfile.hokuyo Normal file
View File

@ -0,0 +1,7 @@
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

21
Dockerfile.rosbridge Normal file
View File

@ -0,0 +1,21 @@
ARG ROS_DISTRO=humble
FROM husarnet/ros:$ROS_DISTRO-ros-core
SHELL ["/bin/bash", "-c"]
RUN apt update && \
apt upgrade -y && \
apt install -y \
ros-$ROS_DISTRO-rosbridge-suite \
ros-$ROS_DISTRO-tf2-msgs \
# custom msgs dependency
ros-$ROS_DISTRO-rosidl-typesupport-c \
&& \
apt-get autoremove -y && \
apt-get clean && \
rm -rf /var/lib/apt/lists/*
COPY --from=delicjusz/pioneer /ros2_ws /ros2_ws
RUN echo $(dpkg -s ros-$ROS_DISTRO-rosbridge-suite | grep 'Version' | sed -r 's/Version:\s([0-9]+.[0-9]+.[0-9]*).*/\1/g') >> /version.txt

8
Makefile
View File

@ -6,7 +6,8 @@ HEADERS := $(wildcard src/ros2aria/src/*.hpp)
build/ros2aria/ros2aria: $(SOURCES) $(HEADERS)
colcon build
build: build/ros2aria/ros2aria
build:
docker build -f Dockerfile -t delicjusz/pioneer .
# .uploaded: build/ros2aria/ros2aria
upload:
@ -29,5 +30,10 @@ push_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

33
code.py
View File

@ -1,33 +0,0 @@
try:
cl, addr = s.accept()
print('client connected from', addr)
request = cl.recv(1024)
request = str(request)
print("request: {}".format(request))
led_on = request.find('/dioda/on')
led_off = request.find('/dioda/off')
text_index = request.find('/text')
print( 'text = ' + str(text_index))
print( 'led on = ' + str(led_on))
print( 'led off = ' + str(led_off))
if text_index == 6:
entrypoint_text = request[text_index+len('/text/'):]
text = entrypoint_text[:entrypoint_text.find(' ')]
print('Hello world {}'.format(text))
if led_on == 6:
led.on()
else:
led.off()
response = html
response = html % stateis
cl.send('HTTP/1.0 200 OK\r\nContent-type: text/html\r\n\r\n')
cl.send(response)
cl.close()
except OSError as e:
cl.close()
print('connection closed')

39
compose.foxglove.master.yaml Normal file
View File

@ -0,0 +1,39 @@
# docker compose -f compose.foxglove.master.yaml up
services:
restrictions-republisher:
image: delicjusz/rosbridge
network_mode: host
ipc: host
restart: on-failure
environment:
- RMW_IMPLEMENTATION=rmw_fastrtps_cpp
- FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml
volumes:
- ./fastdds.xml:/fastdds.xml
- ./rescrictions_republisher.py:/rescrictions_republisher.py
command: >
bash -c "source /ros2_ws/install/setup.bash && python3 /rescrictions_republisher.py"
rosbridge:
image: delicjusz/rosbridge
network_mode: host
ipc: host
restart: on-failure
environment:
- RMW_IMPLEMENTATION=rmw_fastrtps_cpp
- FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml
volumes:
- ./fastdds.xml:/fastdds.xml
command: >
bash -c "source /ros2_ws/install/setup.bash && ros2 launch rosbridge_server rosbridge_websocket_launch.xml"
foxglove-master:
image: husarion/foxglove:humble-1.39.1-20230220
ports:
- 5050:5050
volumes:
- ./config/master_layout.json:/src/FoxgloveDefaultLayout.json
environment:
- FOXGLOVE_PORT=5050
- ROSBRIDGE_PORT=9090

31
compose.foxglove.user.yaml Normal file
View File

@ -0,0 +1,31 @@
# docker compose -f compose.foxglove.yaml up
x-net-config:
&net-config
network_mode: host
ipc: host
services:
foxglove:
image: husarion/foxglove:humble-1.39.1-20230220
restart: on-failure
ports:
- 8080:8080
volumes:
- ./config/user_layout.json:/src/FoxgloveDefaultLayout.json
environment:
- FOXGLOVE_PORT=8080
- ROSBRIDGE_PORT=9090
# rosbridge:
# image: delicjusz/rosbridge
# <<: *net-config
# restart: on-failure
# environment:
# - RMW_IMPLEMENTATION=rmw_fastrtps_cpp
# - FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml
# volumes:
# - ./fastdds.xml:/fastdds.xml
# - ./rosbridge_websocket_launch.xml:/opt/ros/humble/share/rosbridge_server/launch/rosbridge_websocket_launch.xml
# command: >
# bash -c "source /ros2_ws/install/setup.bash && ros2 launch rosbridge_server rosbridge_websocket_launch.xml
# pioneer_id:=${PIONEER_ID}"

27
compose.foxglove.yaml
View File

@ -1,27 +0,0 @@
x-net-config:
&net-config
network_mode: host
ipc: host
services:
foxglove:
image: husarion/foxglove:humble-1.39.1-20230220
ports:
- 8080:8080
volumes:
- ./config/rosbot_sensors_foxglove.json:/src/FoxgloveDefaultLayout.json
environment:
- FOXGLOVE_PORT=8080
- ROSBRIDGE_PORT=9090
rosbridge:
image: husarion/rosbridge:humble-1.3.1-20230220
<<: *net-config
ports:
- 9090:9090
environment:
- RMW_IMPLEMENTATION=rmw_fastrtps_cpp
- FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml
volumes:
- ./fastdds.xml:/fastdds.xml
command: ros2 launch rosbridge_server rosbridge_websocket_launch.xml

19
compose.hokuyo.yaml Normal file
View File

@ -0,0 +1,19 @@
# 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
- /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

2
compose.ros2aria.yaml
View File

@ -12,4 +12,4 @@ services:
- ./fastdds.xml:/fastdds.xml
devices:
- /dev/ttyS0
command: ros2 launch pioneer_bringup robot.launch.py pioneer_id:=${PIONEER_ID}
command: ros2 launch pioneer_bringup robot.launch.py namespace:=pioneer${PIONEER_ID}/

279
config/master_layout copy.json Normal file
View File

@ -0,0 +1,279 @@
{
"configById": {
"Gauge!36uqsdw": {
"path": "/pioneers/restrictions.linear_velocity.data",
"minValue": 0.1,
"maxValue": 0.8,
"colorMap": "turbo",
"colorMode": "colormap",
"gradient": [
"#fff900",
"#ff00ff"
],
"reverse": false,
"foxglovePanelTitle": "Linear Velocity"
},
"Gauge!16s1huo": {
"path": "/pioneers/restrictions.angular_velocity.data",
"minValue": 0.1,
"maxValue": 1.2,
"colorMap": "turbo",
"colorMode": "colormap",
"gradient": [
"#fff900",
"#ff00ff"
],
"reverse": false,
"foxglovePanelTitle": "Angular Velocity"
},
"Gauge!43oiv40": {
"path": "/pioneers/restrictions.linear_velocity.data",
"minValue": 0.02,
"maxValue": 1.2,
"colorMap": "rainbow",
"colorMode": "gradient",
"gradient": [
"#ffffff",
"#ff0000"
],
"reverse": false,
"foxglovePanelTitle": "Distance"
},
"Indicator!2kcvhko": {
"path": "/pioneer5/robot_info.state.data",
"style": "background",
"fallbackColor": "#a0a0a0",
"fallbackLabel": "Inactive",
"rules": [
{
"operator": "=",
"rawValue": "true",
"color": "#68e24a",
"label": "Active"
}
],
"foxglovePanelTitle": "Status"
},
"Publish!1jggeko": {
"topicName": "/pioneer5/clutch",
"datatype": "std_msgs/msg/Bool",
"buttonText": "Enable",
"buttonTooltip": "",
"buttonColor": "#00A871",
"advancedView": false,
"value": "{\n \"data\": true\n}",
"foxglovePanelTitle": "Motors"
},
"Publish!2bcq74b": {
"topicName": "/pioneer5/clutch",
"datatype": "std_msgs/msg/Bool",
"buttonText": "Disable",
"buttonTooltip": "",
"buttonColor": "#aeaeae",
"advancedView": false,
"value": "{\n \"data\": false\n}",
"foxglovePanelTitle": " Motors"
},
"Indicator!s9ir7d": {
"path": "/pioneer5/robot_info.clutch.data",
"style": "background",
"fallbackColor": "#a0a0a0",
"fallbackLabel": "Off",
"rules": [
{
"operator": "=",
"rawValue": "true",
"color": "#68e24a",
"label": "On"
}
],
"foxglovePanelTitle": "Motors"
},
"Indicator!4am2dga": {
"path": "/pioneer5/robot_info.obstacle_detected.data",
"style": "background",
"fallbackColor": "#a0a0a0",
"fallbackLabel": "👌",
"rules": [
{
"operator": "=",
"rawValue": "true",
"color": "#ff005e",
"label": "😱"
}
],
"foxglovePanelTitle": "Obstacle"
},
"Gauge!44f06dp": {
"path": "/pioneer5/robot_info.battery_voltage.data",
"minValue": 11.9,
"maxValue": 12.9,
"colorMap": "red-yellow-green",
"colorMode": "colormap",
"gradient": [
"#0000ff",
"#ff00ff"
],
"reverse": false,
"foxglovePanelTitle": "Battry Voltage"
},
"Indicator!36yp9u": {
"path": "/pioneers/master_stop.data",
"style": "background",
"fallbackColor": "#ff0000",
"fallbackLabel": "Robots Stopped",
"rules": [
{
"operator": "=",
"rawValue": "true",
"color": "#68e24a",
"label": "Robots Released"
}
],
"foxglovePanelTitle": "Master Stop"
},
"Publish!3ucy03l": {
"topicName": "/pioneers/master_stop",
"datatype": "std_msgs/msg/Bool",
"buttonText": "Start",
"buttonTooltip": "",
"buttonColor": "#00A871",
"advancedView": false,
"value": "{\n \"data\": true\n}"
},
"Publish!3eo06na": {
"topicName": "/pioneers/restrictions",
"datatype": "ros2aria_msgs/msg/RestrictionsMsg",
"buttonText": "Publish",
"buttonTooltip": "",
"buttonColor": "#00A871",
"advancedView": true,
"value": "{\n \"distance\": {\n \"data\": 0.2 },\n \"linear_velocity\": {\n \"data\": 0.2\n },\n \"angular_velocity\": {\n \"data\": ${ELKO}\n }\n}\n"
},
"Publish!2llenrw": {
"topicName": "/pioneers/master_stop",
"datatype": "std_msgs/msg/Bool",
"buttonText": "STOP ",
"buttonTooltip": "",
"buttonColor": "#ff0000",
"advancedView": false,
"value": "{\n \"data\": false\n}",
"foxglovePanelTitle": "Master"
},
"RawMessages!2n2d99y": {
"diffEnabled": false,
"diffMethod": "custom",
"diffTopicPath": "",
"showFullMessageForDiff": false,
"topicPath": "/pioneers/restrictions"
},
"Tab!4eriapz": {
"activeTabIdx": 0,
"tabs": [
{
"title": "Restrictions",
"layout": {
"first": "Gauge!36uqsdw",
"second": {
"first": "Gauge!16s1huo",
"second": "Gauge!43oiv40",
"direction": "row",
"splitPercentage": 51.44124168514412
},
"direction": "row",
"splitPercentage": 32.84023668639053
}
}
]
},
"Tab!3irkbr4": {
"activeTabIdx": 0,
"tabs": [
{
"title": "5",
"layout": {
"first": {
"first": "Indicator!2kcvhko",
"second": {
"first": {
"first": "Publish!1jggeko",
"second": "Publish!2bcq74b",
"direction": "row",
"splitPercentage": 47.44522280433317
},
"second": {
"first": "Indicator!s9ir7d",
"second": "Indicator!4am2dga",
"direction": "row",
"splitPercentage": 51.020408163265316
},
"direction": "row",
"splitPercentage": 50.046709461147984
},
"direction": "row",
"splitPercentage": 33.5482533335995
},
"second": "Gauge!44f06dp",
"direction": "row",
"splitPercentage": 83.10937586339708
}
}
]
}
},
"globalVariables": {
"restrictions": {
"distance": {
"data": 0
},
"linear_velocity": {
"data": 0.2
},
"angular_velocity": {
"data": 0.6
}
},
"ELKO": 5,
"elko": 6
},
"userNodes": {
"ed523c47-5740-4915-a163-89d74f63276b": {
"sourceCode": "import { MessageWriter } from \"@foxglove/rosmsg2-serialization\";\n\n// message definition comes from `parse()` in @foxglove/rosmsg\nconst writer = new MessageWriter(pointStampedMessageDefinition);\n\n// serialize the passed in object to a Uint8Array as a geometry_msgs/PointStamped message\nconst uint8Array = writer.writeMessage({\n header: {\n stamp: { sec: 0, nanosec: 0 },\n frame_id: \"\"\n },\n x: 1,\n y: 0,\n z: 0\n});",
"name": "ed523c47"
}
},
"playbackConfig": {
"speed": 1
},
"layout": {
"first": "Indicator!36yp9u",
"second": {
"first": {
"first": {
"first": {
"first": "Publish!3ucy03l",
"second": "Publish!3eo06na",
"direction": "column",
"splitPercentage": 17.645846217274784
},
"second": {
"first": "Publish!2llenrw",
"second": "RawMessages!2n2d99y",
"direction": "column",
"splitPercentage": 18.30550401978973
},
"direction": "row",
"splitPercentage": 55.393939393939384
},
"second": "Tab!4eriapz",
"direction": "column",
"splitPercentage": 71.5909090909091
},
"second": "Tab!3irkbr4",
"direction": "row",
"splitPercentage": 37.61825264329438
},
"direction": "column",
"splitPercentage": 12.5
}
}

1173
config/master_layout.json Normal file
View File

File diff suppressed because it is too large Load Diff

474
config/rosbot_sensors_foxglove.json
View File

@ -1,474 +0,0 @@
{
"configById": {
"Plot!gjvhbp": {
"paths": [
{
"value": "/range/fl.range",
"enabled": true,
"timestampMethod": "receiveTime"
},
{
"value": "/range/rl.range",
"enabled": true,
"timestampMethod": "receiveTime"
},
{
"value": "/range/fr.range",
"enabled": true,
"timestampMethod": "receiveTime"
},
{
"value": "/range/rr.range",
"enabled": true,
"timestampMethod": "receiveTime"
}
],
"minYValue": -0.039230484541325744,
"maxYValue": 1,
"showXAxisLabels": true,
"showYAxisLabels": true,
"showLegend": true,
"legendDisplay": "floating",
"showPlotValuesInLegend": false,
"isSynced": true,
"xAxisVal": "timestamp",
"sidebarDimension": 240,
"foxglovePanelTitle": "Plot",
"followingViewWidth": 30
},
"Plot!1u5bb0v": {
"paths": [
{
"value": "/imu_broadcaster/imu.orientation.w",
"enabled": true,
"timestampMethod": "receiveTime"
},
{
"value": "/imu_broadcaster/imu.orientation.x",
"enabled": true,
"timestampMethod": "receiveTime"
},
{
"value": "/imu_broadcaster/imu.orientation.y",
"enabled": true,
"timestampMethod": "receiveTime"
},
{
"value": "/imu_broadcaster/imu.orientation.z",
"enabled": true,
"timestampMethod": "receiveTime"
}
],
"minYValue": -1.1,
"maxYValue": 1.1,
"showXAxisLabels": true,
"showYAxisLabels": true,
"showLegend": true,
"legendDisplay": "floating",
"showPlotValuesInLegend": false,
"isSynced": true,
"xAxisVal": "timestamp",
"sidebarDimension": 240,
"foxglovePanelTitle": "Plot",
"followingViewWidth": 30
},
"Tab!2qhku9u": {
"activeTabIdx": 0,
"tabs": [
{
"title": "Ranges Plots",
"layout": "Plot!gjvhbp"
},
{
"title": "IMU Plots",
"layout": "Plot!1u5bb0v"
}
]
},
"3D!40jejke": {
"cameraState": {
"perspective": true,
"distance": 4.7565377051017865,
"phi": 0.5121483430698642,
"thetaOffset": 89.80364880250498,
"targetOffset": [
0.04217571585974451,
-0.03971452594915587,
1.5648103050465003e-17
],
"target": [
0,
0,
0
],
"targetOrientation": [
0,
0,
0,
1
],
"fovy": 45,
"near": 0.5,
"far": 5000
},
"followMode": "follow-pose",
"scene": {
"transforms": {
"axisScale": 0.4499999999999996,
"labelSize": 0.013834613718625963
},
"enableStats": true,
"ignoreColladaUpAxis": false,
"syncCamera": false
},
"transforms": {
"frame:base_link": {
"visible": false
},
"frame:body_link": {
"visible": false
},
"frame:cover_link": {
"visible": false
},
"frame:imu_link": {
"visible": false
},
"frame:camera_link": {
"visible": false
},
"frame:fl_range": {
"visible": false
},
"frame:fr_range": {
"visible": false
},
"frame:rl_range": {
"visible": false
},
"frame:rr_range": {
"visible": false
},
"frame:fl_wheel_link": {
"visible": true
},
"frame:fr_wheel_link": {
"visible": true
},
"frame:rl_wheel_link": {
"visible": true
},
"frame:rr_wheel_link": {
"visible": true
},
"frame:slamtec_rplidar_a2_link": {
"visible": false
},
"frame:laser": {
"visible": false
},
"frame:orbbec_astra_link": {
"visible": false
},
"frame:depth": {
"visible": false
},
"frame:odom": {
"visible": false
}
},
"topics": {
"/scan": {
"visible": true,
"colorField": "range",
"colorMode": "flat",
"colorMap": "turbo",
"pointSize": 8,
"flatColor": "#f90000"
},
"/robot_description": {
"visible": false
}
},
"layers": {
"e827a6dc-875b-448a-8475-5497577c2e1b": {
"visible": true,
"frameLocked": true,
"label": "URDF",
"instanceId": "e827a6dc-875b-448a-8475-5497577c2e1b",
"layerId": "foxglove.Urdf",
"url": "http://{{.Host}}:{{env "FOXGLOVE_PORT"}}/rosbot.urdf",
"order": 1
},
"b2c64820-a936-4f65-82b5-7a04ef902009": {
"visible": true,
"frameLocked": true,
"label": "Grid",
"instanceId": "b2c64820-a936-4f65-82b5-7a04ef902009",
"layerId": "foxglove.Grid",
"size": 10,
"divisions": 100,
"lineWidth": 1,
"color": "#248eff",
"position": [
0,
0,
0
],
"rotation": [
0,
0,
0
],
"order": 2
}
},
"publish": {
"type": "point",
"poseTopic": "/move_base_simple/goal",
"pointTopic": "/clicked_point",
"poseEstimateTopic": "/initialpose",
"poseEstimateXDeviation": 0.5,
"poseEstimateYDeviation": 0.5,
"poseEstimateThetaDeviation": 0.26179939
},
"foxglovePanelTitle": "Lidar View"
},
"RosOut!b0toow": {
"searchTerms": [],
"minLogLevel": 2
},
"Teleop!yh7wcv": {
"topic": "/cmd_vel",
"publishRate": 1,
"upButton": {
"field": "linear-x",
"value": 0.19754204525471783
},
"downButton": {
"field": "linear-x",
"value": -0.20000000000000015
},
"leftButton": {
"field": "angular-z",
"value": 1
},
"rightButton": {
"field": "angular-z",
"value": -1
},
"foxglovePanelTitle": "Diff Drive"
},
"ImageViewPanel!40iocf4": {
"cameraTopic": "/camera/color/image_raw/compressed",
"enabledMarkerTopics": [],
"mode": "fit",
"pan": {
"x": 0,
"y": 0
},
"rotation": 0,
"synchronize": true,
"transformMarkers": false,
"zoom": 1,
"foxglovePanelTitle": "Astra Compressed Image"
},
"Gauge!4jffafa": {
"path": "/battery.voltage",
"minValue": 9.8,
"maxValue": 12.6,
"colorMap": "turbo",
"colorMode": "colormap",
"gradient": [
"#0000ff",
"#ff00ff"
],
"reverse": false,
"foxglovePanelTitle": "Battery"
},
"Indicator!11kizr9": {
"path": "/battery.voltage",
"style": "background",
"fallbackColor": "#000000",
"fallbackLabel": "Ok",
"rules": [
{
"operator": "<",
"rawValue": "10.8",
"color": "#ff0000",
"label": "Plug charger!"
}
],
"foxglovePanelTitle": "Plug Charger Info"
},
"Plot!4dl4s92": {
"paths": [
{
"value": "/battery.voltage",
"enabled": true,
"timestampMethod": "receiveTime"
}
],
"minYValue": 9.482842712474614,
"maxYValue": 13,
"showXAxisLabels": true,
"showYAxisLabels": true,
"showLegend": true,
"legendDisplay": "floating",
"showPlotValuesInLegend": true,
"isSynced": true,
"xAxisVal": "timestamp",
"sidebarDimension": 240,
"foxglovePanelTitle": "Plot",
"followingViewWidth": 60
},
"Publish!1f6cruz": {
"topicName": "/led/left",
"datatype": "std_msgs/msg/Bool",
"buttonText": "LED1 Turn ON",
"buttonTooltip": "",
"buttonColor": "#00A871",
"advancedView": false,
"value": "{\n \"data\": true\n}",
"foxglovePanelTitle": "LED1 Turn ON"
},
"Publish!1wozu40": {
"topicName": "/led/right",
"datatype": "std_msgs/msg/Bool",
"buttonText": "LED2 Turn ON",
"buttonTooltip": "",
"buttonColor": "#00A871",
"advancedView": false,
"value": "{\n \"data\": true\n}",
"foxglovePanelTitle": "LED2 Turn ON"
},
"Indicator!2z34jcy": {
"path": "/button/left.data",
"style": "background",
"fallbackColor": "#ff0000",
"fallbackLabel": "Released",
"rules": [
{
"operator": "=",
"rawValue": "true",
"color": "#26c578",
"label": "Pressed"
}
],
"foxglovePanelTitle": "Left Button"
},
"Publish!3cb71c0": {
"topicName": "/led/left",
"datatype": "std_msgs/msg/Bool",
"buttonText": "LED1 Turn OFF",
"buttonTooltip": "",
"buttonColor": "#a80000",
"advancedView": false,
"value": "{\n \"data\": false\n}",
"foxglovePanelTitle": "LED1 Turn OFF"
},
"Publish!46onm9c": {
"topicName": "/led/right",
"datatype": "std_msgs/msg/Bool",
"buttonText": "LED2 Turn OFF",
"buttonTooltip": "",
"buttonColor": "#a80000",
"advancedView": false,
"value": "{\n \"data\": false\n}",
"foxglovePanelTitle": "LED2 Turn OFF"
},
"Indicator!1hywfa1": {
"path": "/button/right.data",
"style": "background",
"fallbackColor": "#ff0000",
"fallbackLabel": "Released",
"rules": [
{
"operator": "=",
"rawValue": "true",
"color": "#26c578",
"label": "Pressed"
}
],
"foxglovePanelTitle": "Left Button"
}
},
"globalVariables": {
"globalVariable": 7
},
"userNodes": {
"f5206e1d-deee-4f90-a03e-f561fbb9a7dd": {
"sourceCode": "// The ./types module provides helper types for your Input events and messages.\nimport { Input, Message } from \"./types\";\n\n// Your script can output well-known message types, any of your custom message types, or\n// complete custom message types.\n//\n// Use `Message` to access your data source types or well-known types:\n// type Twist = Message<\"geometry_msgs/Twist\">;\n//\n// Conventionally, it's common to make a _type alias_ for your script's output type\n// and use that type name as the return type for your script function.\n// Here we've called the type `Output` but you can pick any type name.\ntype Output = {\n hello: string;\n};\n\n// These are the topics your script \"subscribes\" to. Studio will invoke your script function\n// when any message is received on one of these topics.\nexport const inputs = [\"/input/topic\"];\n\n// Any output your script produces is \"published\" to this topic. Published messages are only visible within Studio, not to your original data source.\nexport const output = \"/studio_script/output_topic\";\n\n// This function is called with messages from your input topics.\n// The first argument is an event with the topic, receive time, and message.\n// Use the `Input<...>` helper to get the correct event type for your input topic messages.\nexport default function script(event: Input<\"/input/topic\">): Output {\n return {\n hello: \"world!\",\n };\n};",
"name": "f5206e1d"
},
"1a9e6183-d4b1-47dd-a024-efc14ab90b6b": {
"sourceCode": "// This example shows how to subscribe to multiple input topics.\n//\n// NOTE:\n// User Scripts can subscribe to multiple input topics, but can only publish on a single topic.\n\nimport { Input } from \"./types\";\n\ntype Output = { topic: string };\ntype GlobalVariables = { id: number };\n\n// List all the input topics in the `input` array\nexport const inputs = [\"/input/topic\", \"/input/another\"];\nexport const output = \"/studio_script/output_topic\";\n\n// Make an InputEvent type alias. Since our node will get a message from either input topic, we need to enumerate the topics.\ntype InputEvent = Input<\"/input/topic\"> | Input<\"/input/another\">;\n\nexport default function node(event: InputEvent, globalVars: GlobalVariables): Output {\n // Remember that your node will get messages on each topic, so you\n // need to check each event's topic to know which fields are available on the message.\n switch (event.topic) {\n case \"/input/topic\":\n // topic specific input logic\n // Our message fields are specific to our topic message\n break;\n case \"/input/another\":\n // another specific logic\n break;\n }\n\n // Nodes can only output one type of message regardless of the inputs\n // Here we echo back the input topic as an example.\n return {\n topic: event.topic,\n };\n};\n",
"name": "1a9e6183"
}
},
"playbackConfig": {
"speed": 1
},
"layout": {
"first": {
"direction": "row",
"first": "Tab!2qhku9u",
"second": {
"first": "3D!40jejke",
"second": {
"first": "RosOut!b0toow",
"second": "Teleop!yh7wcv",
"direction": "column"
},
"direction": "row",
"splitPercentage": 57.03330110757996
},
"splitPercentage": 30.5849582172702
},
"second": {
"first": "ImageViewPanel!40iocf4",
"second": {
"first": {
"first": {
"first": "Gauge!4jffafa",
"second": "Indicator!11kizr9",
"direction": "column"
},
"second": "Plot!4dl4s92",
"direction": "row"
},
"second": {
"first": {
"first": "Publish!1f6cruz",
"second": {
"first": "Publish!1wozu40",
"second": "Indicator!2z34jcy",
"direction": "row",
"splitPercentage": 30.461538461538503
},
"direction": "row",
"splitPercentage": 23.167848699763567
},
"second": {
"first": {
"first": "Publish!3cb71c0",
"second": "Publish!46onm9c",
"direction": "row",
"splitPercentage": 49.49494949494945
},
"second": "Indicator!1hywfa1",
"direction": "row",
"splitPercentage": 46.808510638297854
},
"direction": "column"
},
"direction": "row",
"splitPercentage": 57.106729218589656
},
"direction": "row",
"splitPercentage": 30.63012301119672
},
"direction": "column",
"splitPercentage": 67.44897959183673
}
}

482
config/user_layout.json Normal file
View File

@ -0,0 +1,482 @@
{
"configById": {
"Indicator!3tlipvy": {
"path": "/pioneer5/robot_info.obstacle_detected.data",
"style": "background",
"fallbackColor": "#a0a0a0",
"fallbackLabel": "Ok",
"rules": [
{
"operator": "=",
"rawValue": "true",
"color": "#ff0000",
"label": "Detected Obstacle"
}
],
"foxglovePanelTitle": "Obstacle"
},
"Gauge!3u4tuia": {
"path": "/pioneer5/robot_info.minimal_distance.data",
"minValue": 0.02,
"maxValue": 1.002,
"colorMap": "turbo",
"colorMode": "colormap",
"gradient": [
"#0000ff",
"#ff00ff"
],
"reverse": false,
"foxglovePanelTitle": "Actual Distance"
},
"Tab!25ni7og": {
"activeTabIdx": 0,
"tabs": [
{
"title": "Restrictions",
"layout": {
"first": {
"first": "Gauge!njxcom",
"second": {
"first": "Indicator!cjpcwp",
"second": {
"first": "RawMessages!21kcr2s",
"second": "Indicator!2f5tlku",
"direction": "row"
},
"direction": "column"
},
"direction": "column"
},
"second": {
"first": {
"first": "Gauge!3h30jw7",
"second": {
"first": "Indicator!3i4t2ed",
"second": {
"first": "RawMessages!3rm8fcv",
"second": "Indicator!c84ni4",
"direction": "row"
},
"direction": "column"
},
"direction": "column"
},
"second": {
"first": "Gauge!2nkb668",
"second": {
"first": "Indicator!1qk7sa6",
"second": {
"first": "RawMessages!260vbhh",
"second": "Indicator!1o4qvq1",
"direction": "row"
},
"direction": "column"
},
"direction": "column"
},
"direction": "row"
},
"direction": "row",
"splitPercentage": 34.15445097342172
}
}
]
},
"Gauge!njxcom": {
"path": "/pioneers/restrictions.linear_velocity.data",
"minValue": 0.1,
"maxValue": 0.8,
"colorMap": "turbo",
"colorMode": "colormap",
"gradient": [
"#0000ff",
"#ff0000"
],
"reverse": false,
"foxglovePanelTitle": "Max Linear Velocity"
},
"Indicator!cjpcwp": {
"path": "",
"style": "background",
"fallbackColor": "#181818",
"fallbackLabel": "Linear",
"rules": []
},
"RawMessages!21kcr2s": {
"diffEnabled": false,
"diffMethod": "custom",
"diffTopicPath": "",
"showFullMessageForDiff": false,
"topicPath": "/pioneers/restrictions.linear_velocity.data"
},
"Indicator!2f5tlku": {
"path": "",
"style": "background",
"fallbackColor": "#181818",
"fallbackLabel": "m/s",
"rules": []
},
"Gauge!3h30jw7": {
"path": "/pioneers/restrictions.angular_velocity.data",
"minValue": 0.1,
"maxValue": 1.2,
"colorMap": "turbo",
"colorMode": "colormap",
"gradient": [
"#0000ff",
"#ff00ff"
],
"reverse": false,
"foxglovePanelTitle": "Max Angular Velocity"
},
"Indicator!3i4t2ed": {
"path": "",
"style": "background",
"fallbackColor": "#181818",
"fallbackLabel": "Angular",
"rules": []
},
"RawMessages!3rm8fcv": {
"diffEnabled": false,
"diffMethod": "custom",
"diffTopicPath": "",
"showFullMessageForDiff": false,
"topicPath": "/pioneers/restrictions.angular_velocity.data"
},
"Indicator!c84ni4": {
"path": "",
"style": "background",
"fallbackColor": "#181818",
"fallbackLabel": "rad/s",
"rules": []
},
"Gauge!2nkb668": {
"path": "/pioneers/restrictions.distance.data",
"minValue": 0.02,
"maxValue": 1.002,
"colorMap": "turbo",
"colorMode": "colormap",
"gradient": [
"#0000ff",
"#ff00ff"
],
"reverse": false,
"foxglovePanelTitle": "Min Distance"
},
"Indicator!1qk7sa6": {
"path": "",
"style": "background",
"fallbackColor": "#181818",
"fallbackLabel": "Distance",
"rules": []
},
"RawMessages!260vbhh": {
"diffEnabled": false,
"diffMethod": "custom",
"diffTopicPath": "",
"showFullMessageForDiff": false,
"topicPath": "/pioneers/restrictions.distance.data"
},
"Indicator!1o4qvq1": {
"path": "",
"style": "background",
"fallbackColor": "#181818",
"fallbackLabel": "m",
"rules": []
},
"Tab!352avk9": {
"activeTabIdx": 0,
"tabs": [
{
"title": "Power",
"layout": {
"first": "Indicator!2hteqsw",
"second": {
"first": "Gauge!3i4ynjz",
"second": {
"first": "RawMessages!1c7650k",
"second": "Indicator!23srs9s",
"direction": "row"
},
"direction": "row",
"splitPercentage": 64.840306644664
},
"direction": "column",
"splitPercentage": 43.13186813186813
}
}
]
},
"Tab!p30d7b": {
"activeTabIdx": 0,
"tabs": [
{
"title": "Robot Info",
"layout": {
"first": "Indicator!10t7upb",
"second": {
"first": {
"first": "Publish!4g2w69i",
"second": "Publish!16m0ff9",
"direction": "row"
},
"second": {
"first": {
"first": "Indicator!1fbii9b",
"second": {
"first": "RawMessages!4asurb8",
"second": "Indicator!4fr19o1",
"direction": "row"
},
"direction": "column"
},
"second": {
"first": "Indicator!3tmnyvt",
"second": {
"first": "RawMessages!a9n4ba",
"second": "Indicator!3dorkxh",
"direction": "row"
},
"direction": "column"
},
"direction": "row"
},
"direction": "column",
"splitPercentage": 33.29027329235685
},
"direction": "column",
"splitPercentage": 18.67321867321867
}
},
{
"title": "2"
}
]
},
"Indicator!2hteqsw": {
"path": "/pioneer5/robot_info.battery_voltage.data",
"style": "background",
"fallbackColor": "#181818",
"fallbackLabel": "Ok",
"rules": [
{
"operator": "<",
"rawValue": "12.2",
"color": "#ff0000",
"label": "Plug Charger"
}
],
"foxglovePanelTitle": "Battery Status"
},
"Gauge!3i4ynjz": {
"path": "/pioneer5/robot_info.battery_voltage.data",
"minValue": 11.9,
"maxValue": 12.9,
"colorMap": "red-yellow-green",
"colorMode": "colormap",
"gradient": [
"#0000ff",
"#ff00ff"
],
"reverse": false,
"foxglovePanelTitle": "Battery Voltage"
},
"RawMessages!1c7650k": {
"diffEnabled": false,
"diffMethod": "custom",
"diffTopicPath": "",
"showFullMessageForDiff": false,
"topicPath": "/pioneer5/robot_info.battery_voltage.data"
},
"Indicator!23srs9s": {
"path": "",
"style": "background",
"fallbackColor": "#181818",
"fallbackLabel": "V",
"rules": []
},
"Indicator!10t7upb": {
"path": "/pioneer5/clutch.data",
"style": "background",
"fallbackColor": "#a0a0a0",
"fallbackLabel": "Off",
"rules": [
{
"operator": "=",
"rawValue": "true",
"color": "#68e24a",
"label": "On"
}
],
"foxglovePanelTitle": "Motors"
},
"Publish!4g2w69i": {
"topicName": "/pioneer5/clutch",
"datatype": "std_msgs/msg/Bool",
"buttonText": "Enable",
"buttonTooltip": "",
"buttonColor": "#68e24a",
"advancedView": false,
"value": "{\n \"data\": true\n}"
},
"Publish!16m0ff9": {
"topicName": "/pioneer5/clutch",
"datatype": "std_msgs/msg/Bool",
"buttonText": "Disable",
"buttonTooltip": "",
"buttonColor": "#a0a0a0",
"advancedView": false,
"value": "{\n \"data\": false\n}"
},
"Indicator!1fbii9b": {
"path": "",
"style": "background",
"fallbackColor": "#181818",
"fallbackLabel": "Linear",
"rules": []
},
"RawMessages!4asurb8": {
"diffEnabled": false,
"diffMethod": "custom",
"diffTopicPath": "",
"showFullMessageForDiff": false,
"topicPath": "/pioneer5/robot_info.twist.linear.x"
},
"Indicator!4fr19o1": {
"path": "",
"style": "background",
"fallbackColor": "#181818",
"fallbackLabel": "m/s",
"rules": []
},
"Indicator!3tmnyvt": {
"path": "",
"style": "background",
"fallbackColor": "#181818",
"fallbackLabel": "Angular",
"rules": []
},
"RawMessages!a9n4ba": {
"diffEnabled": false,
"diffMethod": "custom",
"diffTopicPath": "",
"showFullMessageForDiff": false,
"topicPath": "/pioneer5/robot_info.twist.angular.z"
},
"Indicator!3dorkxh": {
"path": "",
"style": "background",
"fallbackColor": "#181818",
"fallbackLabel": "rad/s",
"rules": []
},
"Indicator!1b8x9ac": {
"path": "/pioneers/master_stop.data",
"style": "background",
"fallbackColor": "#ff0000",
"fallbackLabel": "Robots Stopped",
"rules": [
{
"operator": "=",
"rawValue": "false",
"color": "#68e24a",
"label": "Robots Released"
}
],
"foxglovePanelTitle": "Master Stop"
},
"Tab!2vc4ruw": {
"activeTabIdx": 0,
"tabs": [
{
"title": "Restrictions",
"layout": {
"first": {
"first": "Indicator!3tlipvy",
"second": "Gauge!3u4tuia",
"direction": "row",
"splitPercentage": 77.87689562890276
},
"second": "Tab!25ni7og",
"direction": "column",
"splitPercentage": 18.796068796068795
}
}
]
},
"Indicator!2q85lgg": {
"path": "",
"style": "background",
"fallbackColor": "#181818",
"fallbackLabel": "",
"rules": [
{
"operator": "=",
"rawValue": "true",
"color": "#68e24a",
"label": "True"
}
]
},
"Tab!3q786g": {
"activeTabIdx": 0,
"tabs": [
{
"title": "1",
"layout": {
"first": "Tab!352avk9",
"second": "Tab!p30d7b",
"direction": "column",
"splitPercentage": 45.796737766624844
}
}
]
},
"Teleop!4981td7": {
"topic": "/pioneer5/cmd_vel",
"publishRate": 1,
"upButton": {
"field": "linear-x",
"value": 1
},
"downButton": {
"field": "linear-x",
"value": -1
},
"leftButton": {
"field": "angular-z",
"value": 1
},
"rightButton": {
"field": "angular-z",
"value": -1
}
}
},
"globalVariables": {},
"userNodes": {},
"playbackConfig": {
"speed": 1
},
"layout": {
"first": "Indicator!1b8x9ac",
"second": {
"first": {
"first": "Tab!2vc4ruw",
"second": {
"first": "Indicator!2q85lgg",
"second": "Tab!3q786g",
"direction": "row",
"splitPercentage": 7.045834893512966
},
"direction": "row",
"splitPercentage": 47.75143090760425
},
"second": "Teleop!4981td7",
"direction": "row",
"splitPercentage": 65.43606206527555
},
"direction": "column",
"splitPercentage": 12.989690721649486
}
}

61
node_info_ros1.txt
View File

@ -1,61 +0,0 @@
--------------------------------------------------------------------------------
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

74
pioneer3dx.urdf
View File

@ -16,7 +16,7 @@
<visual>
<origin rpy="0 0 0" xyz="-0.045 0 0.148"/>
<geometry name="pioneer_geom">
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/chassis.stl"/>
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/chassis.stl"/>
</geometry>
<material name="ChassisRed">
<color rgba="0.851 0.0 0.0 1.0"/>
@ -38,7 +38,7 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry name="pioneer_geom">
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/top.stl"/>
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/top.stl"/>
</geometry>
<material name="TopBlack">
<color rgba="0.038 0.038 0.038 1.0"/>
@ -64,7 +64,7 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry name="pioneer_geom">
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/front_sonar.stl"/>
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/front_sonar.stl"/>
</geometry>
<material name="SonarYellow">
<color rgba="0.715 0.583 0.210 1.0"/>
@ -93,7 +93,7 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry name="pioneer_geom">
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/back_sonar.stl"/>
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/back_sonar.stl"/>
</geometry>
<material name="SonarYellow">
<color rgba="0.715 0.583 0.210 1.0"/>
@ -118,7 +118,7 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry name="pioneer_geom">
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/caster_swivel.stl"/>
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/caster_swivel.stl"/>
</geometry>
<material name="caster_swivel">
<color rgba="0.5 0.5 0.5 1"/>
@ -138,7 +138,7 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry name="pioneer_geom">
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/caster_hubcap.stl"/>
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/caster_hubcap.stl"/>
</geometry>
<material name="caster_swivel">
<color rgba="0.5 0.5 0.5 1"/>
@ -172,7 +172,7 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry name="pioneer_geom">
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/caster_wheel.stl"/>
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/caster_wheel.stl"/>
</geometry>
<material name="WheelBlack">
<color rgba="0.117 0.117 0.117 1"/>
@ -181,7 +181,7 @@
<collision>
<origin rpy="-1.57079635 0 0" xyz="0 0 0"/>
<geometry>
<!--<mesh filename="$(find pioneer_description)/meshes/meshes/p3dx_meshes/caster_wheel.stl"/>-->
<!--<mesh filename="$(find pioneer_description)/meshes/caster_wheel.stl"/>-->
<cylinder length="0.01" radius="0.0375"/>
</geometry>
</collision>
@ -198,7 +198,7 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/left_hubcap.stl"/>
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/left_hubcap.stl"/>
</geometry>
<material name="HubcapYellow">
<color rgba="1.0 0.811 0.151 1.0"/>
@ -207,7 +207,7 @@
<collision>
<origin rpy="-1.57079635 0 0" xyz="0 0 0"/>
<geometry>
<!--<mesh filename="$(find pioneer_description)/meshes/meshes/p3dx_meshes/caster_wheel.stl"/>-->
<!--<mesh filename="$(find pioneer_description)/meshes/caster_wheel.stl"/>-->
<cylinder length="0.01" radius="0.09"/>
</geometry>
</collision>
@ -222,14 +222,14 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/left_wheel.stl"/>
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/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="fixed">
<joint name="left_wheel_joint" type="continuous">
<!-- type="continuous" -->
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="left_hub"/>
@ -242,7 +242,7 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/right_hubcap.stl"/>
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/right_hubcap.stl"/>
</geometry>
<material name="HubcapYellow">
<color rgba="1.0 0.811 0.151 1.0"/>
@ -251,7 +251,7 @@
<collision>
<origin rpy="-1.57079635 0 0" xyz="0 0 0"/>
<geometry>
<!--<mesh filename="$(find pioneer_description)/meshes/meshes/p3dx_meshes/caster_wheel.stl"/>-->
<!--<mesh filename="$(find pioneer_description)/meshes/caster_wheel.stl"/>-->
<cylinder length="0.01" radius="0.09"/>
</geometry>
</collision>
@ -269,14 +269,14 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/right_wheel.stl"/>
<mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/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="fixed">
<joint name="right_wheel_joint" type="continuous">
<!-- type="continuous" -->
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="right_hub"/>
@ -285,6 +285,48 @@
<gazebo reference="right_wheel_joint">
<material value="Gazebo/Black"/>
</gazebo>
<link name="laser">
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="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 rpy="0 0 0" xyz="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 rpy="0 0 0" xyz="0.040 0 0.0625"/>
<parent link="front_sonar"/>
<child link="laser"/>
</joint>
<link name="camera_frame">
<collision>
<origin rpy="0 0 0" xyz="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 rpy="0 0 0" xyz="0.040 0 0.1125"/>
<parent link="front_sonar"/>
<child link="camera_frame"/>
</joint>
<create>
<back_sonar parent="base_link"/>
<top_plate parent="base_link"/>

62
rescrictions_republisher.py Normal file
View File

@ -0,0 +1,62 @@
import rclpy
from rclpy.node import Node
from std_msgs.msg import Bool
from ros2aria_msgs.msg import RestrictionsMsg
from rclpy.qos import QoSDurabilityPolicy
from rclpy.qos import QoSHistoryPolicy
from rclpy.qos import QoSLivelinessPolicy
from rclpy.qos import QoSProfile
from rclpy.qos import QoSReliabilityPolicy
import sys
class RestrictionRepublisher(Node):
def __init__(self):
profile = QoSProfile(
depth=10,
history=QoSHistoryPolicy.KEEP_ALL,
reliability=QoSReliabilityPolicy.RELIABLE,
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL)
super().__init__('restriction_republisher')
self.restriction_pub_ = self.create_publisher(RestrictionsMsg, '/pioneers/restrictions', qos_profile=profile)
self.master_stop_pub_ = self.create_publisher(Bool, '/pioneers/master_stop', qos_profile=profile)
self.restriction_sub_ = self.create_subscription(RestrictionsMsg, '/pioneers/restrictions', self.restriction_callback, qos_profile=profile)
self.master_stop_sub_ = self.create_subscription(Bool, '/pioneers/master_stop', self.master_stop_callback, qos_profile=profile)
timer_period = 1.0
self.timer = self.create_timer(timer_period, self.timer_callback)
self.master_stop_msg = Bool()
self.restriction_msg = RestrictionsMsg()
self.master_stop_msg. data = True
self.restriction_msg.distance.data = sys.float_info.max
self.restriction_msg.linear_velocity.data = 0.0
self.restriction_msg.angular_velocity.data = 0.0
def timer_callback(self):
self.restriction_pub_.publish(self.restriction_msg)
self.master_stop_pub_.publish(self.master_stop_msg)
def restriction_callback(self, msg):
if msg != self.restriction_msg:
self.get_logger().info('New restricitons.')
self.restriction_msg = msg
def master_stop_callback(self, msg):
if msg != self.master_stop_msg:
self.get_logger().info('New master stop.')
self.master_stop_msg = msg
def main(args=None):
rclpy.init(args=args)
restriction_republisher = RestrictionRepublisher()
rclpy.spin(restriction_republisher)
restriction_republisher.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

74
rosbridge_websocket_launch.xml Normal file
View File

@ -0,0 +1,74 @@
<launch>
<arg name="port" default="9090" />
<arg name="address" default="" />
<arg name="ssl" default="false" />
<arg name="certfile" default=""/>
<arg name="keyfile" default="" />
<arg name="retry_startup_delay" default="5.0" />
<arg name="fragment_timeout" default="600" />
<arg name="delay_between_messages" default="0" />
<arg name="max_message_size" default="10000000" />
<arg name="unregister_timeout" default="10.0" />
<arg name="use_compression" default="false" />
<arg name="call_services_in_new_thread" default="false" />
<arg name="topics_glob" default="" />
<arg name="services_glob" default="" />
<arg name="params_glob" default="" />
<arg name="bson_only_mode" default="false" />
<arg name="pioneer_id" default="0" />
<arg unless="$(var bson_only_mode)" name="binary_encoder" default="default"/>
<group if="$(var ssl)">
<node name="rosbridge_websocket" pkg="rosbridge_server" exec="rosbridge_websocket" output="screen">
<param name="certfile" value="$(var certfile)" />
<param name="keyfile" value="$(var keyfile)" />
<param name="port" value="$(var port)"/>
<param name="address" value="$(var address)"/>
<param name="retry_startup_delay" value="$(var retry_startup_delay)"/>
<param name="fragment_timeout" value="$(var fragment_timeout)"/>
<param name="delay_between_messages" value="$(var delay_between_messages)"/>
<param name="max_message_size" value="$(var max_message_size)"/>
<param name="unregister_timeout" value="$(var unregister_timeout)"/>
<param name="use_compression" value="$(var use_compression)"/>
<param name="call_services_in_new_thread" value="$(var call_services_in_new_thread)"/>
<param name="topics_glob" value="$(var topics_glob)"/>
<param name="services_glob" value="$(var services_glob)"/>
<param name="params_glob" value="$(var params_glob)"/>
<remap from="/client_count" to="/pioneer$(var pioneer_id)/client_count"/>
<remap from="/connected_clients" to="/pioneer$(var pioneer_id)/connected_clients"/>
</node>
</group>
<group unless="$(var ssl)">
<node name="rosbridge_websocket" pkg="rosbridge_server" exec="rosbridge_websocket" output="screen">
<param name="port" value="$(var port)"/>
<param name="address" value="$(var address)"/>
<param name="retry_startup_delay" value="$(var retry_startup_delay)"/>
<param name="fragment_timeout" value="$(var fragment_timeout)"/>
<param name="delay_between_messages" value="$(var delay_between_messages)"/>
<param name="max_message_size" value="$(var max_message_size)"/>
<param name="unregister_timeout" value="$(var unregister_timeout)"/>
<param name="use_compression" value="$(var use_compression)"/>
<param name="call_services_in_new_thread" value="$(var call_services_in_new_thread)"/>
<param name="topics_glob" value="$(var topics_glob)"/>
<param name="services_glob" value="$(var services_glob)"/>
<param name="params_glob" value="$(var params_glob)"/>
<param name="bson_only_mode" value="$(var bson_only_mode)"/>
<remap from="/client_count" to="/pioneer$(var pioneer_id)/client_count"/>
<remap from="/connected_clients" to="/pioneer$(var pioneer_id)/connected_clients"/>
</node>
</group>
<node name="rosapi" pkg="rosapi" exec="rosapi_node">
<param name="topics_glob" value="$(var topics_glob)"/>
<param name="services_glob" value="$(var services_glob)"/>
<param name="params_glob" value="$(var params_glob)"/>
</node>
</launch>

3
src/pioneer_bringup/launch/robot.launch.py
View File

@ -41,6 +41,9 @@ def generate_launch_description():
namespace=namespace,
package='ros2aria',
executable='ros2aria',
parameters=[
{'use_sonar': False}
]
)
return LaunchDescription([

2
src/pioneer_bringup/package.xml
View File

@ -10,8 +10,8 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>xacro</depend>
<depend>joint_state_publisher</depend>
<depend>tf2_ros</depend>
<depend>robot_state_publisher</depend>
<depend>pioneer_description</depend>

5
src/pioneer_description/package.xml
View File

@ -20,11 +20,6 @@
<author>MobileRobots Inc.</author>
<author email="jakub.delicat@pwr.edu.pl">Jakub Delicat</author>
<build_depend>urdf</build_depend>
<build_depend>robot_state_publisher</build_depend>
<run_depend>urdf</run_depend>
<run_depend>robot_state_publisher</run_depend>
<export>
<build_type>ament_cmake</build_type>

43
src/pioneer_description/urdf/pioneer3dx.urdf.xacro
View File

@ -317,8 +317,51 @@
<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"/>

2
src/pioneer_description/urdf/pioneer3dx_wheel.xacro
View File

@ -78,7 +78,7 @@
</transmission>
<joint name="base_${suffix}_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<axis xyz="0 1 0"/>
<anchor xyz="0 0 0"/>
<limit effort="100" velocity="100" />
<joint_properties damping="0.0" friction="0.0" />

5
src/ros2aria/CMakeLists.txt
View File

@ -39,7 +39,10 @@ add_executable(ros2aria
src/gripper.cpp
src/wheels.cpp
src/clutch.cpp
src/state.cpp)
src/state.cpp
src/restrictions.cpp
src/scan.cpp
)
ament_target_dependencies(ros2aria rclcpp)
ament_target_dependencies(ros2aria std_srvs)

38
src/ros2aria/include/ros2aria/ros2aria.hpp
View File

@ -8,9 +8,11 @@
#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 "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"
@ -20,6 +22,18 @@
#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:
@ -27,11 +41,18 @@ public:
~Ros2Aria();
private:
// 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;
void handle_parameters();
// publishers
void publish();
sensor_msgs::msg::PointCloud handleSonar(rclcpp::Time stamp);
@ -56,12 +77,25 @@ private:
void publishState();
// 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;

24
src/ros2aria/src/cmd_vel.cpp
View File

@ -1,4 +1,5 @@
#include "ros2aria/ros2aria.hpp"
#include <cmath>
void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) {
float x, y, z;
@ -12,8 +13,23 @@ void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
this->robot->pokeWatchdog();
auto r = robot->getRobot();
r->setVel(x * 1e3);
if (r->hasLatVel())
r->setLatVel(y * 1e3);
r->setRotVel(z * 180 / M_PI);
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);
if (r->hasLatVel())
r->setLatVel(y);
r->setRotVel(z);
}

3
src/ros2aria/src/publish.cpp
View File

@ -2,12 +2,13 @@
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);

21
src/ros2aria/src/restrictions.cpp Normal file
View File

@ -0,0 +1,21 @@
#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;
}

30
src/ros2aria/src/ros2aria.cpp
View File

@ -8,15 +8,33 @@ Ros2Aria::Ros2Aria()
sensorCb(this, &Ros2Aria::publish)
{
this->robot = std::make_shared<RAIIBot>(this, "/dev/ttyS0");
handle_parameters();
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/wheels", 1000);
odom_tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
@ -34,13 +52,21 @@ 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());
}
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);
}
Ros2Aria::~Ros2Aria()
{
auto r = robot->getRobot();

19
src/ros2aria/src/scan.cpp Normal file
View File

@ -0,0 +1,19 @@
#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;
}
}

12
src/ros2aria/src/sonar.cpp
View File

@ -3,14 +3,14 @@
sensor_msgs::msg::PointCloud Ros2Aria::handleSonar(rclcpp::Time stamp)
{
sensor_msgs::msg::PointCloud cloud; //sonar readings.
sensor_msgs::msg::PointCloud cloud;
cloud.header.stamp = stamp;
// sonar sensors relative to base_link
cloud.header.frame_id = "sonar_frame";
cloud.header.frame_id = std::string(get_namespace()) + "/front_sonar";
auto r = robot->getRobot();
for (int i = 0; i < r->getNumSonar(); i++)
auto lenght = r->getNumSonar()/2;
for (int i = 0; i < num_of_sonars; i++)
{
ArSensorReading *reading = NULL;
reading = r->getSonarReading(i);
@ -37,6 +37,10 @@ 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;
}

9
src/ros2aria/src/state.cpp
View File

@ -20,16 +20,13 @@ void Ros2Aria::publishState()
robot_info_msg.robot_id.data = static_cast<uint8_t>(std::string(get_namespace()).back());
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;
// 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.state.data = not user_stop;
robot_info_msg.clutch.data = r->areMotorsEnabled();
// TODO: actually look for obstacles
robot_info_msg.obstacle_detected.data = false;
robot_info_msg.obstacle_detected.data = obstacle_too_close;
this->robot_info_pub_->publish(robot_info_msg);
}

10
src/ros2aria/src/wheels.cpp
View File

@ -1,5 +1,5 @@
#include "ros2aria/ros2aria.hpp"
#include <cmath>
sensor_msgs::msg::JointState Ros2Aria::handleWheels(rclcpp::Time stamp)
{
sensor_msgs::msg::JointState wheels;
@ -16,11 +16,11 @@ sensor_msgs::msg::JointState Ros2Aria::handleWheels(rclcpp::Time stamp)
// robot_state_publisher gives namespace
wheels.name[0] = "left_wheel_joint";
wheels.name[1] = "right_wheel_joint";
wheels.position[0] = r->getLeftEncoder();
wheels.position[1] = r->getRightEncoder();
wheels.velocity[0] = r->getLeftVel();
wheels.velocity[1] = r->getRightVel();
wheels.position[0] = encoder_to_rad(r->getLeftEncoder());
wheels.position[1] = encoder_to_rad(r->getRightEncoder());
wheels.velocity[0] = mm_per_sec_to_rad_per_sec(r->getLeftVel());
wheels.velocity[1] = mm_per_sec_to_rad_per_sec(r->getRightVel());
return wheels;
}

1
src/ros2aria_msgs/msg/RobotInfoMsg.msg
View File

@ -4,3 +4,4 @@ geometry_msgs/Twist twist
std_msgs/Bool state
std_msgs/Bool clutch
std_msgs/Bool obstacle_detected
std_msgs/Float64 minimal_distance

6
wymagania.md Normal file
View File

@ -0,0 +1,6 @@
Student:
- odblokowanie robota
- odblokowanie
- stan przejściowy na przyciskach