Merge pull request #798 from ipa-jba/feature/raw-mini

Feature/raw mini

cob_bringup/CMakeLists.txt

@@ -13,6 +13,15 @@ if(CATKIN_ENABLE_TESTING)
     message("testing for robot: ${robot}")
     roslaunch_add_file_check(robots/${robot}.xml)
   endforeach()
+
+  # additional robots not officially supported
+  set(test_list
+    raw-mini
+  )
+  foreach(robot ${test_list})
+    message("testing for robot: ${robot}")
+    roslaunch_add_file_check(robots/${robot}.xml)
+  endforeach()
 endif()
 
 ### INSTALL ###

cob_bringup/components/raw_mini_base.launch

@@ -0,0 +1,39 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="robot"/>
+
+  <arg name="pkg_hardware_config" default="$(find cob_hardware_config)"/>
+  <arg name="sim" default="false"/>
+
+
+  <group ns="base">
+    <group unless="$(arg sim)">
+      <node pkg="rosserial_python" type="serial_node.py" name ="rosserial" output="screen">
+        <param name="port" value="/dev/ttyACM0"/>
+        <param name="baud" value="115200"/>
+      </node>
+
+      <node pkg="cob_mecanum_controller" type="cob_mecanum_controller_node" name="mecanum_controller" output="log">
+        <param name="lx" value="0.115" />
+        <param name="ly" value="0.09" />
+        <param name="r" value="0.1" />
+      </node>
+    </group>
+    <!-- twist mux -->
+    <include file="$(find cob_bringup)/tools/twist_mux.launch">
+      <arg name="robot" value="$(arg robot)"/>
+      <arg name="cmd_vel_out" default="velocity_smoother/command"/>
+      <arg name="marker_out" default="twist_mux/marker"/>
+    </include>
+
+    <!-- velocity smoother -->
+    <include file="$(find cob_bringup)/tools/velocity_smoother.launch">
+      <arg name="robot" value="$(arg robot)"/>
+      <arg name="config_file" default="$(arg pkg_hardware_config)/robots/$(arg robot)/config/velocity_smoother.yaml"/>
+      <arg name="raw_cmd_vel_topic" default="velocity_smoother/command"/>
+      <arg name="smooth_cmd_vel_topic" default="cmd_vel"/>
+      <arg name="robot_cmd_vel_topic" default="cmd_vel"/>
+      <arg name="odom_topic" default="odom"/>
+    </include>
+  </group>
+</launch>

cob_bringup/drivers/rplidar.launch

@@ -0,0 +1,9 @@
+<launch>
+  <arg name="pkg_hardware_config" />
+  <arg name="robot" />
+
+  <node pkg="rplidar_ros" type="rplidarNode" name="rplidar_node" output="screen">
+    <rosparam command="load" file="$(arg pkg_hardware_config)/robots/$(arg robot)/config/rplidar.yaml"/>
+  </node>
+
+</launch>

cob_bringup/package.xml

@@ -31,24 +31,25 @@
   <exec_depend>cob_default_env_config</exec_depend>
   <exec_depend>cob_default_robot_behavior</exec_depend>
   <exec_depend>cob_default_robot_config</exec_depend>
-  <exec_depend>cob_docker_control</exec_depend> <!-- from cob_substitute -->
+  <exec_depend>cob_docker_control</exec_depend>  <!-- from cob_substitute -->
   <exec_depend>cob_frame_tracker</exec_depend>
   <exec_depend>cob_hand_bridge</exec_depend>
   <exec_depend>cob_hardware_config</exec_depend>
   <exec_depend>cob_helper_tools</exec_depend>
   <exec_depend>cob_image_flip</exec_depend>
   <exec_depend>cob_light</exec_depend>
   <exec_depend>cob_linear_nav</exec_depend>
+  <exec_depend>cob_mecanum_controller</exec_depend>
   <exec_depend>cob_mimic</exec_depend>
   <exec_depend>cob_monitoring</exec_depend>
   <exec_depend>cob_moveit_config</exec_depend>
   <exec_depend>cob_obstacle_distance</exec_depend>
   <exec_depend>cob_omni_drive_controller</exec_depend>
-  <exec_depend>cob_phidgets</exec_depend>
-  <exec_depend>cob_phidget_power_state</exec_depend>
   <exec_depend>cob_phidget_em_state</exec_depend>
