Skip to content

Commit

Permalink
Merge pull request #107 from honeybee-robotics-forks/indigo-devel
Browse files Browse the repository at this point in the history 
Numerous outstanding PRs.
  • Loading branch information
@jbohren
jbohren authored Jul 3, 2017
2 parents 5aa8794 + feb962d commit 40275ec
Show file tree
Hide file tree
Showing 22 changed files with 660 additions and 225 deletions.
15 changes: 5 additions & 10 deletions joy/package.xml
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
<package>
<package format="2">
<name>joy</name>
<version>1.11.0</version>
<license>BSD</license>
Expand All @@ -23,15 +23,10 @@

<buildtool_depend>catkin</buildtool_depend>

<build_depend>roscpp</build_depend>
<build_depend>diagnostic_updater</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>joystick</build_depend>

<run_depend>roscpp</run_depend>
<run_depend>diagnostic_updater</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>joystick</run_depend>
<depend>roscpp</depend>
<depend>diagnostic_updater</depend>
<depend>sensor_msgs</depend>
<depend>joystick</depend>

<test_depend>rosbag</test_depend>

Expand Down
146 changes: 102 additions & 44 deletions joy/src/joy_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,10 +44,12 @@ class Joystick
{
private:
ros::NodeHandle nh_;
bool open_;
bool open_;
bool sticky_buttons_;
bool default_trig_val_;
std::string joy_dev_;
double deadzone_;
double autorepeat_rate_; // in Hz. 0 for no repeat.
double autorepeat_rate_; // in Hz. 0 for no repeat.
double coalesce_interval_; // Defaults to 100 Hz rate limit.
int event_count_;
int pub_count_;
Expand All @@ -74,6 +76,8 @@ class Joystick
stat.add("recent joystick event rate (Hz)", event_count_ / interval);
stat.add("recent publication rate (Hz)", pub_count_ / interval);
stat.add("subscribers", pub_.getNumSubscribers());
stat.add("default trig val", default_trig_val_);
stat.add("sticky buttons", sticky_buttons_);
event_count_ = 0;
pub_count_ = 0;
lastDiagTime_ = now;
Expand All @@ -96,6 +100,8 @@ class Joystick
nh_param.param<double>("deadzone", deadzone_, 0.05);
nh_param.param<double>("autorepeat_rate", autorepeat_rate_, 0);
nh_param.param<double>("coalesce_interval", coalesce_interval_, 0.001);
nh_param.param<bool>("default_trig_val",default_trig_val_,false);
nh_param.param<bool>("sticky_buttons", sticky_buttons_, false);

// Checks on parameters
if (autorepeat_rate_ > 1 / coalesce_interval_)
Expand Down Expand Up @@ -187,6 +193,9 @@ class Joystick
tv.tv_sec = 1;
tv.tv_usec = 0;
sensor_msgs::Joy joy_msg; // Here because we want to reset it on device close.
double val; //Temporary variable to hold event values
sensor_msgs::Joy last_published_joy_msg; // used for sticky buttons option
sensor_msgs::Joy sticky_buttons_joy_msg; // used for sticky buttons option
while (nh_.ok())
{
ros::spinOnce();
Expand All @@ -205,7 +214,7 @@ class Joystick
tv.tv_usec = 0;
//ROS_INFO("Select returned negative. %i", ros::isShuttingDown());
continue;
// break; // Joystick is probably closed. Not sure if this case is useful.
//break; // Joystick is probably closed. Not sure if this case is useful.
}

if (FD_ISSET(joy_fd, &set))
Expand All @@ -224,8 +233,13 @@ class Joystick
{
int old_size = joy_msg.buttons.size();
joy_msg.buttons.resize(event.number+1);
for(unsigned int i=old_size;i<joy_msg.buttons.size();i++)
last_published_joy_msg.buttons.resize(event.number+1);
sticky_buttons_joy_msg.buttons.resize(event.number+1);
for(unsigned int i=old_size;i<joy_msg.buttons.size();i++){
joy_msg.buttons[i] = 0.0;
last_published_joy_msg.buttons[i] = 0.0;
sticky_buttons_joy_msg.buttons[i] = 0.0;
}
}
joy_msg.buttons[event.number] = (event.value ? 1 : 0);
// For initial events, wait a bit before sending to try to catch
Expand All @@ -237,52 +251,96 @@ class Joystick
break;
case JS_EVENT_AXIS:
case JS_EVENT_AXIS | JS_EVENT_INIT:
val = event.value;
if(event.number >= joy_msg.axes.size())
{
int old_size = joy_msg.axes.size();
joy_msg.axes.resize(event.number+1);
for(unsigned int i=old_size;i<joy_msg.axes.size();i++)
last_published_joy_msg.axes.resize(event.number+1);
sticky_buttons_joy_msg.axes.resize(event.number+1);
for(unsigned int i=old_size;i<joy_msg.axes.size();i++){
joy_msg.axes[i] = 0.0;
last_published_joy_msg.axes[i] = 0.0;
sticky_buttons_joy_msg.axes[i] = 0.0;
}
}
if (!(event.type & JS_EVENT_INIT)) // Init event.value is wrong.
{
double val = event.value;
// Allows deadzone to be "smooth"
if (val > unscaled_deadzone)
val -= unscaled_deadzone;
else if (val < -unscaled_deadzone)
val += unscaled_deadzone;
else
val = 0;
joy_msg.axes[event.number] = val * scale;
}
// Will wait a bit before sending to try to combine events.
publish_soon = true;
break;
default:
ROS_WARN("joy_node: Unknown event type. Please file a ticket. time=%u, value=%d, type=%Xh, number=%d", event.time, event.value, event.type, event.number);
break;
}
}
else if (tv_set) // Assume that the timer has expired.
publish_now = true;

