Add elevator operation package

elevator_operation/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(elevator_operation)
+
+find_package(catkin REQUIRED genmsg actionlib_msgs)
+
+add_message_files(DIRECTORY msg FILES DoorState.msg)
+
+add_service_files(DIRECTORY srv FILES LookAtTarget.srv)
+
+add_action_files(DIRECTORY action FILES MoveFloorWithElevator.action)
+
+generate_messages(DEPENDENCIES actionlib_msgs)
+
+catkin_package()

elevator_operation/README.md

@@ -0,0 +1,43 @@
+# elevator_operation
+
+This package provides elevator operation and recognition interface to robot.
+
+## Setup
+
+Basically, dependencies are installed with rosdep.
+
+If you want to use spinal as altitude input, please install `spinal` package from jsk_aerial_robot. https://github.com/jsk-ros-pkg/jsk_aerial_robot/tree/master/aerial_robot_nerve/spinal
+
+## How to run
+
+Connect spinal or M5Stack_ROS ENVIII to PC and start `elevator_state_publisher.launch`
+
+```bash
+roslaunch elevator_operation elevator_state_publisher.launch device_type:=<spinal or enviii> device_name:=<port name> robot_type:=<robot_type, default is fetch>
+```
+
+And run elevator_operation node
+
+```bash
+roslaunch elevator_operation elevator_operation.launch input_topic_points:=<point cloud topic> robot_type:=<robot type> launch_switchbot_client:=<launch switchbot client if necessary> switchbot_token_yaml:=<path to switchbot token yaml>
+```
+
+Then call action.
+
+```bash
+rostopic pub -1 TODO
+```
+
+## How to run on Fetch1075
+
+```bash
+roslaunch elevator_operation elevator_state_publisher.launch device_type:=fetch1075_enviii robot_type:=fetch
+```
+
+```bash
+roslaunch elevator_operation elevator_operation.launch input_topic_points:=/head_camera/depth_registered/points robot_type:=fetch
+```
+
+## How to configure
+
+To configure door frames, please edit [`doors_frame_publisher.launch`](./launch/doors_frame_publisher.launch).

elevator_operation/action/MoveFloorWithElevator.action

@@ -0,0 +1,4 @@
+string target_floor_name
+---
+bool success
+---

elevator_operation/config/config.yaml

@@ -0,0 +1,66 @@
+- floor: 8
+  altitude: 29.6
+  map_name: '/eng2/8f'
+- floor: 7
+  altitude: 25.8
+  map_name: '/eng2/7f'
+  floor_name: 'eng2/7f'
+  door_frame_id: '/eng2/7f/elevator_door'
+  door_dimensions: [1.0, 0.5, 1.0]
+  outside_pose:
+    frame_id: 'map'
+    position: [2.631, -30.344, -0.000]
+    orientation: [0.000, 0.000, -0.861, 0.509]
+  inside_pose:
+    frame_id: 'map'
+    position: [1.634, -33.990, -0.000]
+    orientation: [-0.000, -0.000, 0.476, 0.879]
+  buttons:
+    down: '/eng2/7f/elevator_bot_down'
+- floor: 6
+  altitude: 22.0
+  map_name: '/eng2/6f'
+- floor: 5
+  altitude: 16.8
+  map_name: '/eng2/5f'
+- floor: 4
+  altitude: 13.1
+  map_name: '/eng2/4f'
+- floor: 3
+  altitude: 9.1
+  map_name: '/eng2/3f'
+  floor_name: 'eng2/3f'
+  door_frame_id: '/eng2/3f/elevator_door'
+  door_dimensions: [1.0, 0.5, 1.0]
+  outside_pose:
+    frame_id: 'map'
+    position: [2.631, -30.344, -0.000]
+    orientation: [0.000, 0.000, -0.861, 0.509]
+  inside_pose:
+    frame_id: 'map'
+    position: [1.634, -33.990, -0.000]
+    orientation: [-0.000, -0.000, 0.476, 0.879]
+  buttons:
+    up: '/eng2/3f/elevator_bot_up'
+- floor: 2
+  altitude: 5.0
+  map_name: '/eng2/2f'
+- floor: 1
+  altitude: 0
+  map_name: '/eng2/1f'
+  floor_name: 'eng2/1f'
+  door_frame_id: '/eng2/1f/elevator_door'
+  door_dimensions: [1.0, 0.5, 1.0]
+  outside_pose:
+    frame_id: 'map'
+    position: [2.631, -30.344, -0.000]
+    orientation: [0.000, 0.000, -0.861, 0.509]
+  inside_pose:
+    frame_id: 'map'
+    position: [1.634, -33.990, -0.000]
+    orientation: [-0.000, -0.000, 0.476, 0.879]
+  buttons:
+    up: '/eng2/1f/elevator_bot_up'
+- floor: -1
+  altitude: -4
+  map_name: '/eng2/1f'

