Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[7835] pkg/trunk
|
From: <hsu...@us...> - 2008-12-09 22:21:08
|
Revision: 7835
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7835&view=rev
Author: hsujohnhsu
Date: 2008-12-09 22:21:04 +0000 (Tue, 09 Dec 2008)
Log Message:
-----------
* collisions are reported through ROS topic base_bumper for now.
* udpate bumper and ros_bumper plugin to report collision.
* implemented contact sensor and bumper controller for base as an example.
* patched gazebo to return geom name and contact geom name as well.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Bumper.hh
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Bumper.cc
pkg/trunk/drivers/simulator/gazebo_plugin/test/testcameras.py
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/base_defs.xml
Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-12-09 22:19:32 UTC (rev 7834)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-12-09 22:21:04 UTC (rev 7835)
@@ -1834,6 +1834,83 @@
Ogre::HardwarePixelBufferSharedPtr mBuffer;
std::ostringstream sstream;
Ogre::ImageCodec::ImageData *imgData;
+Index: server/sensors/contact/ContactSensor.hh
+===================================================================
+--- server/sensors/contact/ContactSensor.hh (revision 7168)
++++ server/sensors/contact/ContactSensor.hh (working copy)
+@@ -68,6 +68,12 @@
+ /// \brief Return a contact state
+ public: uint8_t GetContactState(unsigned int index) const;
+
++ /// \brief Return contact geometry name
++ public: std::string GetContactGeomName(unsigned int index) const;
++
++ /// \brief Return geometry name
++ public: std::string GetGeomName(unsigned int index) const;
++
+ /// \brief Reset the contact states
+ public: void ResetContactStates();
+
+@@ -93,9 +99,11 @@
+ /// Geom name parameter
+ private: std::vector< ParamT<std::string> *> geomNamesP;
+
++ private: std::vector<std::string> geomNames;
+ private: uint8_t *contactStates;
+ private: double *contactTimes;
+ private: unsigned int contactCount;
++ private: std::vector<std::string> contactNames;
+ };
+ /// \}
+ /// \}
+Index: server/sensors/contact/ContactSensor.cc
+===================================================================
+--- server/sensors/contact/ContactSensor.cc (revision 7168)
++++ server/sensors/contact/ContactSensor.cc (working copy)
+@@ -107,6 +107,26 @@
+ }
+
+ //////////////////////////////////////////////////////////////////////////////
++/// Return the contact geom name
++std::string ContactSensor::GetContactGeomName(unsigned int index) const
++{
++ if (index < this->contactCount)
++ return this->contactNames[index];
++
++ return std::string("");
++}
++
++//////////////////////////////////////////////////////////////////////////////
++/// Return the self geom name
++std::string ContactSensor::GetGeomName(unsigned int index) const
++{
++ if (index < this->contactCount)
++ return this->geomNamesP[index]->GetValue();
++
++ return std::string("");
++}
++
++//////////////////////////////////////////////////////////////////////////////
+ /// Reset the contact states
+ void ContactSensor::ResetContactStates()
+ {
+@@ -138,6 +158,8 @@
+ this->contactCount = this->geomNamesP.size();
+ this->contactTimes = new double[ this->contactCount ];
+ this->contactStates = new uint8_t[ this->contactCount ];
++ for (unsigned int i=0; i< this->contactCount; i++)
++ this->contactNames.push_back("");
+
+ memset(this->contactStates,0, sizeof(uint8_t) * this->contactCount);
+ memset(this->contactStates,0, sizeof(double) * this->contactCount);
+@@ -206,6 +228,7 @@
+ {
+ this->contactStates[i] = 1;
+ this->contactTimes[i] = Simulator::Instance()->GetRealTime();
++ this->contactNames[i] = **(*iter)==g1->GetName()? g2->GetName() : g1->GetName();
+ }
+ }
+
Index: server/sensors/ray/RaySensor.hh
===================================================================
--- server/sensors/ray/RaySensor.hh (revision 7168)
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-12-09 22:19:32 UTC (rev 7834)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-12-09 22:21:04 UTC (rev 7835)
@@ -19,6 +19,7 @@
rospack_add_library(F3D src/F3D.cc)
rospack_add_library(gazebo_mechanism_control src/gazebo_mechanism_control.cpp)
rospack_add_library(gazebo_battery src/gazebo_battery.cpp)
+rospack_add_library(Ros_Bumper src/Ros_Bumper.cc)
# This target requires adding player to the manifest.xml.
#rospack_add_executable(player_pr2 src/player/Pr2_Player.cc)
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Bumper.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Bumper.hh 2008-12-09 22:19:32 UTC (rev 7834)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Bumper.hh 2008-12-09 22:21:04 UTC (rev 7835)
@@ -28,9 +28,14 @@
#include <sys/time.h>
-#include "Controller.hh"
-#include "Entity.hh"
+#include <gazebo/Controller.hh>
+#include <gazebo/Entity.hh>
+#include <gazebo/Param.hh>
+// ros messages
+#include <ros/node.h>
+#include <std_msgs/String.h>
+
namespace gazebo
{
class ContactSensor;
@@ -86,7 +91,20 @@
/// The parent Model
private: ContactSensor *myParent;
-
+
+
+ /// \brief pointer to ros node
+ private: ros::node *rosnode;
+
+ /// \brief set topic name of broadcast
+ private: ParamT<std::string> *bumperTopicNameP;
+ private: std::string bumperTopicName;
+
+ /// \brief A mutex to lock access to fields that are used in message callbacks
+ private: ros::thread::mutex lock;
+
+ /// \brief broadcast some string for now.
+ private: std_msgs::String bumperMsg;
};
/** \} */
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Bumper.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Bumper.cc 2008-12-09 22:19:32 UTC (rev 7834)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Bumper.cc 2008-12-09 22:21:04 UTC (rev 7835)
@@ -24,14 +24,14 @@
* Date: 09 Setp. 2008
*/
-#include "Global.hh"
-#include "XMLConfig.hh"
+#include "gazebo/Global.hh"
+#include "gazebo/XMLConfig.hh"
#include "ContactSensor.hh"
-#include "World.hh"
-#include "gazebo.h"
-#include "GazeboError.hh"
-#include "ControllerFactory.hh"
-#include "Simulator.hh"
+#include "gazebo/World.hh"
+#include "gazebo/gazebo.h"
+#include "gazebo/GazeboError.hh"
+#include "gazebo/ControllerFactory.hh"
+#include "gazebo/Simulator.hh"
#include "gazebo_plugin/Ros_Bumper.hh"
using namespace gazebo;
@@ -47,18 +47,40 @@
if (!this->myParent)
gzthrow("Bumper controller requires a Contact Sensor as its parent");
+
+ Param::Begin(&this->parameters);
+ this->bumperTopicNameP = new ParamT<std::string>("bumperTopicName", "bumper", 0);
+ Param::End();
+
+ rosnode = ros::g_node; // comes from where?
+ int argc = 0;
+ char** argv = NULL;
+ if (rosnode == NULL)
+ {
+ // this only works for a single camera.
+ ros::init(argc,argv);
+ rosnode = new ros::node("ros_gazebo",ros::node::DONT_HANDLE_SIGINT);
+ printf("-------------------- starting node in bumper \n");
+ }
+
}
////////////////////////////////////////////////////////////////////////////////
// Destructor
Ros_Bumper::~Ros_Bumper()
{
+ delete this->bumperTopicNameP;
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void Ros_Bumper::LoadChild(XMLConfigNode *node)
{
+ this->bumperTopicNameP->Load(node);
+ this->bumperTopicName = this->bumperTopicNameP->GetValue();
+ std::cout << " publishing contact/collisions to topic name: " << this->bumperTopicName << std::endl;
+
+ rosnode->advertise<std_msgs::String>(this->bumperTopicName,100);
}
////////////////////////////////////////////////////////////////////////////////
@@ -71,6 +93,39 @@
// Update the controller
void Ros_Bumper::UpdateChild()
{
+ this->lock.lock();
+
+ unsigned int num_contacts = this->myParent->GetContactCount();
+
+ double current_time = Simulator::Instance()->GetRealTime();
+
+ std::string my_geom_name;
+ int i_hit_geom;
+ double when_i_hit;
+ std::string geom_i_hit;;
+
+ for (unsigned int i=0; i < num_contacts; i++)
+ {
+ my_geom_name = this->myParent->GetGeomName(i);
+ i_hit_geom = this->myParent->GetContactState(i);
+ when_i_hit= this->myParent->GetContactTime(i);
+ geom_i_hit = this->myParent->GetContactGeomName(i);
+ if (i_hit_geom == 1)
+ {
+ std::ostringstream stream;
+ stream << "touched! i:" << i
+ << " my geom:" << my_geom_name
+ << " other geom:" << geom_i_hit
+ << " time:" << when_i_hit << std::endl;
+ std::cout << stream;
+ this->bumperMsg.data = stream.str();
+ rosnode->publish(this->bumperTopicName,this->bumperMsg);
+ }
+ }
+
+ this->myParent->ResetContactStates();
+
+ this->lock.unlock();
}
////////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testcameras.py
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/testcameras.py 2008-12-09 22:19:32 UTC (rev 7834)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/testcameras.py 2008-12-09 22:21:04 UTC (rev 7835)
@@ -120,8 +120,8 @@
#compare thumbnails only
for i in range(len(i0d)):
- (r0,g0,b0) = i0d[i-1]
- (r1,g1,b1) = i1d[i-1]
+ (r0,g0,b0) = i0d[i]
+ (r1,g1,b1) = i1d[i]
#if abs(r0-r1) > 0 or abs(g0-g1) > 0 or abs(b0-b1) > 0:
# print "debug errors ",i,abs(r0-r1),abs(g0-g1),abs(b0-b1)
if abs(r0-r1) > pixel_tol or abs(g0-g1) > pixel_tol or abs(b0-b1) > pixel_tol:
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/base_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/base_defs.xml 2008-12-09 22:19:32 UTC (rev 7834)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/base_defs.xml 2008-12-09 22:21:04 UTC (rev 7835)
@@ -152,6 +152,20 @@
<box size="${base_size_x} ${base_size_y} ${base_size_z}" />
</geometry>
</collision>
+ <map name="bumper_sensor" flag="gazebo">
+ <verbatim key="sensor_base_bumper">
+ <sensor:contact name="${name}_contact_sensor">
+ <geom>${name}_collision</geom>
+ <updateRate>100.0</updateRate>
+ <controller:ros_bumper name="${name}_ros_bumper_controller" plugin="libRos_Bumper.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>100.0</updateRate>
+ <bumperTopicName>${name}_bumper</bumperTopicName>
+ <interface:bumper name="${name}_bumper_iface" />
+ </controller:ros_bumper>
+ </sensor:contact>
+ </verbatim>
+ </map>
</link>
@@ -212,7 +226,6 @@
<interface:laser name="ros_base_laser_iface" />
</controller:ros_laser>
</sensor:ray>
-
</verbatim>
</map>
</sensor>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
