Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[5493] pkg/trunk
|
From: <tf...@us...> - 2008-10-17 18:47:44
|
Revision: 5493
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5493&view=rev
Author: tfoote
Date: 2008-10-17 18:47:35 +0000 (Fri, 17 Oct 2008)
Log Message:
-----------
adding teleop_head
Modified Paths:
--------------
pkg/trunk/nav/teleop_base/CMakeLists.txt
pkg/trunk/nav/teleop_base/manifest.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/joy_pr2_roslaunch.xml
Added Paths:
-----------
pkg/trunk/nav/teleop_base/teleop_head.cpp
Modified: pkg/trunk/nav/teleop_base/CMakeLists.txt
===================================================================
--- pkg/trunk/nav/teleop_base/CMakeLists.txt 2008-10-17 17:55:28 UTC (rev 5492)
+++ pkg/trunk/nav/teleop_base/CMakeLists.txt 2008-10-17 18:47:35 UTC (rev 5493)
@@ -2,3 +2,4 @@
include(rosbuild)
rospack(teleop_base)
rospack_add_executable(teleop_base teleop_base.cpp)
+rospack_add_executable(teleop_head teleop_head.cpp)
Modified: pkg/trunk/nav/teleop_base/manifest.xml
===================================================================
--- pkg/trunk/nav/teleop_base/manifest.xml 2008-10-17 17:55:28 UTC (rev 5492)
+++ pkg/trunk/nav/teleop_base/manifest.xml 2008-10-17 18:47:35 UTC (rev 5493)
@@ -3,4 +3,5 @@
<license>BSD</license>
<depend package="joy"/>
<depend package="std_msgs"/>
+ <depend package="pr2_mechanism_controllers"/>
</package>
Added: pkg/trunk/nav/teleop_base/teleop_head.cpp
===================================================================
--- pkg/trunk/nav/teleop_base/teleop_head.cpp (rev 0)
+++ pkg/trunk/nav/teleop_base/teleop_head.cpp 2008-10-17 18:47:35 UTC (rev 5493)
@@ -0,0 +1,109 @@
+#include <unistd.h>
+#include <math.h>
+#include "ros/node.h"
+#include "joy/Joy.h"
+
+#include "pr2_mechanism_controllers/SetJointCmd.h"
+
+using namespace ros;
+
+class TeleopHead : public node
+{
+ public:
+ // std_msgs::HeadVel cmd, cmd_passthrough;
+ joy::Joy joy;
+ double req_pan, req_tilt, max_pan, max_tilt;
+ int axis_pan, axis_tilt;
+ int deadman_button, passthrough_button;
+ double pan_step, tilt_step;
+ bool deadman_no_publish_;
+
+
+ TeleopHead(bool deadman_no_publish = false) : node("teleop_head"), max_pan(0.6), max_tilt(0.4), pan_step(0.1), tilt_step(0.1), deadman_no_publish_(deadman_no_publish)
+ {
+ // cmd.vx = cmd.vy = cmd.vw = 0;
+ if (!has_param("max_pan") || !get_param("max_pan", max_pan))
+ log(WARNING, "maximum pan not set. Assuming 0.6");
+ if (!has_param("max_tilt") || !get_param("max_tilt", max_tilt))
+ log(WARNING, "maximum tilt not set. Assuming 0.4");
+
+ param<int>("axis_pan", axis_pan, 4);
+ param<int>("axis_tilt", axis_tilt, 5);
+ param<int>("deadman_button", deadman_button, 0);
+ param<int>("passthrough_button", passthrough_button, 1);
+
+ /* printf("max_vx: %.3f m/s\n", max_vx);
+ printf("max_vy: %.3f m/s\n", max_vy);
+ printf("max_vw: %.3f deg/s\n", max_vw*180.0/M_PI);
+ printf("axis_vx: %d\n", axis_vx);
+ printf("axis_vy: %d\n", axis_vy);
+ printf("axis_vw: %d\n", axis_vw);
+ printf("deadman_button: %d\n", deadman_button);
+ printf("passthrough_button: %d\n", passthrough_button);
+ */
+ // advertise<std_msgs::HeadVel>("cmd_vel", 1);
+ subscribe("joy", joy, &TeleopHead::joy_cb, 1);
+ //subscribe("cmd_passthrough", cmd_passthrough, &TeleopHead::passthrough_cb, 1);
+ printf("done with ctor\n");
+ }
+ void joy_cb()
+ {
+ /*
+ printf("axes: ");
+ for(int i=0;i<joy.get_axes_size();i++)
+ printf("%.3f ", joy.axes[i]);
+ puts("");
+ printf("buttons: ");
+ for(int i=0;i<joy.get_buttons_size();i++)
+ printf("%d ", joy.buttons[i]);
+ puts("");
+ */
+
+ if((axis_pan >= 0) && (((unsigned int)axis_pan) < joy.get_axes_size()))
+ {
+ req_pan += joy.axes[axis_pan] * pan_step;
+ req_pan = std::max(std::min(req_pan, max_pan), -max_pan);
+ }
+
+ if ((axis_tilt >= 0) && (((unsigned int)axis_tilt) < joy.get_axes_size()))
+ {
+ req_tilt += joy.axes[axis_tilt] * tilt_step;
+ req_tilt = std::max(std::min(req_tilt, max_tilt), -max_tilt);
+ }
+
+ pr2_mechanism_controllers::SetJointCmd::request req;
+ pr2_mechanism_controllers::SetJointCmd::response res;
+ req.positions.push_back(req_pan);
+ req.positions.push_back(req_tilt);
+ req.velocity.push_back(0.0);
+ req.velocity.push_back(0.0);
+ req.acc.push_back(0.0);
+ req.acc.push_back(0.0);
+ req.names.push_back("head_pan_joint");
+ req.names.push_back("head_tilt_joint");
+ ros::service::call("head_controller/set_command_array", req, res);
+
+ }
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ const char* opt_no_publish = "--deadman_no_publish";
+
+ bool no_publish = false;
+ for(int i=1;i<argc;i++)
+ {
+ if(!strncmp(argv[i], opt_no_publish, strlen(opt_no_publish)))
+ no_publish = true;
+ }
+ TeleopHead teleop_base(no_publish);
+ while (teleop_base.ok())
+ {
+ usleep(50000);
+ }
+ ros::fini();
+ exit(0);
+ return 0;
+}
+
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/joy_pr2_roslaunch.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/joy_pr2_roslaunch.xml 2008-10-17 17:55:28 UTC (rev 5492)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/joy_pr2_roslaunch.xml 2008-10-17 18:47:35 UTC (rev 5493)
@@ -1,8 +1,10 @@
<launch>
<group name="wg">
<param name="axis_vx" value="3" type="int"/>
-<param name="axis_vy" value="0" type="int"/>
-<param name="axis_vw" value="2" type="int"/>
+<param name="axis_vy" value="2" type="int"/>
+<param name="axis_vw" value="0" type="int"/>
+<param name="pan" value="4" type="int"/>
+<param name="tilt" value="5" type="int"/>
<param name="max_vw" value="1.0" />
<param name="max_vx" value="0.6" />
<param name="max_vy" value="0.6" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
