Add Teleop handler and node

README.md

@@ -277,6 +277,52 @@ sudo ENABLE_MANIPULATOR=true MANIPULATOR_GUI="rviz" TELEOP_CONTROLLER="xbox" doc
 
 ```
 
+### Simultaneous teleoperation
+
+A custom node is included the `isar_turtlebot` ROS package that allows for teleoperation of the turtlebot base and manipulator simultaneously. The manipulator responds directly to input and can be used without the motion planner in RViz.
+
+The arm is controlled by moving a reference point for the end effector based on control inputs, and then using the moveit package for performing inverse kinematics at every change. The joint states are sent directly to the control system on the turtlebot.
+
+Run the node with the following command:
+
+```bash
+roslaunch isar_turtlebot turtlebot_manipulator_teleoperation.launch teleop_controller:="xbox"
+
+```
+
+Add the argument below to activate the node without waiting for a mission step from ISAR:
+
+```bash
+roslaunch isar_turtlebot turtlebot_manipulator_teleoperation.launch teleop_controller:="xbox" teleop_activate_from_start:="True"
+
+```
+
+The control mappings are as follows:
+
+#### Keyboard:
+* `WASD`: Wheel velocities
+* `IJKL`: Move reference of arm end effector in the horizontal plane
+* `UO`: Move reference of arm end effector along the vertical axis
+* `,.`: Close and open the gripper
+* `SHIFT`: Hold to double speed
+* `h`: Reset to home position
+* `p`: Pick up object
+* `v`: Place down object
+* `t`: Throw held object
+* `ENTER`: Deactivate teleoperation
+
+#### Xbox one controller:
+* `Left-stick`: Wheel velocities
+* `Right-stick` Move reference of arm end effector in the horizontal plane
+* `Triggers`: Move reference of arm end effector along the vertical axis
+* `Bumpers`: Close and open the gripper
+* `A`: Hold to double speed
+* `Y`: Reset to home position
+* `B`: Pick up object
+* `X`: Place down object
+* `View`: Throw held object
+* `Menu`: Deactivate teleoperation
+
 ## Development
 
 For local development, please fork the repository. Then, clone and install in the repository root folder:

ros_packages/isar_turtlebot/CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
   rospy
   turtlebot3_bringup
   turtlebot3_navigation
+  std_msgs
 )
 
 ## System dependencies are found with CMake's conventions
@@ -206,3 +207,8 @@ include_directories(
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
+
+
+catkin_install_python(PROGRAMS src/teleop.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

ros_packages/isar_turtlebot/launch/turtlebot_manipulator_teleoperation.launch

@@ -0,0 +1,59 @@
+<launch>
+    <arg name="teleop_controller" default="disabled"/>
+    <arg name="dev" default="/dev/input/js0" />
+    <arg name="map_file" default="$(env HOME)/map.yaml"/>
+    <arg name="move_forward_only" default="false"/>
+    <arg name="open_rviz" default="true"/>
+    <arg name="initial_pose_x" default="0.0"/>
+    <arg name="initial_pose_y" default="0.0"/>
+    <arg name="initial_pose_a" default="0.0"/>
+    <arg name="use_navigation" default="True"/>
+    <arg name="teleop_activate_from_start" default="False"/>
+
+    <include file="$(find turtlebot3_manipulation_bringup)/launch/turtlebot3_manipulation_bringup.launch" />
+    <include file="$(find isar_turtlebot_moveit_config)/launch/move_group.launch" />
+
+    <group if="$(arg use_navigation)">
+        <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" />
+
+        <node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/>
+
+        <node pkg="laser_filters" type="scan_to_scan_filter_chain" name="laser_filter" output="screen" >
+            <rosparam command="load" file="$(find turtlebot3_manipulation_slam)/config/scan_data_filter.yaml" />
+        </node>  
+
+        <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
+            <param name="publish_frequency" type="double" value="50.0" />
+            <param name="tf_prefix" value=""/>
+        </node>
+
+        <include file="$(find turtlebot3_navigation)/launch/amcl.launch">
+            <arg name="initial_pose_x" value="$(arg initial_pose_x)"/>
+            <arg name="initial_pose_y" value="$(arg initial_pose_y)"/>
+            <arg name="initial_pose_a" value="$(arg initial_pose_a)"/>
+        </include>
+
+        <include file="$(find turtlebot3_navigation)/launch/move_base.launch">
+            <arg name="move_forward_only" value="$(arg move_forward_only)"/>
+        </include>
+
+        <group if="$(arg open_rviz)"> 
+            <node pkg="rviz" type="rviz" name="rviz" required="true"
+                args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navigation.rviz"/>
+        </group>
+    </group>
+
+    <group if="$(eval arg('teleop_controller')=='xbox')">
+        <node pkg="joy" type="joy_node" name="joy">
+            <param name="dev" value="$(arg dev)" />
+            <param name="deadzone" value="0.2" />
+            <param name="autorepeat_rate" value="40" />
+            <param name="coalesce_interval" value="0.025" />
+        </node>
+    </group>
+
+    <node pkg="isar_turtlebot" type="teleop.py" name="teleop">
+        <param name="teleop_activate_from_start" value="$(arg teleop_activate_from_start)" />
+    </node>
+
+</launch>