if (publish_now)
{
// Assume that all the JS_EVENT_INIT messages have arrived already.
// This should be the case as the kernel sends them along as soon as
// the device opens.
//ROS_INFO("Publish...");
pub_.publish(joy_msg);
publish_now = false;
tv_set = false;
publication_pending = false;
publish_soon = false;
pub_count_++;
if(default_trig_val_){
// Allows deadzone to be "smooth"
if (val > unscaled_deadzone)
val -= unscaled_deadzone;
else if (val < -unscaled_deadzone)
val += unscaled_deadzone;
else
val = 0;
joy_msg.axes[event.number] = val * scale;
// Will wait a bit before sending to try to combine events.
publish_soon = true;
break;
}
else
{
if (!(event.type & JS_EVENT_INIT))
{
val = event.value;
if(val > unscaled_deadzone)
val -= unscaled_deadzone;
else if(val < -unscaled_deadzone)
val += unscaled_deadzone;
else
val=0;
joy_msg.axes[event.number]= val * scale;
}

publish_soon = true;
break;
default:
ROS_WARN("joy_node: Unknown event type. Please file a ticket. time=%u, value=%d, type=%Xh, number=%d", event.time, event.value, event.type, event.number);
break;
}
}
}

// If an axis event occurred, start a timer to combine with other
// events.
else if (tv_set) // Assume that the timer has expired.
publish_now = true;

if (publish_now) {
// Assume that all the JS_EVENT_INIT messages have arrived already.
// This should be the case as the kernel sends them along as soon as
// the device opens.
//ROS_INFO("Publish...");
if (sticky_buttons_ == true) {
// cycle through buttons
for (int i = 0; i < joy_msg.buttons.size(); i++) {
// change button state only on transition from 0 to 1
if (joy_msg.buttons[i] == 1 && last_published_joy_msg.buttons[i] == 0) {
sticky_buttons_joy_msg.buttons[i] = sticky_buttons_joy_msg.buttons[i] ? 0 : 1;
} else {
// do not change the message sate
//sticky_buttons_joy_msg.buttons[i] = sticky_buttons_joy_msg.buttons[i] ? 0 : 1;
}
}
// update last published message
last_published_joy_msg = joy_msg;
// fill rest of sticky_buttons_joy_msg (time stamps, axes, etc)
sticky_buttons_joy_msg.header.stamp.nsec = joy_msg.header.stamp.nsec;
sticky_buttons_joy_msg.header.stamp.sec = joy_msg.header.stamp.sec;
sticky_buttons_joy_msg.header.frame_id = joy_msg.header.frame_id;
for(int i=0; i < joy_msg.axes.size(); i++){
sticky_buttons_joy_msg.axes[i] = joy_msg.axes[i];
}
pub_.publish(sticky_buttons_joy_msg);
} else {
pub_.publish(joy_msg);
}

publish_now = false;
tv_set = false;
publication_pending = false;
publish_soon = false;
pub_count_++;
}

