Menu
▾
▴
personalrobots-commit
|
From: <wa...@us...> - 2009-07-28 03:11:32
|
Revision: 19820
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19820&view=rev
Author: wattsk
Date: 2009-07-28 03:11:25 +0000 (Tue, 28 Jul 2009)
Log Message:
-----------
Fixed max tilt bug
Modified Paths:
--------------
pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp
Modified: pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp
===================================================================
--- pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp 2009-07-28 03:08:11 UTC (rev 19819)
+++ pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp 2009-07-28 03:11:25 UTC (rev 19820)
@@ -54,7 +54,7 @@
ROS_WARN("maximum pan not set. Assuming 0.6");
if (!hasParam("max_tilt") || !getParam("max_tilt", max_tilt))
ROS_WARN("maximum tilt not set. Assuming 1.4");
- if (!hasParam("min_tilt") || !getParam("min_tilt", max_tilt))
+ if (!hasParam("min_tilt") || !getParam("min_tilt", min_tilt))
ROS_WARN("maximum tilt not set. Assuming -0.4");
if (!hasParam("tilt_step") || !getParam("tilt_step", tilt_step))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <wa...@us...> - 2009-07-28 19:08:59
|
Revision: 19877
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19877&view=rev
Author: wattsk
Date: 2009-07-28 19:08:47 +0000 (Tue, 28 Jul 2009)
Log Message:
-----------
Converted teleop_pr2.cpp to NodeHandle API. Tested on PRE
Modified Paths:
--------------
pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp
Modified: pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp
===================================================================
--- pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp 2009-07-28 19:06:06 UTC (rev 19876)
+++ pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp 2009-07-28 19:08:47 UTC (rev 19877)
@@ -1,7 +1,40 @@
+/*
+ * teleop_pr2
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the <ORGANIZATION> nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+// Author: Kevin Watts
+
#include <cstdlib>
#include <unistd.h>
#include <math.h>
-#include "ros/node.h"
+#include <fcntl.h>
+#include "ros/ros.h"
#include "joy/Joy.h"
#include "robot_msgs/PoseDot.h"
#include "mechanism_msgs/JointState.h"
@@ -9,75 +42,73 @@
#include "std_msgs/Float64.h"
-#define TORSO_TOPIC "/torso_lift_controller/set_command"
-#define HEAD_TOPIC "/head_controller/command"
+#define TORSO_TOPIC "torso_lift_controller/set_command"
+#define HEAD_TOPIC "head_controller/command"
-using namespace ros;
-
-class TeleopBase : public Node
+class TeleopPR2
{
public:
- robot_msgs::PoseDot cmd, cmd_passthrough;
+ robot_msgs::PoseDot cmd;
std_msgs::Float64 torso_vel;
- joy::Joy joy;
+ //joy::Joy joy;
double req_vx, req_vy, req_vw, req_torso, req_pan, req_tilt;
double max_vx, max_vy, max_vw, max_vx_run, max_vy_run, max_vw_run;
double max_pan, max_tilt, min_tilt, pan_step, tilt_step;
int axis_vx, axis_vy, axis_vw, axis_pan, axis_tilt;
- int deadman_button, run_button, torso_dn_button, torso_up_button, head_button, passthrough_button;
+ int deadman_button, run_button, torso_dn_button, torso_up_button, head_button;
bool deadman_no_publish_, torso_publish, head_publish;
+
+ bool deadman_;
+
ros::Time last_recieved_joy_message_time_;
ros::Duration joy_msg_timeout_;
- // Set pan, tilt steps as params
- TeleopBase(bool deadman_no_publish = false) : Node("teleop_base"), max_vx(0.6), max_vy(0.6), max_vw(0.8), max_vx_run(0.6), max_vy_run(0.6), max_vw_run(0.8), max_pan(2.7), max_tilt(1.4), min_tilt(-0.4), pan_step(0.1), tilt_step(0.1), deadman_no_publish_(deadman_no_publish)
- {
+ ros::NodeHandle n_;
+ ros::Publisher vel_pub_;
+ ros::Publisher head_pub_;
+ ros::Publisher torso_pub_;
+ ros::Subscriber joy_sub_;
+
+ TeleopPR2(bool deadman_no_publish = false) : max_vx(0.6), max_vy(0.6), max_vw(0.8), max_vx_run(0.6), max_vy_run(0.6), max_vw_run(0.8), max_pan(2.7), max_tilt(1.4), min_tilt(-0.4), pan_step(0.02), tilt_step(0.015), deadman_no_publish_(deadman_no_publish)
+ { }
+
+ void init()
+ {
torso_vel.data = 0;
cmd.vel.vx = cmd.vel.vy = cmd.ang_vel.vz = 0;
req_pan = req_tilt = 0;
- if (!hasParam("max_vx") || !getParam("max_vx", max_vx))
- ROS_WARN("maximum linear velocity (max_vx) not set. Assuming 0.6");
- if (!hasParam("max_vy") || !getParam("max_vy", max_vy))
- ROS_WARN("maximum linear velocity (max_vy) not set. Assuming 0.6");
- if (!hasParam("max_vw") || !getParam("max_vw", max_vw))
- ROS_WARN("maximum angular velocity (max_vw) not set. Assuming 0.8");
+ n_.param("max_vx", max_vx, max_vx);
+ n_.param("max_vy", max_vy, max_vy);
+ n_.param("max_vw", max_vw, max_vw);
// Set max speed while running
- if (!hasParam("max_vx_run") || !getParam("max_vx_run", max_vx_run))
- ROS_WARN("maximum running linear velocity (max_vx_run) not set. Assuming 0.6");
- if (!hasParam("max_vy_run") || !getParam("max_vy_run", max_vy_run))
- ROS_WARN("maximum running linear velocity (max_vy_run) not set. Assuming 0.6");
- if (!hasParam("max_vw_run") || !getParam("max_vw_run", max_vw_run))
- ROS_WARN("maximum running angular velocity (max_vw_run) not set. Assuming 0.8");
+ n_.param("max_vx_run", max_vx_run, max_vx_run);
+ n_.param("max_vy_run", max_vy_run, max_vy_run);
+ n_.param("max_vw_run", max_vw_run, max_vw_run);
+
+ // Head pan/tilt parameters
+ n_.param("max_pan", max_pan, max_pan);
+ n_.param("max_tilt", max_tilt, max_tilt);
+ n_.param("min_tilt", min_tilt, min_tilt);
- if (!hasParam("max_pan") || !getParam("max_pan", max_pan))
- ROS_WARN("maximum pan not set. Assuming 0.6");
- if (!hasParam("max_tilt") || !getParam("max_tilt", max_tilt))
- ROS_WARN("maximum tilt not set. Assuming 1.4");
- if (!hasParam("min_tilt") || !getParam("min_tilt", min_tilt))
- ROS_WARN("maximum tilt not set. Assuming -0.4");
+ n_.param("tilt_step", tilt_step, tilt_step);
+ n_.param("pan_step", pan_step, pan_step);
+
+ n_.param("axis_pan", axis_pan, 0);
+ n_.param("axis_tilt", axis_tilt, 2);
- if (!hasParam("tilt_step") || !getParam("tilt_step", tilt_step))
- ROS_WARN("tilt step not set. Assuming 0.1");
- if (!hasParam("pan_step") || !getParam("pan_step", pan_step))
- ROS_WARN("pan step not set. Assuming 0.1");
-
- param<int>("axis_pan", axis_pan, 0);
- param<int>("axis_tilt", axis_tilt, 2);
-
- param<int>("axis_vx", axis_vx, 3);
- param<int>("axis_vw", axis_vw, 0);
- param<int>("axis_vy", axis_vy, 2);
+ n_.param("axis_vx", axis_vx, 3);
+ n_.param("axis_vw", axis_vw, 0);
+ n_.param("axis_vy", axis_vy, 2);
- param<int>("deadman_button", deadman_button, 0);
- param<int>("run_button", run_button, 0);
- param<int>("torso_dn_button", torso_dn_button, 0);
- param<int>("torso_up_button", torso_up_button, 0);
- param<int>("head_button", head_button, 0);
- param<int>("passthrough_button", passthrough_button, 1);
+ n_.param("deadman_button", deadman_button, 0);
+ n_.param("run_button", run_button, 0);
+ n_.param("torso_dn_button", torso_dn_button, 0);
+ n_.param("torso_up_button", torso_up_button, 0);
+ n_.param("head_button", head_button, 0);
double joy_msg_timeout;
- param<double>("joy_msg_timeout", joy_msg_timeout, -1.0); //default to no timeout
+ n_.param("joy_msg_timeout", joy_msg_timeout, -1.0); //default to no timeout
if (joy_msg_timeout <= 0)
{
joy_msg_timeout_ = ros::Duration().fromSec(9999999);//DURATION_MAX;
@@ -111,182 +142,157 @@
ROS_DEBUG("torso_dn_button: %d\n", torso_dn_button);
ROS_DEBUG("torso_up_button: %d\n", torso_up_button);
ROS_DEBUG("head_button: %d\n", head_button);
- ROS_DEBUG("passthrough_button: %d\n", passthrough_button);
ROS_DEBUG("joy_msg_timeout: %f\n", joy_msg_timeout);
if (torso_dn_button != 0)
- advertise<std_msgs::Float64>(TORSO_TOPIC, 1);
+ torso_pub_ = n_.advertise<std_msgs::Float64>(TORSO_TOPIC, 1);
+
if (head_button != 0)
- advertise<mechanism_msgs::JointStates>(HEAD_TOPIC, 1);
+ head_pub_ = n_.advertise<mechanism_msgs::JointStates>(HEAD_TOPIC, 1);
- advertise<robot_msgs::PoseDot>("cmd_vel", 1);
- subscribe("joy", joy, &TeleopBase::joy_cb, 1);
- subscribe("cmd_passthrough", cmd_passthrough, &TeleopBase::passthrough_cb, 1);
- ROS_DEBUG("done with ctor\n");
+ vel_pub_ = n_.advertise<robot_msgs::PoseDot>("cmd_vel", 1);
+
+ joy_sub_ = n_.subscribe("joy", 10, &TeleopPR2::joy_cb, this);
}
- ~TeleopBase()
+ ~TeleopPR2() { }
+
+
+ /** Callback for joy topic **/
+ void joy_cb(const joy::Joy::ConstPtr& joy_msg)
{
- unsubscribe("joy");
- unsubscribe("cmd_passthrough");
- unadvertise("cmd_vel");
+ //Record this message reciept
+ last_recieved_joy_message_time_ = ros::Time::now();
+
+ bool cmd_head = (((unsigned int)head_button < joy_msg->get_buttons_size()) && joy_msg->buttons[head_button]);
+
+ deadman_ = (((unsigned int)deadman_button < joy_msg->get_buttons_size()) && joy_msg->buttons[deadman_button]);
+
+ // Base
+ bool running = (((unsigned int)run_button < joy_msg->get_buttons_size()) && joy_msg->buttons[run_button]);
+ double vx = running ? max_vx_run : max_vx;
+ double vy = running ? max_vy_run : max_vy;
+ double vw = running ? max_vw_run : max_vw;
+
+ if((axis_vx >= 0) && (((unsigned int)axis_vx) < joy_msg->get_axes_size()) && !cmd_head)
+ req_vx = joy_msg->axes[axis_vx] * vx;
+ else
+ req_vx = 0.0;
+ if((axis_vy >= 0) && (((unsigned int)axis_vy) < joy_msg->get_axes_size()) && !cmd_head)
+ req_vy = joy_msg->axes[axis_vy] * vy;
+ else
+ req_vy = 0.0;
+ if((axis_vw >= 0) && (((unsigned int)axis_vw) < joy_msg->get_axes_size()) && !cmd_head)
+ req_vw = joy_msg->axes[axis_vw] * vw;
+ else
+ req_vw = 0.0;
+
+ // Head
+ // Update commanded position by how joysticks moving
+ // Don't add commanded position if deadman off
+ if((axis_pan >= 0) && (((unsigned int)axis_pan) < joy_msg->get_axes_size()) && cmd_head && deadman_)
+ {
+ req_pan += joy_msg->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_msg->get_axes_size()) && cmd_head && deadman_)
+ {
+ req_tilt += joy_msg->axes[axis_tilt] * tilt_step;
+ req_tilt = std::max(std::min(req_tilt, max_tilt), min_tilt);
+ }
+
+ // Torso
+ bool down = (((unsigned int)torso_dn_button < joy_msg->get_buttons_size()) && joy_msg->buttons[torso_dn_button]);
+ bool up = (((unsigned int)torso_up_button < joy_msg->get_buttons_size()) && joy_msg->buttons[torso_up_button]);
+
+ // Bring torso up/down
+ if (down && !up)
+ req_torso = -0.01;
+ else if (up && !down)
+ req_torso = 0.01;
+ else
+ req_torso = 0;
+ }
- if (torso_dn_button != 0)
- unadvertise(TORSO_TOPIC);
- if (head_button != 0)
- unadvertise(HEAD_TOPIC);
+
+ void send_cmd_vel()
+ {
+ //joy.lock();
- }
+ // Need to lock shared data...
+ // share cmd_vel, joint_cmd, torso_vel, lock those
- void joy_cb()
+ if(deadman_ &&
+ last_recieved_joy_message_time_ + joy_msg_timeout_ > ros::Time::now())
+ {
+ cmd.vel.vx = req_vx;
+ cmd.vel.vy = req_vy;
+ cmd.ang_vel.vz = req_vw;
+ vel_pub_.publish(cmd);
+
+ // Torso
+ torso_vel.data = req_torso;
+ if (torso_dn_button != 0)
+ torso_pub_.publish(torso_vel);
+
+ // Head
+ if (head_button != 0)
{
- //Record this message reciept
- last_recieved_joy_message_time_ = ros::Time::now();
-
- bool cmd_head = (((unsigned int)head_button < joy.get_buttons_size()) && joy.buttons[head_button]);
-
- bool deadman = (((unsigned int)deadman_button < joy.get_buttons_size()) && joy.buttons[deadman_button]);
+ mechanism_msgs::JointState joint_cmd ;
+ mechanism_msgs::JointStates joint_cmds;
- // Base
- bool running = (((unsigned int)run_button < joy.get_buttons_size()) && joy.buttons[run_button]);
- double vx = running ? max_vx_run : max_vx;
- double vy = running ? max_vy_run : max_vy;
- double vw = running ? max_vw_run : max_vw;
-
- if((axis_vx >= 0) && (((unsigned int)axis_vx) < joy.get_axes_size()) && !cmd_head)
- req_vx = joy.axes[axis_vx] * vx;
- else
- req_vx = 0.0;
- if((axis_vy >= 0) && (((unsigned int)axis_vy) < joy.get_axes_size()) && !cmd_head)
- req_vy = joy.axes[axis_vy] * vy;
- else
- req_vy = 0.0;
- if((axis_vw >= 0) && (((unsigned int)axis_vw) < joy.get_axes_size()) && !cmd_head)
- req_vw = joy.axes[axis_vw] * vw;
- else
- req_vw = 0.0;
-
- // Head
- // Update commanded position by how joysticks moving
- // Don't add commanded position if deadman off
- if((axis_pan >= 0) && (((unsigned int)axis_pan) < joy.get_axes_size()) && cmd_head && deadman)
- {
- 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()) && cmd_head && deadman)
- {
- req_tilt += joy.axes[axis_tilt] * tilt_step;
- req_tilt = std::max(std::min(req_tilt, max_tilt), min_tilt);
- }
-
-
- // Torso
- bool down = (((unsigned int)torso_dn_button < joy.get_buttons_size()) && joy.buttons[torso_dn_button]);
- bool up = (((unsigned int)torso_up_button < joy.get_buttons_size()) && joy.buttons[torso_up_button]);
-
- // Bring torso up/down with max effort
- if (down && !up)
- req_torso = -0.01;
- else if (up && !down)
- req_torso = 0.01;
- else
- req_torso = 0;
-
-
+ joint_cmd.name ="head_pan_joint";
+ joint_cmd.position = req_pan;
+ joint_cmds.joints.push_back(joint_cmd);
+ joint_cmd.name="head_tilt_joint";
+ joint_cmd.position = req_tilt;
+ joint_cmds.joints.push_back(joint_cmd);
+
+ head_pub_.publish(joint_cmds);
}
- void passthrough_cb() { }
- void send_cmd_vel()
+
+ if (req_torso != 0)
+ fprintf(stderr,"teleop_base:: %f, %f, %f. Head:: %f, %f. Torso cmd: %f.\n", cmd.vel.vx, cmd.vel.vy, cmd.ang_vel.vz, req_pan, req_tilt, torso_vel.data);
+ else
+ fprintf(stderr,"teleop_base:: %f, %f, %f. Head:: %f, %f\n", cmd.vel.vx ,cmd.vel.vy, cmd.ang_vel.vz, req_pan, req_tilt);
+ }
+ else
+ {
+ // Publish zero commands iff deadman_no_publish is false
+ cmd.vel.vx = cmd.vel.vy = cmd.ang_vel.vz = 0;
+ torso_vel.data = 0;
+ if (!deadman_no_publish_)
{
- joy.lock();
- if(((deadman_button < 0) ||
- ((((unsigned int)deadman_button) < joy.get_buttons_size()) &&
- joy.buttons[deadman_button]))
- &&
- last_recieved_joy_message_time_ + joy_msg_timeout_ > ros::Time::now())
- {
- if (passthrough_button >= 0 &&
- passthrough_button < (int)joy.get_buttons_size() &&
- joy.buttons[passthrough_button])
- {
- // pass through commands that we have received (e.g. from wavefront)
- cmd_passthrough.lock();
- cmd = cmd_passthrough;
- cmd_passthrough.unlock();
- }
- else
- {
- // use commands from the local sticks
- cmd.vel.vx = req_vx;
- cmd.vel.vy = req_vy;
- cmd.ang_vel.vz = req_vw;
- }
- publish("cmd_vel", cmd);
-
- // Torso
- torso_vel.data = req_torso;
- if (torso_dn_button != 0)
- publish(TORSO_TOPIC, torso_vel);
-
- // Head
- if (head_button != 0)
- {
-
- mechanism_msgs::JointState joint_cmd ;
- mechanism_msgs::JointStates joint_cmds;
-
- joint_cmd.name ="head_pan_joint";
- joint_cmd.position = req_pan;
- joint_cmds.joints.push_back(joint_cmd);
- joint_cmd.name="head_tilt_joint";
- joint_cmd.position = req_tilt;
- joint_cmds.joints.push_back(joint_cmd);
-
- publish(HEAD_TOPIC, joint_cmds);
- }
-
- if (req_torso != 0)
- fprintf(stderr,"teleop_base:: %f, %f, %f. Head:: %f, %f. Torso effort: %f.\n",cmd.vel.vx,cmd.vel.vy,cmd.ang_vel.vz, req_pan, req_tilt, torso_vel.data);
- else
- fprintf(stderr,"teleop_base:: %f, %f, %f. Head:: %f, %f\n",cmd.vel.vx,cmd.vel.vy,cmd.ang_vel.vz, req_pan, req_tilt);
- }
- else
- {
- cmd.vel.vx = cmd.vel.vy = cmd.ang_vel.vz = 0;
- torso_vel.data = 0;
- if (!deadman_no_publish_)
- {
- publish("cmd_vel", cmd);//Only publish if deadman_no_publish is enabled
- if (torso_dn_button != 0)
- publish(TORSO_TOPIC, torso_vel);
-
- // Publish head
- if (head_button != 0)
- {
- mechanism_msgs::JointState joint_cmd ;
- mechanism_msgs::JointStates joint_cmds;
-
- joint_cmd.name ="head_pan_joint";
- joint_cmd.position = req_pan;
- joint_cmds.joints.push_back(joint_cmd);
- joint_cmd.name="head_tilt_joint";
- joint_cmd.position = req_tilt;
- joint_cmds.joints.push_back(joint_cmd);
-
-
- publish(HEAD_TOPIC, joint_cmds);
- }
-
- }
- }
- joy.unlock();
+ vel_pub_.publish(cmd);//Only publish if deadman_no_publish is enabled
+ if (torso_dn_button != 0)
+ torso_pub_.publish(torso_vel);
+
+ // Publish head
+ if (head_button != 0)
+ {
+ mechanism_msgs::JointState joint_cmd ;
+ mechanism_msgs::JointStates joint_cmds;
+
+ joint_cmd.name ="head_pan_joint";
+ joint_cmd.position = req_pan;
+ joint_cmds.joints.push_back(joint_cmd);
+ joint_cmd.name="head_tilt_joint";
+ joint_cmd.position = req_tilt;
+ joint_cmds.joints.push_back(joint_cmd);
+
+ head_pub_.publish(joint_cmds);
+ }
+
}
+ }
+ //joy.unlock();
+ }
};
int main(int argc, char **argv)
{
- ros::init(argc, argv);
+ ros::init(argc, argv, "teleop_pr2");
const char* opt_no_publish = "--deadman_no_publish";
bool no_publish = false;
@@ -295,13 +301,20 @@
if(!strncmp(argv[i], opt_no_publish, strlen(opt_no_publish)))
no_publish = true;
}
- TeleopBase teleop_base(no_publish);
- while (teleop_base.ok())
+
+ TeleopPR2 teleop_pr2(no_publish);
+ teleop_pr2.init();
+
+ ros::Rate pub_rate(20);
+
+ while (teleop_pr2.n_.ok())
{
- usleep(50000);
- teleop_base.send_cmd_vel();
+ ros::spinOnce();
+ teleop_pr2.send_cmd_vel();
+ pub_rate.sleep();
}
+
exit(0);
return 0;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <wa...@us...> - 2009-08-10 19:03:11
|
Revision: 21442
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21442&view=rev
Author: wattsk
Date: 2009-08-10 19:03:01 +0000 (Mon, 10 Aug 2009)
Log Message:
-----------
Changed torso velocity topic name
Modified Paths:
--------------
pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp
Modified: pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp
===================================================================
--- pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp 2009-08-10 18:57:50 UTC (rev 21441)
+++ pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp 2009-08-10 19:03:01 UTC (rev 21442)
@@ -42,7 +42,7 @@
#include "std_msgs/Float64.h"
-#define TORSO_TOPIC "torso_lift_controller/set_command"
+#define TORSO_TOPIC "torso_lift_velocity_controller/set_command"
#define HEAD_TOPIC "head_controller/command"
class TeleopPR2
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <wa...@us...> - 2009-08-28 17:35:09
|
Revision: 23269
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23269&view=rev
Author: wattsk
Date: 2009-08-28 17:34:57 +0000 (Fri, 28 Aug 2009)
Log Message:
-----------
Changed to enforce max velocity limits even if joystick not normalized
Modified Paths:
--------------
pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp
Modified: pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp
===================================================================
--- pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp 2009-08-28 17:34:53 UTC (rev 23268)
+++ pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp 2009-08-28 17:34:57 UTC (rev 23269)
@@ -45,6 +45,8 @@
#define TORSO_TOPIC "torso_lift_velocity_controller/set_command"
#define HEAD_TOPIC "head_controller/command"
+using namespace std;
+
class TeleopPR2
{
public:
@@ -189,6 +191,12 @@
req_vw = joy_msg->axes[axis_vw] * vw;
else
req_vw = 0.0;
+
+ // Enforce max/mins for velocity
+ // Joystick should be [-1, 1], but it might not be
+ req_vx = max(min(req_vx, vx), -vx);
+ req_vy = max(min(req_vy, vy), -vy);
+ req_vw = max(min(req_vw, vw), -vw);
// Head
// Update commanded position by how joysticks moving
@@ -196,13 +204,13 @@
if((axis_pan >= 0) && (((unsigned int)axis_pan) < joy_msg->get_axes_size()) && cmd_head && deadman_)
{
req_pan += joy_msg->axes[axis_pan] * pan_step;
- req_pan = std::max(std::min(req_pan, max_pan), -max_pan);
+ req_pan = max(min(req_pan, max_pan), -max_pan);
}
if ((axis_tilt >= 0) && (((unsigned int)axis_tilt) < joy_msg->get_axes_size()) && cmd_head && deadman_)
{
req_tilt += joy_msg->axes[axis_tilt] * tilt_step;
- req_tilt = std::max(std::min(req_tilt, max_tilt), min_tilt);
+ req_tilt = max(min(req_tilt, max_tilt), min_tilt);
}
// Torso
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <wa...@us...> - 2009-09-04 20:19:13
|
Revision: 23861
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23861&view=rev
Author: wattsk
Date: 2009-09-04 20:19:06 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
Changed torso command topic
Modified Paths:
--------------
pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp
Modified: pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp
===================================================================
--- pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp 2009-09-04 20:00:56 UTC (rev 23860)
+++ pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp 2009-09-04 20:19:06 UTC (rev 23861)
@@ -41,7 +41,7 @@
#include "std_msgs/Float64.h"
-#define TORSO_TOPIC "torso_lift_velocity_controller/set_command"
+#define TORSO_TOPIC "torso_lift_velocity_controller/command"
#define HEAD_TOPIC "head_controller/command"
using namespace std;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
×
Want the latest updates on software, tech news, and AI?
Get latest updates about software, tech news, and AI from SourceForge directly in your inbox once a month.
Thanks for helping keep SourceForge clean.
X