Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[19494] pkg/trunk
|
From: <hsu...@us...> - 2009-07-23 02:05:47
|
Revision: 19494
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19494&view=rev
Author: hsujohnhsu
Date: 2009-07-23 02:05:44 +0000 (Thu, 23 Jul 2009)
Log Message:
-----------
update erratic demo.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp
pkg/trunk/robot_descriptions/erratic/erratic_defs/robots/erratic.xml
Added Paths:
-----------
pkg/trunk/demos/erratic_gazebo/erratic_demo.launch
Removed Paths:
-------------
pkg/trunk/demos/erratic_gazebo/erratic.launch
Deleted: pkg/trunk/demos/erratic_gazebo/erratic.launch
===================================================================
--- pkg/trunk/demos/erratic_gazebo/erratic.launch 2009-07-23 02:05:17 UTC (rev 19493)
+++ pkg/trunk/demos/erratic_gazebo/erratic.launch 2009-07-23 02:05:44 UTC (rev 19494)
@@ -1,16 +0,0 @@
-<launch>
- <!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/pr2/erratic_defs/robots for full erratic -->
-
- <!-- Create a transform sender for linking these frames. -->
- <node pkg="tf" type="transform_sender" args="0 0 0 0 0 0 base_footprint base_link 40" />
-
- <!-- send pr2.xml to param server -->
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find erratic_defs)/robots/erratic.xml'" />
-
- <!-- push robot_description to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robot_description" respawn="false" output="screen" />
-
- <!-- load controllers -->
- <node pkg="gazebo_plugin" type="ros_diffdrive" respawn="true" />
-</launch>
-
Copied: pkg/trunk/demos/erratic_gazebo/erratic_demo.launch (from rev 19422, pkg/trunk/demos/erratic_gazebo/erratic.launch)
===================================================================
--- pkg/trunk/demos/erratic_gazebo/erratic_demo.launch (rev 0)
+++ pkg/trunk/demos/erratic_gazebo/erratic_demo.launch 2009-07-23 02:05:44 UTC (rev 19494)
@@ -0,0 +1,25 @@
+<launch>
+ <!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/pr2/erratic_defs/robots for full erratic -->
+ <!-- start up empty world -->
+ <include file="$(find gazebo)/launch/wg_world.launch"/>
+
+ <!-- Create a transform sender for linking these frames. -->
+ <node pkg="tf" type="transform_sender" args="0 0 0 0 0 0 base_footprint base_link 40" />
+
+ <!-- send pr2.xml to param server -->
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find erratic_defs)/robots/erratic.xml'" />
+
+ <!-- Robot state publisher -->
+ <node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher" output="screen">
+ <param name="publish_frequency" type="double" value="50.0" />
+ <param name="tf_prefix" type="string" value="" />
+ </node>
+
+ <!-- push robot_description to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robot_description" respawn="false" output="screen" />
+
+ <!-- load controllers -->
+ <node pkg="gazebo_plugin" type="ros_diffdrive" respawn="true" output="screen"/>
+
+</launch>
+
Property changes on: pkg/trunk/demos/erratic_gazebo/erratic_demo.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/demos/erratic_gazebo/erratic.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp 2009-07-23 02:05:17 UTC (rev 19493)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp 2009-07-23 02:05:44 UTC (rev 19494)
@@ -27,25 +27,34 @@
#include <gazebo/gazebo.h>
#include <gazebo/GazeboError.hh>
-#include <ros/node.h>
+#include <ros/ros.h>
#include "tf/transform_broadcaster.h"
#include <robot_msgs/PoseDot.h>
#include <deprecated_msgs/RobotBase2DOdom.h>
+#include <boost/bind.hpp>
+
gazebo::PositionIface *posIface = NULL;
-robot_msgs::PoseDot cmd_msg;
class DiffDrive {
public:
gazebo::PositionIface *posIface;
- robot_msgs::PoseDot cmd_msg;
+ ros::NodeHandle* rnh_;
+ ros::Subscriber sub_;
+ ros::Publisher pub_;
- void cmdVelCallBack() {
+ void cmdVelCallBack(const robot_msgs::PoseDot::ConstPtr& cmd_msg) {
+ std::cout << " pos " << posIface
+ << " x " << cmd_msg->vel.vx
+ << " y " << cmd_msg->vel.vy
+ << " z " << cmd_msg->ang_vel.vz
+ << std::endl;
+
if (posIface) {
posIface->Lock(1);
- posIface->data->cmdVelocity.pos.x = cmd_msg.vel.vx;
- posIface->data->cmdVelocity.pos.y = cmd_msg.vel.vy;
- posIface->data->cmdVelocity.yaw = cmd_msg.ang_vel.vz;
+ posIface->data->cmdVelocity.pos.x = cmd_msg->vel.vx;
+ posIface->data->cmdVelocity.pos.y = cmd_msg->vel.vy;
+ posIface->data->cmdVelocity.yaw = cmd_msg->ang_vel.vz;
posIface->Unlock();
}
}
@@ -71,11 +80,11 @@
} catch (gazebo::GazeboError e) {
std::cout << "Gazebo error: Unable to connect to the sim interface\n" << e << "\n";
return;
- }
+ }
/// Open the Position interface
try {
- posIface->Open(client, "position_iface_0");
+ posIface->Open(client, "robot_description::position_iface_0");
} catch (std::string e) {
std::cout << "Gazebo error: Unable to connect to the position interface\n" << e << "\n";
return;
@@ -85,62 +94,68 @@
posIface->Lock(1);
posIface->data->cmdEnableMotors = 1;
posIface->Unlock();
-
- ros::Node n("gazebo_diffdrive");
- ros::Node::instance()->subscribe("/cmd_vel", cmd_msg, &DiffDrive::cmdVelCallBack, this, 10);
- ros::Node::instance()->advertise<deprecated_msgs::RobotBase2DOdom>("odom", 1);
+
+ this->rnh_ = new ros::NodeHandle();
+ this->sub_ = rnh_->subscribe<robot_msgs::PoseDot>("/cmd_vel", 100, boost::bind(&DiffDrive::cmdVelCallBack,this,_1));
+ this->pub_ = rnh_->advertise<deprecated_msgs::RobotBase2DOdom>("/odom", 1);
deprecated_msgs::RobotBase2DOdom odom;
tf::TransformBroadcaster tfb;
ros::Duration d; d.fromSec(0.01);
- while(n.ok()) {
+ while(rnh_->ok()) {
if (posIface) {
- posIface->Lock(1);
-
- btQuaternion qt; qt.setEulerZYX(posIface->data->pose.yaw, posIface->data->pose.pitch, posIface->data->pose.roll);
- btVector3 vt(posIface->data->pose.pos.x, posIface->data->pose.pos.y, posIface->data->pose.pos.z);
+ posIface->Lock(1);
+
+ btQuaternion qt; qt.setEulerZYX(posIface->data->pose.yaw, posIface->data->pose.pitch, posIface->data->pose.roll);
+ btVector3 vt(posIface->data->pose.pos.x, posIface->data->pose.pos.y, posIface->data->pose.pos.z);
- tf::Transform latest_tf(qt, vt);
+ tf::Transform latest_tf(qt, vt);
- // We want to send a transform that is good up until a
- // tolerance time so that odom can be used
- ros::Time transform_expiration = ros::Time::now();
- tf::Stamped<tf::Transform> tmp_tf_stamped(latest_tf.inverse(),
- transform_expiration,
- "base_link", "odom");
- tfb.sendTransform(tmp_tf_stamped);
-
+ // We want to send a transform that is good up until a
+ // tolerance time so that odom can be used
+ ros::Time transform_expiration = ros::Time::now();
+ tf::Stamped<tf::Transform> tmp_tf_stamped(latest_tf.inverse(),
+ transform_expiration,
+ "base_link", "odom");
+ tfb.sendTransform(tmp_tf_stamped);
+
-
- odom.pos.x = posIface->data->pose.pos.x;
- odom.pos.y = posIface->data->pose.pos.y;
- odom.pos.th = posIface->data->pose.yaw;
- odom.vel.x = posIface->data->velocity.pos.x;
- odom.vel.y = posIface->data->velocity.pos.y;
- odom.vel.th = posIface->data->velocity.yaw;
- odom.stall = 0;
-
- odom.header.frame_id = "odom";
-
- odom.header.stamp = transform_expiration;
-
- ros::Node::instance()->publish("odom", odom);
+ odom.pos.x = posIface->data->pose.pos.x;
+ odom.pos.y = posIface->data->pose.pos.y;
+ odom.pos.th = posIface->data->pose.yaw;
+ odom.vel.x = posIface->data->velocity.pos.x;
+ odom.vel.y = posIface->data->velocity.pos.y;
+ odom.vel.th = posIface->data->velocity.yaw;
+ odom.stall = 0;
+
+ odom.header.frame_id = "odom";
+
+ odom.header.stamp = transform_expiration;
+
+ this->pub_.publish(odom);
-
- posIface->Unlock();
+ posIface->Unlock();
}
d.sleep();
}
}
+ ~DiffDrive() {
+ delete this->rnh_;
+ }
};
int main(int argc, char** argv) {
- ros::init(argc, argv);
+ ros::init(argc,argv,"ros_diffdrive");
+
+ // spawn 2 threads by default, ///@todo: make this a parameter
+ ros::MultiThreadedSpinner s(1);
+ boost::thread spinner_thread( boost::bind( &ros::spin, s ) );
+
DiffDrive d;
return 0;
}
Modified: pkg/trunk/robot_descriptions/erratic/erratic_defs/robots/erratic.xml
===================================================================
--- pkg/trunk/robot_descriptions/erratic/erratic_defs/robots/erratic.xml 2009-07-23 02:05:17 UTC (rev 19493)
+++ pkg/trunk/robot_descriptions/erratic/erratic_defs/robots/erratic.xml 2009-07-23 02:05:44 UTC (rev 19494)
@@ -16,7 +16,7 @@
<map name="sensor" flag="gazebo">
<verbatim key="ros_time_controller">
- <controller:ros_time name="ros_time" plugin="libros_time.so">
+ <controller:ros_time name="ros_time_controller" plugin="libros_time.so">
<alwaysOn>true</alwaysOn>
<updateRate>1000.0</updateRate>
<interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
@@ -44,7 +44,7 @@
<link name="base_link">
<parent name="world" />
<origin xyz="0 0 0.051" rpy="0 0 0" />
- <joint name="base_joint" />
+ <joint name="base_joint" type="planar" />
<inertial>
<mass value="10" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
