diff options
| -rw-r--r-- | src/platforms/px4_defines.h | 2 | ||||
| -rw-r--r-- | src/platforms/ros/nodes/manual_input/manual_input.cpp | 38 | ||||
| -rw-r--r-- | src/platforms/ros/nodes/manual_input/manual_input.h | 10 |
3 files changed, 48 insertions, 2 deletions
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index c1a0782c4..d6d23d013 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -78,7 +78,7 @@ /* Parameter handle datatype */ typedef const char *px4_param_t; -/* Helper fucntions to set ROS params, only int and float supported */ +/* Helper functions to set ROS params, only int and float supported */ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) { ros::param::set(name, value); diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 41e597ab7..2ee2dadbc 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,31 @@ 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 + /* Map sticks to x, y, z, r */ + 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); + + /* Map buttons to switches */ + //XXX todo + /* for now just publish switches in position for manual flight */ + msg_out.mode_switch = 0; + msg_out.return_switch = 0; + msg_out.posctl_switch = 0; + msg_out.loiter_switch = 0; + msg_out.acro_switch = 0; + msg_out.offboard_switch = 0; _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]; + }; |