-  <exec_depend>cob_reflector_referencing</exec_depend> <!-- from cob_substitute -->
-  <exec_depend>cob_safety_controller</exec_depend> <!-- from cob_substitute -->
+  <exec_depend>cob_phidget_power_state</exec_depend>
+  <exec_depend>cob_phidgets</exec_depend>
+  <exec_depend>cob_reflector_referencing</exec_depend>  <!-- from cob_substitute -->
+  <exec_depend>cob_safety_controller</exec_depend>  <!-- from cob_substitute -->
   <exec_depend>cob_scan_unifier</exec_depend>
   <exec_depend>cob_script_server</exec_depend>
   <exec_depend>cob_sick_lms1xx</exec_depend>
@@ -57,8 +58,8 @@
   <exec_depend>cob_teleop</exec_depend>
   <exec_depend>cob_twist_controller</exec_depend>
   <exec_depend>cob_voltage_control</exec_depend>
-  <exec_depend>compressed_depth_image_transport</exec_depend> <!-- needed by the cameras to publish the compressed topics -->
-  <exec_depend>compressed_image_transport </exec_depend> <!-- needed by the cameras to publish the compressed topics -->
+  <exec_depend>compressed_depth_image_transport</exec_depend>  <!-- needed by the cameras to publish the compressed topics -->
+  <exec_depend>compressed_image_transport </exec_depend>  <!-- needed by the cameras to publish the compressed topics -->
   <exec_depend>controller_manager</exec_depend>
   <exec_depend>costmap_2d</exec_depend>
   <exec_depend>diagnostic_aggregator</exec_depend>
@@ -74,7 +75,7 @@
   <exec_depend>openni2_launch</exec_depend>
   <exec_depend>position_controllers</exec_depend>
   <exec_depend>realsense_camera</exec_depend>
-  <!--exec_depend>realsense2_camera</exec_depend--> <!-- not released into kinetic -->
+  <!--exec_depend>realsense2_camera</exec_depend-->  <!-- not released into kinetic -->
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>rosserial_python</exec_depend>
   <exec_depend>rosserial_server</exec_depend>
@@ -84,7 +85,7 @@
   <!--exec_depend>sick_visionary_t_driver</exec_depend-->
   <exec_depend>spacenav_node</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
-  <exec_depend>theora_image_transport</exec_depend> <!-- needed by the cameras to publish the compressed topics -->
+  <exec_depend>theora_image_transport</exec_depend>  <!-- needed by the cameras to publish the compressed topics -->
   <exec_depend>topic_tools</exec_depend>
   <exec_depend>twist_mux</exec_depend>
   <!--exec_depend>ur_driver</exec_depend-->

cob_bringup/robots/raw-mini.launch

@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+<launch>
+
+  <arg name="env-script" default="$(find cob_bringup)/env.sh"/>
+
+  <include file="$(find cob_bringup)/robots/raw-mini.xml"/>
+
+</launch>

cob_bringup/robots/raw-mini.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- args -->
+  <arg name="robot" value="raw-mini"/>
+  <arg name="sim" default="false"/>
+  <arg name="pkg_hardware_config" default="$(find cob_hardware_config)"/>
+
+
+  <!-- upload robot description -->
+  <include file="$(find cob_hardware_config)/upload_robot.launch">
+    <arg name="robot" value="$(arg robot)"/>
+  </include>
+
+  <!-- start hardware -->
+  <group unless="$(arg sim)">
+    <include file="$(find cob_bringup)/drivers/rplidar.launch">
+      <arg name="robot" value="$(arg robot)" />
+      <arg name="pkg_hardware_config" value="$(arg pkg_hardware_config)" />
+    </include>
+  </group>
+
+  <include file="$(find cob_bringup)/components/raw_mini_base.launch">
+    <arg name="robot" value="$(arg robot)" />
+    <arg name="sim" value="$(arg sim)"/>
+  </include>
+
+  <!-- start additional packages -->
+  <include file="$(find cob_bringup)/tools/teleop.launch">
+    <arg name="robot" value="$(arg robot)"/>
+  </include>
+
+  <include file="$(find cob_bringup)/tools/robot_state_publisher.launch">
+    <arg name="robot" value="$(arg robot)"/>
+  </include>
+</launch>