// If an axis event occurred, start a timer to combine with other
// events.
if (!publication_pending && publish_soon)
{
tv.tv_sec = trunc(coalesce_interval_);
Expand All @@ -298,7 +356,7 @@ class Joystick
tv.tv_sec = trunc(autorepeat_interval);
tv.tv_usec = (autorepeat_interval - tv.tv_sec) * 1e6;
tv_set = true;
//ROS_INFO("Autorepeat pending... %i %i", tv.tv_sec, tv.tv_usec);
//ROS_INFO("Autorepeat pending... %li %li", tv.tv_sec, tv.tv_usec);
}

if (!tv_set)
Expand Down
10 changes: 5 additions & 5 deletions joystick_drivers/package.xml
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
<package>
<package format="2">
<name>joystick_drivers</name>
<version>1.11.0</version>
<license>BSD</license>
Expand All @@ -16,10 +16,10 @@

<buildtool_depend>catkin</buildtool_depend>

<run_depend>joy</run_depend>
<run_depend>ps3joy</run_depend>
<run_depend>spacenav_node</run_depend>
<run_depend>wiimote</run_depend>
<exec_depend>joy</exec_depend>
<exec_depend>ps3joy</exec_depend>
<exec_depend>spacenav_node</exec_depend>
<exec_depend>wiimote</exec_depend>

<export>
<metapackage/>
Expand Down
46 changes: 0 additions & 46 deletions ps3joy/README
View file

This file was deleted.

75 changes: 75 additions & 0 deletions ps3joy/README.md
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,75 @@
# PlayStation 3 Joystick Driver for ROS

This package provides a driver for the PS3 (SIXAXIS or DUALSHOCK3) bluetooth joystick.

This driver provides a more reliable connection, and provides access to the joystick's accelerometers and gyroscope. Linux's native support for the PS3 joystick does lacks this functionality.

Additional documentation:

* [Testing Instructions](doc/testing.md)
* [Bluetooth Device Compatibility](doc/bluetooth_devices.md)

## Dependencies

* joystick
* libusb-dev
* bluez-5.37

## Pairing instructions

1. If you can connect the joystick and the bluetooth dongle into the same
computer connect the joystick to the computer using a USB cable.

2. Load the bluetooth dongle's MAC address into the ps3 joystick using:
```
sudo bash
rosrun ps3joy sixpair
```
If you cannot connect the joystick to the same computer as the dongle,
find out the bluetooth dongle's MAC address by running (on the computer
that has the bluetooth dongle):
```
hciconfig
```
If this does not work, you may need to do
```
sudo hciconfig hci0 up
```
and retry
```
hciconfig
```
3. Plug the PS3 joystick into some other computer using a USB cable.

4. Replace the joystick's mac address in the following command:
```
sudo rosrun ps3joy sixpair 01:23:45:67:89:ab
```

## Starting the PS3 joystick

5. Run the following command
```
rosrun ps3joy ps3joy.py
```
6. Open a new terminal and reboot bluez and run joy with:
```
sudo systemctl restart bluetooth
rosrun joy joy_node
```
7. Open a new terminal and echo the joy topic
```
rostopic echo joy
```
8. This should make a joystick appear at /dev/input/js?

9. You can check that it is working with
jstest /dev/input/js?
(replace ? with the name of your joystick)

## Limitations

This driver will not coexist with any other bluetooth device. In future releases, we plan to allow first non-HID and later any bluetooth device to coexist with this driver. The following devices do coexist:

* Non-HID devices using a userland driver, such as one written using pybluez.
* Keyboards or mice running in HID proxy mode, which appear to the kernel as USB devices.
13 changes: 13 additions & 0 deletions ps3joy/doc/bluetooth_devices.md
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@

## Bluetooth Device Compatibility

This driver works with most 2.x Bluetooth adapters. Please report other compatible and incompatible Bluetooth adapters through a pull request to this page.

### Adapters that are known to work

*

### Adapters that are known not to work

* Linksys USBBT100 version 2 (Bluetooth 1.1)
* USB device 0a12:0x0001
Loading

0 comments on commit 40275ec

Please sign in to comment.