elevator_operation/euslisp/look-at-elevator.l

@@ -0,0 +1,36 @@
+#!/usr/bin/env roseus
+
+(ros::roseus-add-srvs "elevator_operation")
+(ros::roseus-add-msgs "move_base_msgs")
+
+(load "package://elevator_operation/euslisp/utils.l")
+
+
+(defun handler-start-look-at-target (req)
+  (let (success
+        (res (instance elevator_operation::LookAtTargetResponse :init)))
+    (setq success (look-at-target-frame (send req :frame_id) :robot-type *robot-type*))
+    (send res :success success)
+    res))
+
+(defun main()
+  (ros::roseus "look_at_elevator_server")
+  (setq *robot-type* (ros::get-param "~robot_type" ""))
+  (cond
+    ((equal *robot-type* "fetch")
+      (load "package://fetcheus/fetch-interface.l")
+      (load "package://jsk_fetch_startup/euslisp/navigation-utils.l")
+      (fetch-init)
+      (setq *robot* *fetch*))
+    (t
+      (setq *move-base-action-client* (instance ros::simple-action-client :init "/move_base" move_base_msgs::MoveBaseAction))
+      (send *move-base-action-client* :wait-for-server)
+      )
+    )
+  (setq *tfl* (instance ros::transform-listener :init))
+  (ros::advertise-service "~look_at_target" elevator_operation::LookAtTarget #'handler-start-look-at-target)
+  (ros::ros-info "initialized")
+  (ros::spin)
+  )
+
+(main)

elevator_operation/euslisp/utils.l

@@ -0,0 +1,98 @@
+(ros::roseus-add-msgs "move_base_msgs")
+(ros::load-ros-manifest "move_base_msgs")
+(ros::roseus-add-srvs "elevator_operation")
+(ros::load-ros-manifest "elevator_operation")
+
+(defun update-model (&key (robot-type "fetch"))
+  (if (equal robot-type "fetch")
+    (send *robot* :angle-vector (send *ri* :state :potentio-vector)))
+  )
+
+(defun look-at-target-with-head (coords-base-to-target &key (robot-type "fetch"))
+  (if (equal robot-type "fetch")
+    (progn
+      (send *robot* :look-at-target coords-base-to-target)
+      (send *ri* :angle-vector-raw (send *robot* :angle-vector) 500)))
+  )
+
+(defun send-move-base-goal (x y z qx qy qz qw frame-id &key (wait t) (timeout 60))
+  (if (not (boundp `*move-base-action-client*))
+    (setq *move-base-action-client* (instance ros::simple-action-client :init "/move_base" move_base_msgs::MoveBaseAction))
+    (ros::ros-warn "create move-base-action-client")
+    (ros::spin-once)
+    (send *move-base-action-client* :wait-for-server)
+    )
+  (let ((time-stamp (ros::time-now))
+        (goal (instance move_base_msgs::MoveBaseActionGoal :init)))
+    (ros::ros-warn "got a goal x: ~A, y: ~A, z: ~A, qx: ~A, qy: ~A, qz: ~A, qw: ~A on frame ~A"
+                   x y z qx qy qz qw frame-id)
+    (send goal :header :stamp time-stamp)
+    (send goal :goal :target_pose :header :frame_id frame-id)
+    (send goal :goal :target_pose :pose :position :x x)
+    (send goal :goal :target_pose :pose :position :y y)
+    (send goal :goal :target_pose :pose :position :z z)
+    (send goal :goal :target_pose :pose :orientation :x qx)
+    (send goal :goal :target_pose :pose :orientation :y qy)
+    (send goal :goal :target_pose :pose :orientation :z qz)
+    (send goal :goal :target_pose :pose :orientation :w qw)
+    (send *move-base-action-client* :send-goal goal)
+    (ros::spin-once)
+    (ros::ros-warn "send a goal")
+    (if wait
+      (progn
+        (ros::ros-warn "waiting for a result of a goal")
+        (send *move-base-action-client* :wait-for-result :timeout timeout)
+        )))
+  )
+
+(defun rotate-base (theta &key (robot-type "fetch"))
+  (ros::ros-warn "rotating base ~A radians" theta)
+  (cond
+    ((equal robot-type "fetch")
+      (send *ri* :go-pos 0 0 (rad2deg theta)))
+    (t
+      (send-move-base-goal 0 0 0 0 0 (sin (/ theta 2)) (cos (/ theta 2)) "base_link" :wait t))
+    ))
+
+(defun look-at-target-frame (target-frame-id &key (robot-type "fetch"))
+  (ros::ros-warn "target-frame-id: ~A" target-frame-id)
+  (let ((coords-base-to-target (send *tfl* :lookup-transform "base_link" target-frame-id (ros::time))))
+    (update-model :robot-type robot-type)
+    (ros::ros-info "coords-base-to-target: ~A" coords-base-to-target)
+    (if (not coords-base-to-target)
+      (return-from look-at-target nil)
+      )
+    (let* ((target-x (elt (send coords-base-to-target :pos) 0))
+           (target-y (elt (send coords-base-to-target :pos) 1)))
+      (rotate-base (atan target-y target-x) :robot-type robot-type)
+      (update-model :robot-type robot-type)
+      (setq coords-base-to-target (send *tfl* :lookup-transform "base_link" target-frame-id (ros::time)))
+      (if (not coords-base-to-target)
+        (return-from look-at-target nil)
+        )
+      (look-at-target-with-head coords-base-to-target :robot-type robot-type)
+      )
+    t))
+
+(defun move-elevator (target-floor-name)
+  (if (not (boundp `*move-elevator-action-client*))
+    (progn
+        (setq *move-elevator-action-client*
+              (instance ros::simple-action-client :init
+                        "/elevator_operation_server/move_floor_with_elevator"
+                        elevator_operation::MoveFloorWithElevatorAction))
+        (send *move-elevator-action-client* :wait-for-server)))
+  (let (result (goal (instance elevator_operation::MoveFloorWithElevatorGoal :init)))
+    (send goal :target_floor_name target-floor-name)
+    (send *move-elevator-action-client* :send-goal goal)
+    (send *move-elevator-action-client* :wait-for-result)
+    (setq result (send *move-elevator-action-client* :get-result))
+    (send result :success))
+  )
+
+(defun call-look-at-target (topic-name frame-id)
+  (let (res (req (instance elevator_operation::LookAtTargetRequest :init)))
+    (send req :frame_id frame-id)
+    (setq res (ros::service-call topic-name req t))
+    (send res :success)
+    ))

elevator_operation/launch/doors_frame_publisher.launch

@@ -0,0 +1,33 @@
+<launch>
+    <!-- door frames definitions -->
+    <node
+        pkg="tf2_ros"
+        type="static_transform_publisher"
+        name="elevator_1f_door_frame_publisher"
+        args="2.3 -33 1 1.57 0 0 eng2/1f eng2/1f/elevator_door"
+        />
+    <node
+        pkg="tf2_ros"
+        type="static_transform_publisher"
+        name="elevator_3f_door_frame_publisher"
+        args="2 -32.5 1 1.57 0 0 eng2/3f eng2/3f/elevator_door"
+        />
+    <node
+        pkg="tf2_ros"
+        type="static_transform_publisher"
+        name="elevator_7f_door_frame_publisher"
+        args="2 -32.8 1 1.57 0 0 eng2/7f eng2/7f/elevator_door"
+        />
+    <node
+        pkg="tf2_ros"
+        type="static_transform_publisher"
+        name="elevator_7f_inside_panel_frame_publisher"
+        args="2.9 -33.85 1 0 0 0 eng2/7f eng2/7f/elevator_inside_panel"
+        />
+    <node
+        pkg="tf2_ros"
+        type="static_transform_publisher"
+        name="static_73b2_frame_publisher"
+        args="0 -0.6 1 0 0 0 eng2/7f/73B2 eng2/7f/73B2/door"
+        />
+</launch>

elevator_operation/launch/drivers/enviii_driver.launch

@@ -0,0 +1,19 @@
+<launch>
+    <arg name="device" default="/dev/rfcomm1" />
+    <arg name="output_altitude_topic" default="/altitude" />
+
+    <!-- m5stack (barometer) -->
+    <node pkg="rosserial_python" type="serial_node.py" name="m5stack_ros_env3_driver" output="screen">
+        <rosparam subst_value="true">
+            baud: 115200
+            port: $(arg device)
+        </rosparam>
+    </node>
+
+    <!-- calculate altitude from air pressure -->
+    <node pkg="elevator_operation" type="altitude_calculator.py" name="altitude_calculator" output="screen">
+        <remap from="~input_pressure" to="/pressure" />
+        <remap from="~input_temperature" to="/temperature" />
+        <remap from="~output" to="$(arg output_altitude_topic)" />
+    </node>
+</launch>

elevator_operation/launch/drivers/fetch1075_enviii_driver.launch

@@ -0,0 +1,10 @@
+<launch>
+    <arg name="output_altitude_topic" default="/altitude" />
+
+    <!-- calculate altitude from air pressure -->
+    <node pkg="elevator_operation" type="altitude_calculator.py" name="altitude_calculator" output="screen">
+        <remap from="~input_pressure" to="/enviii/pressure" />
+        <remap from="~input_temperature" to="/enviii/temperature" />
+        <remap from="~output" to="$(arg output_altitude_topic)" />
+    </node>
+</launch>

elevator_operation/launch/drivers/spinal_driver.launch

@@ -0,0 +1,22 @@
+<launch>
+    <arg name="device" default="/dev/ttyUSB0" />
+    <arg name="output_altitude_topic" default="/altitude" />
+
+    <!-- spinal (Imu, barometer and so on) -->
+    <group ns="/spinal">
+        <node pkg="rosserial_python" type="serial_node.py" name="fetch_spinal" output="log">
+            <rosparam subst_value="true">
+                baud: 921600
+                port: $(arg device_spinal)
+            </rosparam>
+        </node>
+    </group>
+
+    <!-- Extract altitude field as std_msgs/Float32 from barometer message -->
+    <node
+        pkg="topic_tools"
+        type="transform"
+        name="barometer_to_altitude"
+        args="/spinal/baro $(arg output_altitude_topic) std_msgs/Float32 'std_msgs.msg.Float32(data=m.altitude)' --import std_msgs"
+        />
+</launch>

elevator_operation/launch/elevator_door_detector.launch

@@ -0,0 +1,33 @@
+<launch>
+    <arg name="input_topic_points"/>
+    <arg name="elevator_door_frame_id"/>
+    <arg name="output_topic_door_state" default="/elevator_door_detector/door_state"/>
+    <arg name="door_position_offset" default="[0, 0, 0]"/>
+    <arg name="door_rotation_offset" default="[0, 0, 0]"/>
+    <arg name="door_dimension_x" default="1.0"/>
+    <arg name="door_dimension_y" default="1.0"/>
+    <arg name="door_dimension_z" default="0.8"/>
+    <arg name="prefix" default=""/>
+
+    <node pkg="nodelet" type="nodelet" name="$(arg prefix)elevator_detection_door_detector_attention_clipper" args="standalone jsk_pcl/AttentionClipper">
+        <remap from="~input/points" to="$(arg input_topic_points)" />
+        <rosparam subst_value="true">
+            initial_pos: $(arg door_position_offset)
+            initial_rot: $(arg door_rotation_offset)
+            dimension_x: $(arg door_dimension_x)
+            dimension_y: $(arg door_dimension_y)
+            dimension_z: $(arg door_dimension_z)
+            frame_id: $(arg elevator_door_frame_id)
+        </rosparam>
+    </node>
+    <node pkg="jsk_pcl_ros" type="extract_indices" name="$(arg prefix)elevator_detection_door_detector_extract_indices">
+        <remap from="~input" to="$(arg input_topic_points)" />
+        <remap from="~indices" to="/$(arg prefix)elevator_detection_door_detector_attention_clipper/output/point_indices" />
+        <!-- <remap from="~output" to="" /> -->
+    </node>
+    <node pkg="elevator_operation" type="elevator_door_opening_checker.py" name="$(arg prefix)elevator_door_opening_checker">
+        <remap from="~input" to="/$(arg prefix)elevator_detection_door_detector_extract_indices/output" />
+        <!-- <remap from="~output" to="" /> -->
+        <remap from="~door_state" to="$(arg output_topic_door_state)" />
+    </node>
+</launch>