cob_hardware_config/robots/raw-mini/config/joint_state_publisher.yaml

@@ -0,0 +1,2 @@
+rate: 100 #Hz
+source_list: [/base/joint_states]

cob_hardware_config/robots/raw-mini/config/joy.yaml

@@ -0,0 +1,2 @@
+dev: /dev/input/js0
+deadzone: 0.12

cob_hardware_config/robots/raw-mini/config/rplidar.yaml

@@ -0,0 +1,5 @@
+serial_port: "/dev/ttyUSB0"
+serial_baudrate: 115200
+frame_id: laser_link
+inverted: false
+angle_compensate: true

cob_hardware_config/robots/raw-mini/config/teleop.yaml

@@ -0,0 +1,33 @@
+#raw-mini
+##########
+# common params
+run_factor: 2.0
+apply_ramp: false
+joy_num_buttons: 12
+joy_num_axes: 6
+joy_num_modes: 1
+
+# axes
+axis_vx: 4
+axis_vy: 3
+axis_vz: 5
+axis_roll: 4
+axis_pitch: 3
+axis_yaw: 0
+
+# buttons
+deadman_button: 5
+safety_button: 6
+init_button: 9
+
+#mode1: Base
+run_button: 7
+
+components: {
+  base: {
+    twist_topic_name: '/base/twist_mux/command_teleop_joy',
+    twist_safety_topic_name: '/base/twist_mux/command_teleop_joy',
+    twist_max_velocity: [0.5, 0.5, 0.5],
+    twist_max_acc: [0.5, 0.5, 0.7]
+  }
+}

cob_hardware_config/robots/raw-mini/config/twist_mux_locks.yaml

@@ -0,0 +1,26 @@
+# Locks to stop the twist inputs.
+# 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_navigation
+  topic   : twist_mux/locks/pause_navigation
+  timeout : 0.0
+  priority: 21
+-
+  name    : pause_teleop 
+  topic   : twist_mux/locks/pause_teleop
+  timeout : 0.0
+  priority: 101
+-
+  name    : pause_all 
+  topic   : twist_mux/locks/pause_all
+  timeout : 0.0
+  priority: 255

cob_hardware_config/robots/raw-mini/config/twist_mux_topics.yaml

@@ -0,0 +1,43 @@
+# Input topics handled/muxed.
+# 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    : collision_velocity_filter
+  topic   : twist_mux/command_safe
+  timeout : 0.25
+  priority: 10
+-
+  name    : navigation
+  topic   : twist_mux/command_navigation
+  timeout : 0.25
+  priority: 20
+-
+  name    : syncmm # cob_twist_controller
+  topic   : twist_mux/command_syncmm
+  timeout : 0.5
+  priority: 60
+-
+  name    : script_server
+  topic   : twist_mux/command_script_server
+  timeout : 0.5
+  priority: 70
+-
+  name    : teleop_keyboard
+  topic   : twist_mux/command_teleop_keyboard
+  timeout : 0.5
+  priority: 80
+-
+  name    : teleop_android
+  topic   : twist_mux/command_teleop_android
+  timeout : 0.5
+  priority: 90
+-
+  name    : teleop_joystick
+  topic   : twist_mux/command_teleop_joy
+  timeout : 0.25
+  priority: 100

cob_hardware_config/robots/raw-mini/config/velocity_smoother.yaml

@@ -0,0 +1,23 @@
+# Example configuration:
+# - velocity limits are around a 10% above the physical limits
+# - acceleration limits are just low enough to avoid jerking
+
+# Mandatory parameters
+speed_lim_vx: 1.0
+speed_lim_vy: 1.0
+speed_lim_w: 1.0
+
+accel_lim_vx: 0.5
+accel_lim_vy: 0.5
+accel_lim_w: 0.5
+
+# Optional parameters
+frequency: 100.0
+decel_factor: 1.5       #used if zero velocity is received or goal velocity is far away from current velocity
+decel_factor_safe: 3.5  #used if no velocity commands received (eg. no deadman on joystick)
+
+# Robot velocity feedback type:
+#  0 - none
+#  1 - odometry
+#  2 - end robot commands
+robot_feedback: 1