Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

new control #734

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

new control #734

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
37a028a
new control
Jul 3, 2024
df0a724
Update README.md
NadiaKhodaei Jul 15, 2024
19618cd
Update README.md
NadiaKhodaei Jul 15, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
229 changes: 24 additions & 205 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,216 +1,35 @@
RotorS
===============
# Keyboard Teleoperation for Position and Yaw Control

RotorS is a MAV gazebo simulator.
It provides some multirotor models such as the [AscTec Hummingbird](http://www.asctec.de/en/uav-uas-drone-products/asctec-hummingbird/), the [AscTec Pelican](http://www.asctec.de/en/uav-uas-drone-products/asctec-pelican/), or the [AscTec Firefly](http://www.asctec.de/en/uav-uas-drone-products/asctec-firefly/), but the simulator is not limited for the use with these multicopters.
This Python script, `key_pose.py`, allows for teleoperation of a drone or robot using keyboard inputs. The script reads from the keyboard and publishes `PoseStamped` messages to control the position and yaw of the robot in a ROS (Robot Operating System) environment.

There are simulated sensors coming with the simulator such as an IMU, a generic odometry sensor, and the [VI-Sensor](http://wiki.ros.org/vi_sensor), which can be mounted on the multirotor.
## Features

This package also contains some example controllers, basic worlds, a joystick interface, and example launch files.
- **Move forward/backward** in the x direction using the up and down arrow keys.
- **Move left/right** in the y direction using `k` and `m` keys.
- **Move up/down** in the z direction using `i` and `,` keys.
- **Rotate left/right** (yaw) in the z direction using the left and right arrow keys.
- **Adjust speed** using `+` and `-` keys.
- **Reset position** to the default using the `s` key.

Below we provide the instructions necessary for getting started. See RotorS' wiki for more instructions and examples (https:/ethz-asl/rotors_simulator/wiki).
## Key Bindings
Reading from the keyboard and Publishing to PoseStamped!

If you are using this simulator within the research for your publication, please cite:
```bibtex
@Inbook{Furrer2016,
author="Furrer, Fadri
and Burri, Michael
and Achtelik, Markus
and Siegwart, Roland",
editor="Koubaa, Anis",
chapter="RotorS---A Modular Gazebo MAV Simulator Framework",
title="Robot Operating System (ROS): The Complete Reference (Volume 1)",
year="2016",
publisher="Springer International Publishing",
address="Cham",
pages="595--625",
isbn="978-3-319-26054-9",
doi="10.1007/978-3-319-26054-9_23",
url="http://dx.doi.org/10.1007/978-3-319-26054-9_23"
}
```
Installation Instructions - Ubuntu 16.04 with ROS Kinetic
---------------------------------------------------------
1. Install and initialize ROS kinetic desktop full, additional ROS packages, catkin-tools, and wstool:
Moving around:
Arrow Keys:
↑ ↓ ← →

```
$ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu `lsb_release -sc` main" > /etc/apt/sources.list.d/ros-latest.list'
$ wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
$ sudo apt-get update
$ sudo apt-get install ros-kinetic-desktop-full ros-kinetic-joy ros-kinetic-octomap-ros ros-kinetic-mavlink python-wstool python-catkin-tools protobuf-compiler libgoogle-glog-dev ros-kinetic-control-toolbox ros-kinetic-mavros
$ sudo rosdep init
$ rosdep update
$ source /opt/ros/kinetic/setup.bash
```
2. If you don't have ROS workspace yet you can do so by
↑/↓ : move forward/backward in x direction
←/→ : rotate left/right (yaw) in z direction
i/, : move up/down in z direction
k/m : move left/right in y direction
anything else : stop

```
$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/src
$ catkin_init_workspace # initialize your catkin workspace
$ wstool init
$ wget https://raw.githubusercontent.com/ethz-asl/rotors_simulator/master/rotors_hil.rosinstall
$ wstool merge rotors_hil.rosinstall
$ wstool update
```
Adjust speed:

> **Note** On OS X you need to install yaml-cpp using Homebrew `brew install yaml-cpp`.
: increase step

3. Build your workspace with `python_catkin_tools` (therefore you need `python_catkin_tools`)
: decrease step

```
$ cd ~/catkin_ws/
$ catkin build
```
s : reset position to default

4. Add sourcing to your `.bashrc` file

```
$ echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
$ source ~/.bashrc
```

Installation Instructions - Ubuntu 14.04 with ROS Indigo
--------------------------------------------------------

1. Install and initialize ROS indigo desktop full, additional ROS packages, catkin-tools, and wstool:

```
$ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu `lsb_release -sc` main" > /etc/apt/sources.list.d/ros-latest.list'
$ wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
$ sudo apt-get update
$ sudo apt-get install ros-indigo-desktop-full ros-indigo-joy ros-indigo-octomap-ros python-wstool python-catkin-tools protobuf-compiler libgoogle-glog-dev
$ sudo rosdep init
$ rosdep update
$ source /opt/ros/indigo/setup.bash
```
2. If you don't have ROS workspace yet you can do so by

```
$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/src
$ catkin_init_workspace # initialize your catkin workspace
$ wstool init
```
> **Note** for setups with multiple workspaces please refer to the official documentation at http://docs.ros.org/independent/api/rosinstall/html/ by replacing `rosws` by `wstool`.
3. Get the simulator and additional dependencies

```
$ cd ~/catkin_ws/src
$ git clone [email protected]:ethz-asl/rotors_simulator.git
$ git clone [email protected]:ethz-asl/mav_comm.git
```
> **Note** On OS X you need to install yaml-cpp using Homebrew `brew install yaml-cpp`.

> **Note** if you want to use `wstool` you can replace the above commands with
```
wstool set --git local_repo_name [email protected]:organization/repo_name.git
```
> **Note** if you want to build and use the `gazebo_mavlink_interface` plugin you have to get MAVROS as an additional dependency from link below. Follow the installation instructions provided there and build all of its packages prior to building the rest of your workspace.
```
https:/mavlink/mavros
```
4. Build your workspace with `python_catkin_tools` (therefore you need `python_catkin_tools`)

```
$ cd ~/catkin_ws/
$ catkin init # If you haven't done this before.
$ catkin build
```
> **Note** if you are getting errors related to "future" package, you may need python future:
```
sudo apt-get install python-pip
pip install --upgrade pip
pip install future
```

5. Add sourcing to your `.bashrc` file

```
$ echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
$ source ~/.bashrc
```

Basic Usage
-----------

Launch the simulator with a hex-rotor helicopter model, in our case, the AscTec Firefly in a basic world.

```
$ roslaunch rotors_gazebo mav_hovering_example.launch mav_name:=firefly world_name:=basic
```

> **Note** The first run of gazebo might take considerably long, as it will download some models from an online database. Should you receive a timeout error, try running gazebo by itself (e.g. roslaunch gazebo_ros empty_world.launch ) so it has sufficient time to actually download all of the models.

The simulator starts by default in paused mode. To start it you can either
- use the Gazebo GUI and press the play button
- or you can send the following service call.

```
$ rosservice call gazebo/unpause_physics
```

There are some basic launch files where you can load the different multicopters with additional sensors. They can all be found in `~/catkin_ws/src/rotors_simulator/rotors_gazebo/launch`.

The `world_name` argument looks for a .world file with a corresponding name in `~/catkin_ws/src/rotors_simulator/rotors_gazebo/worlds`. By default, all launch files, with the exception of those that have the world name explicitly included in the file name, use the empty world described in `basic.world`.

### Getting the multicopter to fly

To let the multicopter fly you need to generate thrust with the rotors, this is achieved by sending commands to the multicopter, which make the rotors spin.
There are currently a few ways to send commands to the multicopter, we will show one of them here.
The rest is documented [here](../../wiki) in our Wiki.
We will here also show how to write a stabilizing controller and how you can control the multicopter with a joystick.

#### Send direct motor commands

We will for now just send some constant motor velocities to the multicopter.

```
$ rostopic pub /firefly/command/motor_speed mav_msgs/Actuators '{angular_velocities: [100, 100, 100, 100, 100, 100]}'
```

> **Note** The size of the `motor_speed` array should be equal to the number of motors you have in your model of choice (e.g. 6 in the Firefly model).

You should see (if you unpaused the simulator and you have a multicopter in it), that the rotors start spinning. The thrust generated by these motor velocities is not enough though to let the multicopter take off.
> You can play with the numbers and will realize that the Firefly will take off with motor speeds of about 545 on each rotor. The multicopter is unstable though, since there is no controller running, if you just set the motor speeds.


#### Let the helicopter hover with ground truth odometry

You can let the helicopter hover with ground truth odometry (perfect state estimation), by launching:

```
$ roslaunch rotors_gazebo mav_hovering_example.launch mav_name:=firefly world_name:=basic
```

#### Create an attitude controller

**TODO(ff):** `Write something here.`

#### Usage with a joystick

Connect a USB joystick to your computer and launch the simulation alongside ROS joystick driver and the RotorS joystick node:
```
$ roslaunch rotors_gazebo mav_with_joy.launch mav_name:=firefly world_name:=basic
```

Depending on the type of joystick and the personal preference for operation, you can assign the axis number using the `axis_<roll/pitch/thrust>_` parameter and the axis direction using the `axis_direction_<roll/pitch/thrust>` parameter.

#### Usage with a keyboard

First, perform a one-time setup of virtual keyboard joystick as described here: https:/ethz-asl/rotors_simulator/wiki/Setup-virtual-keyboard-joystick.

Launch the simulation with the keyboard interface using the following launch file:
```
$ roslaunch rotors_gazebo mav_with_keyboard.launch mav_name:=firefly world_name:=basic
```

If everything was setup correctly, an additional GUI should appear with bars indicating the current throttle, roll, pitch, and yaw inputs. While this window is active, the Arrows and W, A, S, D keys will generate virtual joystick inputs, which can then be processed by the RotorS joystick node in the same way as real joystick commands.

Gazebo Version
--------------

At a minimum, Gazebo `v2.x` is required (which is installed by default with ROS Indigo). However, it is **recommended to install at least Gazebo `v5.x`** for full functionlity, as there are the following limitations:

1. `iris.sdf` can only be generated with Gazebo >= `v3.0`, as it requires use of the `gz sdf ...` tool. If this requirement is not met, you will not be able to use the Iris MAV in any of the simulations.
2. The Gazebo plugins `GazeboGeotaggedImagesPlugin`, `LidarPlugin` and the `LiftDragPlugin` all require Gazebo >= `v5.0`, and will not be built if this requirement is not met.
CTRL-C to quit
140 changes: 140 additions & 0 deletions rotors_joy_interface/scripts/key_pose.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,140 @@
#!/usr/bin/env python3
# Author: Nadia Khodaei
import rospy
from geometry_msgs.msg import PoseStamped
import sys, select, termios, tty
import tf.transformations

msg = """
Reading from the keyboard and Publishing to PoseStamped!
---------------------------
Moving around:
Arrow Keys:
↑ ↓ ← →

↑/↓ : move forward/backward in x direction
←/→ : rotate left/right (yaw) in z direction
i/, : move up/down in z direction
k/m : move left/right in y direction
anything else : stop

Adjust speed:
+ : increase step
- : decrease step

s : reset position to default

CTRL-C to quit
"""

# Arrow key codes
arrowBindings = {
'\x1b[A': (1, 0, 0, 0), # up arrow
'\x1b[B': (-1, 0, 0, 0), # down arrow
'\x1b[D': (0, 0, 0, 1), # left arrow (rotate left)
'\x1b[C': (0, 0, 0, -1), # right arrow (rotate right)
}

# Normal key codes
moveBindings = {
'i': (0, 0, 1, 0),
',': (0, 0, -1, 0),
'k': (0, 1, 0, 0),
'm': (0, -1, 0, 0),
's': 'reset',
'+': 'increase_step',
'-': 'decrease_step'
}

def getKey():
tty.setraw(sys.stdin.fileno())
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
if rlist:
key = sys.stdin.read(1)
if key == '\x1b':
key += sys.stdin.read(2)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
else:
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return ''

if __name__ == "__main__":
settings = termios.tcgetattr(sys.stdin)

rospy.init_node('keyboard_teleop_position')
pub = rospy.Publisher('/firefly/command/pose', PoseStamped, queue_size=10)

# Default position values
default_x = 0
default_y = 0
default_z = 1.0
default_yaw = 0
x = default_x
y = default_y
z = default_z
yaw = default_yaw
step = 0.1 # Initial step value, now a float

try:
print(msg)
while not rospy.is_shutdown():
key = getKey()
if key in arrowBindings.keys():
x += arrowBindings[key][0] * step
y += arrowBindings[key][1] * step
z += arrowBindings[key][2] * step
yaw += arrowBindings[key][3] * step
elif key in moveBindings.keys():
if moveBindings[key] == 'reset':
x = default_x
y = default_y
z = default_z
yaw = default_yaw
print("Position reset to default")
elif moveBindings[key] == 'increase_step':
step = min(step + 0.1, 10) # Increase step, max is 10
print(f"Step increased to: {step}")
elif moveBindings[key] == 'decrease_step':
step = max(step - 0.1, 0.1) # Decrease step, min is 0.1
print(f"Step decreased to: {step}")
else:
x += moveBindings[key][0] * step
y += moveBindings[key][1] * step
z += moveBindings[key][2] * step
yaw += moveBindings[key][3] * step
else:
if key == '\x03':
break

pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = 'world'
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z

# Convert yaw to quaternion
q = tf.transformations.quaternion_from_euler(0, 0, yaw)
pose.pose.orientation.x = q[0]
pose.pose.orientation.y = q[1]
pose.pose.orientation.z = q[2]
pose.pose.orientation.w = q[3]

pub.publish(pose)

except Exception as e:
print(e)

finally:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = 'world'
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = 1.0
pose.pose.orientation.w = 1.0
pub.publish(pose)

termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)