From 90edb4a2f237083d67b577d1490f2c269ed32b59 Mon Sep 17 00:00:00 2001
From: Sebastian <sebol117@student.liu.se>
Date: Thu, 6 Jul 2023 15:11:11 +0200
Subject: [PATCH] Working safety check
---
docker-compose.yml | 1 +
presets.json | 15 +++--
.../behaviours/motion_behaviours.py | 41 +++----------
.../behaviours/wait_for_touch_behaviour.py | 61 ++++++++++---------
.../lhw_intelligence/set_head.py | 2 +-
.../trees/scenario_trees/safety_check_tree.py | 12 ++--
src/lhw_qi/activate | 2 +-
src/lhw_tablet/activate | 2 +-
src/move_base_msgs/CMakeLists.txt | 29 +++++++++
.../msg/MoveBaseActionResult.msg | 3 +
src/move_base_msgs/msg/MoveBaseResult.msg | 0
src/move_base_msgs/package.xml | 26 ++++++++
12 files changed, 115 insertions(+), 79 deletions(-)
create mode 100644 src/move_base_msgs/CMakeLists.txt
create mode 100644 src/move_base_msgs/msg/MoveBaseActionResult.msg
create mode 100644 src/move_base_msgs/msg/MoveBaseResult.msg
create mode 100644 src/move_base_msgs/package.xml
diff --git a/docker-compose.yml b/docker-compose.yml
index 6d405b26..95071a00 100644
--- a/docker-compose.yml
+++ b/docker-compose.yml
@@ -25,6 +25,7 @@ services:
- NVIDIA_VISIBLE_DEVICES=all
- LD_LIBRARY_PATH=/usr/local/cuda/lib:/usr/local/cuda/lib64
- NVIDIA_DRIVER_CAPABILITIES=compute,utility
+ - PEPPER_IP=169.254.127.207
network_mode: host
ipc: host
tty: True
diff --git a/presets.json b/presets.json
index f2eef492..ca6e9c7f 100644
--- a/presets.json
+++ b/presets.json
@@ -31,12 +31,6 @@
"safety_check" : {
"containers" : ["lhw_qi", "lhw_tablet", "lhw_intelligence"],
"commands" : [
-
- {
- "container" : "lhw_qi",
- "command" : "ros2 run lhw_qi tablet -i $PEPPER_IP",
- "verbose" : true
- },
{
"container" : "lhw_qi",
"command" : "ros2 run lhw_qi microphone -i $PEPPER_IP",
@@ -48,8 +42,13 @@
"verbose" : true
},
{
- "container" : "lhw_tablet",
- "command" : "ros2 run lhw_tablet action_server",
+ "container" : "lhw_qi",
+ "command" : "ros2 run lhw_qi motion_topics -i $PEPPER_IP",
+ "verbose" : false
+ },
+ {
+ "container" : "lhw_intelligence",
+ "command" : "ros2 run lhw_intelligence head_fix_node",
"verbose" : true
},
{
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/motion_behaviours.py b/src/lhw_intelligence/lhw_intelligence/behaviours/motion_behaviours.py
index a3f5fac9..1978c666 100644
--- a/src/lhw_intelligence/lhw_intelligence/behaviours/motion_behaviours.py
+++ b/src/lhw_intelligence/lhw_intelligence/behaviours/motion_behaviours.py
@@ -12,14 +12,16 @@ Behaviours for basic motions
import py_trees
import time
+import math
import lhw_interfaces.msg as lhw_msgs
import lhw_interfaces.srv as lhw_srvs
from rclpy.action import ActionClient
import lhw_interfaces.action as lhw_actions
from geometry_msgs.msg import Twist, Pose, PoseStamped
from actionlib_msgs.msg import GoalStatusArray
+from move_base_msgs.msg import MoveBaseActionResult
-from geometry_msgs.msg import Pose
+from geometry_msgs.msg import Pose, PoseWithCovarianceStamped
from std_msgs.msg import Bool, String
# from lhw_interfaces.action import RobotAction
@@ -99,12 +101,8 @@ class MoveToPoint(py_trees.behaviour.Behaviour):
msg = Bool()
msg.data = True
- self.accepted = False
self.fix_head_pub.publish(msg)
self.move_base_status = 0
- self.prior_status = 0
-
-
#Orientation
goal_orientation = '{{x: {0}, y: {1}, z: {2}, w: {3}}}'
g_o = goal_orientation.format(self.pose.orientation.x, self.pose.orientation.y, self.pose.orientation.z, self.pose.orientation.w) # Unpack the list as laid out args
@@ -112,7 +110,7 @@ class MoveToPoint(py_trees.behaviour.Behaviour):
print(g_o)
#Position
goal_position = '{{x: {0}, y: {1}, z: {2}}}'
- self.move_status_subscription = self.node.create_subscription(GoalStatusArray, "/lhw/nav/move_base/status", self.goal_feedback_callback, 10)
+ self.move_result_subscription = self.node.create_subscription(MoveBaseActionResult, "/lhw/nav/move_base/result", self.goal_result_callback, 10)
g_p = goal_position.format(self.pose.position.x, self.pose.position.y, self.pose.position.z)
self.goal_cmd = f"rostopic pub /lhw/nav/goal_server/goal geometry_msgs/PoseStamped \"{{header: {{frame_id: 'map'}}, pose: {{position: {g_p}, orientation: {g_o} }}}}\" --once"
@@ -129,40 +127,19 @@ class MoveToPoint(py_trees.behaviour.Behaviour):
def update(self):
print("Listening for callback...")
-
- if self.move_base_status == 2 and self.prior_status == 2:
- return py_trees.common.Status.SUCCESS
- if not self.accepted:
- return py_trees.common.Status.RUNNING
-
- if self.move_base_status == 3:
+ if self.move_base_status == 2 or self.move_base_status == 3:
return py_trees.common.Status.SUCCESS
- if self.move_base_status >= 4:
+ elif self.move_base_status >= 4:
return py_trees.common.Status.FAILED
else:
return py_trees.common.Status.RUNNING
def terminate(self, new_status: py_trees.common.Status):
- self.node.destroy_subscription(self.move_status_subscription)
-
- def goal_feedback_callback(self, msg):
- try:
- print(msg.status_list[0].status, "Current Status")
- except IndexError:
- return
-
- if msg.status_list[0].status == 1:
- self.accepted = True
-
- # if self.move_base_status == 1 and self.prior_status == 2:
- # self.resend = True
- # stdin, stdout, stderr = self.ssh_client.exec_command("source ~/.bash_profile && " + self.goal_cmd)
-
- #May Add more Statuses if necessary
+ self.node.destroy_subscription(self.move_result_subscription)
- self.prior_status = self.move_base_status
- self.move_base_status = msg.status_list[0].status
+ def goal_result_callback(self, msg):
+ self.move_base_status = msg.status.status
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/wait_for_touch_behaviour.py b/src/lhw_intelligence/lhw_intelligence/behaviours/wait_for_touch_behaviour.py
index be4fcecd..a0aadb76 100644
--- a/src/lhw_intelligence/lhw_intelligence/behaviours/wait_for_touch_behaviour.py
+++ b/src/lhw_intelligence/lhw_intelligence/behaviours/wait_for_touch_behaviour.py
@@ -42,16 +42,16 @@ class WaitForHeadTouch(py_trees.behaviour.Behaviour):
raise KeyError(error_message) from e # 'direct cause' traceability
self.led_pub = self.node.create_publisher(std_msgs.msg.Bool, "turn_on_off", 10)
- self.tablet_action_client = rclpy.action.ActionClient(
- node=self.node,
- action_type=TabletAction,
- action_name="tablet_action_server"
- )
+ #self.tablet_action_client = rclpy.action.ActionClient(
+ # node=self.node,
+ # action_type=TabletAction,
+ # action_name="tablet_action_server"
+ #)
- if not self.tablet_action_client.wait_for_server(timeout_sec=3.0):
- raise RuntimeError("client timed out waiting for server tablet_action_server")
+ #if not self.tablet_action_client.wait_for_server(timeout_sec=3.0):
+ # raise RuntimeError("client timed out waiting for server tablet_action_server")
- self.logger.debug("Action client successfully created")
+ #self.logger.debug("Action client successfully created")
def initialise(self):
@@ -64,14 +64,14 @@ class WaitForHeadTouch(py_trees.behaviour.Behaviour):
#self.timer = self.node.create_timer(1.0, self._toggle_leds_callback)
self.head_touch_sub = self.node.create_subscription(HeadTouch, 'head_touch', self._head_touch_callback, 10)
self.blackboard.scenario_started = False
- goal = TabletAction.Goal()
- goal.type = "text"
- goal.text = "Please tap on my head"
-
- self.send_goal_future = self.tablet_action_client.send_goal_async(
- goal,
- feedback_callback=None
- )
+ #goal = TabletAction.Goal()
+ #goal.type = "text"
+ #goal.text = "Please tap on my head"
+
+ #self.send_goal_future = self.tablet_action_client.send_goal_async(
+ # goal,
+ # feedback_callback=None
+ #)
self.logger.debug("Action goal sent")
@@ -107,26 +107,27 @@ class WaitForHeadTouch(py_trees.behaviour.Behaviour):
goal.type = "text"
goal.text = "Scenario Started"
- self.send_goal_future = self.tablet_action_client.send_goal_async(
- goal,
- feedback_callback=None
- )
+ #self.send_goal_future = self.tablet_action_client.send_goal_async(
+ # goal,
+ # feedback_callback=None
+ #)
def shutdown(self):
"""
This function is executed when the tree is destroyed. Destroy the publisher created in setup.
"""
- goal = TabletAction.Goal()
- goal.type = "image"
- goal.url = "assets/fia_logo.png"
-
- self.send_goal_future = self.tablet_action_client.send_goal_async(
- goal,
- feedback_callback=None
- )
- self.led_pub.destroy()
- self.tablet_action_client.destroy()
+ pass
+ #goal = TabletAction.Goal()
+ #goal.type = "image"
+ #goal.url = "assets/fia_logo.png"
+
+ #self.send_goal_future = self.tablet_action_client.send_goal_async(
+ # goal,
+ # feedback_callback=None
+ #)
+ #self.led_pub.destroy()
+ #self.tablet_action_client.destroy()
def _toggle_leds_callback(self):
diff --git a/src/lhw_intelligence/lhw_intelligence/set_head.py b/src/lhw_intelligence/lhw_intelligence/set_head.py
index 0307c7e7..09334d60 100644
--- a/src/lhw_intelligence/lhw_intelligence/set_head.py
+++ b/src/lhw_intelligence/lhw_intelligence/set_head.py
@@ -14,7 +14,7 @@ class HeadMotion(Node):
self.robot_action_pub = self.create_publisher(RobotAction, 'robot_action', 1)
self.head_fix_timer = self.create_timer(1.0, self.head_fix_timer_cb)
- self.fix_head = False
+ self.fix_head = True
def head_fix_callback(self, msg):
self.fix_head = msg.data
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/safety_check_tree.py b/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/safety_check_tree.py
index 18ac4672..911f755c 100644
--- a/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/safety_check_tree.py
+++ b/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/safety_check_tree.py
@@ -100,17 +100,17 @@ def create_root(ssh) -> py_trees.behaviour.Behaviour:
name="Safety Check Sequence",
children=[
set_pose,
- py_trees.timers.Timer(name="Wait", duration=5),
+ py_trees.timers.Timer(name="Wait1", duration=5),
say_door_open,
wait_for_door,
move_inside_door,
- py_trees.timers.Timer(name="Wait", duration=1),
+ #py_trees.timers.Timer(name="Wait2", duration=1),
move_middle,
- py_trees.timers.Timer(name="Wait", duration=1),
+ #py_trees.timers.Timer(name="Wait3", duration=1),
move_before_safety,
- py_trees.timers.Timer(name="Wait", duration=1),
+ #py_trees.timers.Timer(name="Wait4", duration=1),
move_to_safety_point,
- py_trees.timers.Timer(name="Wait", duration=8),
+ #py_trees.timers.Timer(name="Wait5", duration=8),
speech_tree,
move_to_exit,
idle
@@ -128,7 +128,7 @@ def main():
ssh_client.set_missing_host_key_policy(paramiko.AutoAddPolicy)
pepper_ip = os.getenv("PEPPER_IP")
if not pepper_ip:
- pepper_ip = "169.254.3.76"
+ pepper_ip = "169.254.127.207"
ssh_client.connect(hostname = pepper_ip, username = "nao", password = "ellis")
ssh_client.invoke_shell()
root = create_root(ssh_client)
diff --git a/src/lhw_qi/activate b/src/lhw_qi/activate
index 2987a047..6551058b 100644
--- a/src/lhw_qi/activate
+++ b/src/lhw_qi/activate
@@ -23,7 +23,7 @@ echo "Searching for the IP of Pepper"
#export PEPPER_IP=$(arp-scan --localnet | grep -i "48:a9:d2:8c:6a:0c\|00:13:95:1d:4c:43" | cut -f1) - pepper linköping MAC
#export PEPPER_IP=$(arp-scan --localnet | grep -i "48:a9:d2:8c:68:0c\|00:13:95:1d:4c:43" | cut -f1)
-export PEPPER_IP=169.254.3.76
+export PEPPER_IP=169.254.127.207
if [[ -z "${PEPPER_IP}" ]]; then
echo "${ORANGE}Pepper was not found on the current network, please set PEPPER_IP manually!${NC}"
else
diff --git a/src/lhw_tablet/activate b/src/lhw_tablet/activate
index 2b9a761f..f61e0cac 100644
--- a/src/lhw_tablet/activate
+++ b/src/lhw_tablet/activate
@@ -21,7 +21,7 @@ ldconfig
# Search for pepper on the local network
echo "Searching for the IP of Pepper"
#export EPPER_IP=$(arp-scan --localnet | grep -i "48:a9:d2:8c:6a:0c\|00:13:95:1d:4c:43" | cut -f1)
-export PEPPER_IP=169.254.3.76
+
if [[ -z "${PEPPER_IP}" ]]; then
echo "${ORANGE}Pepper was not found on the current network, please set PEPPER_IP manually!${NC}"
else
diff --git a/src/move_base_msgs/CMakeLists.txt b/src/move_base_msgs/CMakeLists.txt
new file mode 100644
index 00000000..ddc60329
--- /dev/null
+++ b/src/move_base_msgs/CMakeLists.txt
@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.5)
+project(move_base_msgs)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(actionlib_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+include_directories(${PYTHON_INCLUDE_DIRS})
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+ "msg/MoveBaseActionResult.msg"
+ "msg/MoveBaseResult.msg"
+ DEPENDENCIES std_msgs actionlib_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()
diff --git a/src/move_base_msgs/msg/MoveBaseActionResult.msg b/src/move_base_msgs/msg/MoveBaseActionResult.msg
new file mode 100644
index 00000000..c1a07d4d
--- /dev/null
+++ b/src/move_base_msgs/msg/MoveBaseActionResult.msg
@@ -0,0 +1,3 @@
+std_msgs/Header header
+actionlib_msgs/GoalStatus status
+MoveBaseResult result
\ No newline at end of file
diff --git a/src/move_base_msgs/msg/MoveBaseResult.msg b/src/move_base_msgs/msg/MoveBaseResult.msg
new file mode 100644
index 00000000..e69de29b
diff --git a/src/move_base_msgs/package.xml b/src/move_base_msgs/package.xml
new file mode 100644
index 00000000..455f03a6
--- /dev/null
+++ b/src/move_base_msgs/package.xml
@@ -0,0 +1,26 @@
+<?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>move_base_msgs</name>
+ <version>0.0.0</version>
+ <description>TODO: Package description</description>
+ <maintainer email="root@todo.todo">root</maintainer>
+ <license>TODO: License declaration</license>
+
+ <buildtool_depend>ament_cmake</buildtool_depend>
+ <build_depend>rosidl_default_generators</build_depend>
+
+ <exec_depend>rosidl_default_runtime</exec_depend>
+
+ <depend>std_msgs</depend>
+ <depend>actionlib_msgs</depend>
+
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>ament_lint_common</test_depend>
+
+ <member_of_group>rosidl_interface_packages</member_of_group>
+
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
+</package>
--
GitLab