From c53f3eb02c35bc51d485d300f63ccbb72ab3c374 Mon Sep 17 00:00:00 2001
From: Hilario Tome <hilario.tome@pal-robotics.com>
Date: Sun, 23 Oct 2016 16:54:09 +0200
Subject: [PATCH] Succesfull walking in talos, added talos teleop
---
talos_bringup/config/joy_teleop.yaml | 76 +++++++++++++++++++
talos_bringup/config/twist_mux/joystick.yaml | 10 +++
.../config/twist_mux/twist_mux_locks.yaml | 28 +++++++
.../config/twist_mux/twist_mux_topics.yaml | 28 +++++++
talos_bringup/launch/joystick_teleop.launch | 15 ++++
talos_bringup/launch/talos_bringup.launch | 6 ++
talos_bringup/launch/twist_mux.launch | 12 +++
talos_bringup/package.xml | 4 +-
talos_controller_configuration/package.xml | 4 +-
talos_description/package.xml | 2 +-
10 files changed, 180 insertions(+), 5 deletions(-)
create mode 100644 talos_bringup/config/joy_teleop.yaml
create mode 100644 talos_bringup/config/twist_mux/joystick.yaml
create mode 100644 talos_bringup/config/twist_mux/twist_mux_locks.yaml
create mode 100644 talos_bringup/config/twist_mux/twist_mux_topics.yaml
create mode 100644 talos_bringup/launch/joystick_teleop.launch
create mode 100644 talos_bringup/launch/twist_mux.launch
diff --git a/talos_bringup/config/joy_teleop.yaml b/talos_bringup/config/joy_teleop.yaml
new file mode 100644
index 0000000..25e2e6b
--- /dev/null
+++ b/talos_bringup/config/joy_teleop.yaml
@@ -0,0 +1,76 @@
+teleop:
+ walk:
+ type: topic
+ message_type: geometry_msgs/Twist
+ topic_name: cmd_vel
+ deadman_buttons: [4]
+ axis_mappings:
+ -
+ axis: 1
+ target: linear.x
+ scale: 0.25
+ -
+ axis: 0
+ target: angular.z
+ scale: 0.25
+ -
+ axis: 2
+ target: linear.y
+ scale: 0.15
+
+ joy_priority:
+ type: action
+ action_name: joy_priority_action
+ buttons: [0]
+
+ # undocumented poses, with 'back' button
+ home:
+ type: action
+ action_name: play_motion
+ action_goal:
+ motion_name: home
+ buttons: [8, 1]
+
+ hands_walk:
+ type: action
+ action_name: play_motion
+ action_goal:
+ motion_name: hands_walk
+ buttons: [8, 2]
+
+ # documented motions, with right upper button and right trigger
+ # see SDE4 documentation
+ wave:
+ type: action
+ action_name: play_motion
+ action_goal:
+ motion_name: wave
+ buttons: [5, 1]
+
+ bow:
+ type: action
+ action_name: play_motion
+ action_goal:
+ motion_name: bow
+ buttons: [5, 0]
+
+ open_arms:
+ type: action
+ action_name: play_motion
+ action_goal:
+ motion_name: open_arms
+ buttons: [5, 3]
+
+ shake_hand:
+ type: action
+ action_name: play_motion
+ action_goal:
+ motion_name: shake_right
+ buttons: [5, 2]
+
+ point_front_left:
+ type: action
+ action_name: play_motion
+ action_goal:
+ motion_name: point_front_left
+ buttons: [7, 0]
diff --git a/talos_bringup/config/twist_mux/joystick.yaml b/talos_bringup/config/twist_mux/joystick.yaml
new file mode 100644
index 0000000..e89cc5d
--- /dev/null
+++ b/talos_bringup/config/twist_mux/joystick.yaml
@@ -0,0 +1,10 @@
+turbo:
+ linear_forward_min : 1.0
+ linear_forward_max : 1.0
+ linear_backward_min : 1.0
+ linear_backward_max : 1.0
+ linear_lateral_min : 1.0
+ linear_lateral_max : 1.0
+ angular_min : 1.0
+ angular_max : 1.0
+ steps : 1
diff --git a/talos_bringup/config/twist_mux/twist_mux_locks.yaml b/talos_bringup/config/twist_mux/twist_mux_locks.yaml
new file mode 100644
index 0000000..d09a73e
--- /dev/null
+++ b/talos_bringup/config/twist_mux/twist_mux_locks.yaml
@@ -0,0 +1,28 @@
+# Locks to stop the twist mux.
+# For each lock:
+# - topic : input topic that provides the lock; it must be of type std_msgs::Bool?!!!
+# - timeout : == 0.0 -> not used
+# > 0.0 -> the lock is supposed to published at a certain frequency in order
+# to detect that the publisher is alive; the timeout in seconds allows
+# to detect that, and if the publisher dies we will enable the lock
+# - priority: priority in the range [0, 255], so all the topics with priority lower than it
+# will be stopped/disabled
+
+locks:
+-
+ name : pause
+ topic : pause_navigation
+ timeout : 0.0
+ # Same priority as joystick control, so it'll not block it.
+ priority: 100
+-
+ name : loop_closure
+ topic : stop_closing_loop
+ timeout : 0.0
+ priority: 200
+-
+ name : joystick
+ topic : joy_priority
+ timeout : 0.0
+ priority: 100
+
diff --git a/talos_bringup/config/twist_mux/twist_mux_topics.yaml b/talos_bringup/config/twist_mux/twist_mux_topics.yaml
new file mode 100644
index 0000000..9e3fc28
--- /dev/null
+++ b/talos_bringup/config/twist_mux/twist_mux_topics.yaml
@@ -0,0 +1,28 @@
+# Input topics handled/muxed by the twist mux.
+# For each topic:
+# - name : name identifier to select the topic
+# - topic : input topic of geometry_msgs::Twist type
+# - timeout : timeout in seconds to start discarding old messages, and use 0.0 speed instead
+# - priority: priority in the range [0, 255]; the higher the more priority over other topics
+
+topics:
+-
+ name : navigation
+ topic : nav_vel
+ timeout : 0.5
+ priority: 10
+-
+ name : joystick
+ topic : joy_vel
+ timeout : 0.5
+ priority: 100
+-
+ name : keyboard
+ topic : key_vel
+ timeout : 0.5
+ priority: 90
+-
+ name : tablet
+ topic : tab_vel
+ timeout : 0.5
+ priority: 100
diff --git a/talos_bringup/launch/joystick_teleop.launch b/talos_bringup/launch/joystick_teleop.launch
new file mode 100644
index 0000000..49f142c
--- /dev/null
+++ b/talos_bringup/launch/joystick_teleop.launch
@@ -0,0 +1,15 @@
+<launch>
+
+ <arg name="cmd_vel" default="input_joy/cmd_vel" />
+ <arg name="teleop_config" default="$(find talos_bringup)/config/joy_teleop.yaml" />
+
+ <rosparam file="$(arg teleop_config)" command="load" />
+ <node pkg="joy_teleop" type="joy_teleop.py" name="joy_teleop">
+ <remap from="cmd_vel" to="$(arg cmd_vel)" />
+ </node>
+
+ <node pkg="joy" type="joy_node" name="joystick">
+ <param name="autorepeat_rate" value="10" />
+ </node>
+
+</launch>
diff --git a/talos_bringup/launch/talos_bringup.launch b/talos_bringup/launch/talos_bringup.launch
index 5f2fb7a..b4654e0 100644
--- a/talos_bringup/launch/talos_bringup.launch
+++ b/talos_bringup/launch/talos_bringup.launch
@@ -18,4 +18,10 @@
<arg name="robot" value="$(arg robot)"/>
</include>
+ <!-- Twist mux -->
+ <include file="$(find talos_bringup)/launch/twist_mux.launch" />
+
+ <!-- Joystick controller -->
+ <include file="$(find talos_bringup)/launch/joystick_teleop.launch" />
+
</launch>
diff --git a/talos_bringup/launch/twist_mux.launch b/talos_bringup/launch/twist_mux.launch
new file mode 100644
index 0000000..09c3d8a
--- /dev/null
+++ b/talos_bringup/launch/twist_mux.launch
@@ -0,0 +1,12 @@
+<launch>
+
+ <include file="$(find twist_mux)/launch/twist_mux.launch">
+ <arg name="cmd_vel_out" value="walking_controller/cmd_vel"/>
+
+ <arg name="config_locks" value="$(find reemc_bringup)/config/twist_mux/twist_mux_locks.yaml"/>
+ <arg name="config_topics" value="$(find reemc_bringup)/config/twist_mux/twist_mux_topics.yaml"/>
+
+ <arg name="config_joy" value="$(find talos_bringup)/config/twist_mux/joystick.yaml"/>
+ </include>
+
+</launch>
diff --git a/talos_bringup/package.xml b/talos_bringup/package.xml
index c68f24b..d4a6fe3 100644
--- a/talos_bringup/package.xml
+++ b/talos_bringup/package.xml
@@ -4,9 +4,9 @@
<version>0.0.2</version>
<description>The talos_bringup package</description>
- <maintainer email="hilario@todo.todo">hilario</maintainer>
+ <maintainer email="hilario.tome@pal-robotics.com">Hilario Tome</maintainer>
- <license>TODO</license>
+ <license>Proprietary</license>
<buildtool_depend>catkin</buildtool_depend>
diff --git a/talos_controller_configuration/package.xml b/talos_controller_configuration/package.xml
index 0953b54..89bbf19 100644
--- a/talos_controller_configuration/package.xml
+++ b/talos_controller_configuration/package.xml
@@ -4,9 +4,9 @@
<version>0.0.2</version>
<description>The talos_controller_configuration package</description>
- <maintainer email="hilario@todo.todo">hilario</maintainer>
+ <maintainer email="hilario.tome@pal-robotics.com">Hilario Tome</maintainer>
- <license>TODO</license>
+ <license>Proprietary</license>
<buildtool_depend>catkin</buildtool_depend>
diff --git a/talos_description/package.xml b/talos_description/package.xml
index 4c430b2..20276da 100644
--- a/talos_description/package.xml
+++ b/talos_description/package.xml
@@ -6,7 +6,7 @@
<maintainer email="hilario.tome@pal-robotics.com">Hilario Tomé</maintainer>
- <license>TODO</license>
+ <license>Proprietary</license>
<buildtool_depend>catkin</buildtool_depend>
--
GitLab