diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-30 11:16:53 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-30 11:16:53 +0100 |
commit | c01680459409d0119e72becc48d497fdf59e7023 (patch) | |
tree | 8d65e65d1d94f57f2282a592ce5d87616007d1a0 | |
parent | 824446bf2f91d76743d9c8c17d710d4ac51bfaf8 (diff) | |
download | px4-firmware-c01680459409d0119e72becc48d497fdf59e7023.tar.gz px4-firmware-c01680459409d0119e72becc48d497fdf59e7023.tar.bz2 px4-firmware-c01680459409d0119e72becc48d497fdf59e7023.zip |
manual input: map axes
-rw-r--r-- | src/platforms/ros/nodes/manual_input/manual_input.cpp | 27 | ||||
-rw-r--r-- | src/platforms/ros/nodes/manual_input/manual_input.h | 10 |
2 files changed, 36 insertions, 1 deletions
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 41e597ab7..8fc158504 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -47,6 +47,22 @@ ManualInput::ManualInput() : _sub_joy(_n.subscribe("joy", 10, &ManualInput::JoyCallback, this)), _man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 10)) { + /* Load parameters, default values work for Microsoft XBox Controller */ + _n.param("map_x", param_axes_map[0], 4); + _n.param("scale_x", param_axes_scale[0], 1.0); + _n.param("off_x", param_axes_offset[0], 0.0); + + _n.param("map_y", param_axes_map[1], 3); + _n.param("scale_y", param_axes_scale[1], -1.0); + _n.param("off_y", param_axes_offset[1], 0.0); + + _n.param("map_z", param_axes_map[2], 2); + _n.param("scale_z", param_axes_scale[2], -0.5); + _n.param("off_z", param_axes_offset[2], 0.5); + + _n.param("map_r", param_axes_map[3], 0); + _n.param("scale_r", param_axes_scale[3], -1.0); + _n.param("off_r", param_axes_offset[3], 0.0); } void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg) @@ -54,11 +70,20 @@ void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg) px4::manual_control_setpoint msg_out; /* Fill px4 manual control setpoint topic with contents from ros joystick */ - //XXX + MapAxis(msg, param_axes_map[0], param_axes_scale[0], param_axes_offset[0], msg_out.x); + MapAxis(msg, param_axes_map[1], param_axes_scale[1], param_axes_offset[1], msg_out.y); + MapAxis(msg, param_axes_map[2], param_axes_scale[2], param_axes_offset[2], msg_out.z); + MapAxis(msg, param_axes_map[3], param_axes_scale[3], param_axes_offset[3], msg_out.r); + ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_out.x, msg_out.y, msg_out.z, msg_out.r); _man_ctrl_sp_pub.publish(msg_out); } +void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, float &out) +{ + out = (float)(msg->axes[map_index] * scale + offset); +} + int main(int argc, char **argv) { ros::init(argc, argv, "manual_input"); diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h index 1721d5e81..fbe635cf2 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.h +++ b/src/platforms/ros/nodes/manual_input/manual_input.h @@ -53,9 +53,19 @@ protected: */ void JoyCallback(const sensor_msgs::JoyConstPtr& msg); + /** + * Helper function to map and scale joystick input + */ + void MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, float &out); + ros::NodeHandle _n; ros::Subscriber _sub_joy; ros::Publisher _man_ctrl_sp_pub; + /* Parameters */ + int param_axes_map[4]; + double param_axes_scale[4]; + double param_axes_offset[4]; + }; |