Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[16682] pkg/trunk
|
From: <is...@us...> - 2009-06-03 23:42:59
|
Revision: 16682
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16682&view=rev
Author: isucan
Date: 2009-06-03 23:42:57 +0000 (Wed, 03 Jun 2009)
Log Message:
-----------
fixing self_watch
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/KinematicStateMonitor.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp
pkg/trunk/util/self_watch/self_watch.cpp
Modified: pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-06-03 23:39:25 UTC (rev 16681)
+++ pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-06-03 23:42:57 UTC (rev 16682)
@@ -110,7 +110,8 @@
void run(void)
{
loadRobotDescription();
- ros::spin();
+ if (loadedRobot())
+ ros::spin();
}
protected:
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/KinematicStateMonitor.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/KinematicStateMonitor.cpp 2009-06-03 23:39:25 UTC (rev 16681)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/KinematicStateMonitor.cpp 2009-06-03 23:42:57 UTC (rev 16682)
@@ -96,9 +96,11 @@
void kinematic_planning::KinematicStateMonitor::waitForState(void)
{
- ROS_INFO("Waiting for mechanism state ...");
while (m_nodeHandle.ok() && (m_haveMechanismState ^ loadedRobot()))
+ {
+ ROS_INFO("Waiting for mechanism state ...");
ros::Duration().fromSec(0.05).sleep();
+ }
ROS_INFO("Mechanism state received!");
}
@@ -174,6 +176,7 @@
void kinematic_planning::KinematicStateMonitor::mechanismStateCallback(const robot_msgs::MechanismStateConstPtr &mechanismState)
{
bool change = false;
+
if (m_robotState)
{
static bool first_time = true;
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-03 23:39:25 UTC (rev 16681)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-03 23:42:57 UTC (rev 16682)
@@ -234,17 +234,20 @@
void run(void)
{
loadRobotDescription();
+
std::vector<std::string> mlist;
knownModels(mlist);
- ROS_DEBUG("Known models:");
+ ROS_INFO("Known models:");
for (unsigned int i = 0 ; i < mlist.size() ; ++i)
- ROS_DEBUG(" * %s", mlist[i].c_str());
+ ROS_INFO(" * %s", mlist[i].c_str());
- waitForState();
startPublishingStatus();
if (mlist.size() > 0)
+ {
+ ROS_INFO("Motion planning is now available.");
ros::spin();
+ }
else
ROS_ERROR("No robot models loaded. OMPL planning node cannot start.");
}
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp 2009-06-03 23:39:25 UTC (rev 16681)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp 2009-06-03 23:42:57 UTC (rev 16682)
@@ -70,6 +70,7 @@
{
plan_id_ = -1;
robot_stopped_ = true;
+ goalA_ = true;
// we use the topic for sending commands to the controller, so we need to advertise it
jointCommandPublisher_ = m_nodeHandle.advertise<robot_msgs::JointTraj>("right_arm/trajectory_controller/trajectory_command", 1);
@@ -182,34 +183,22 @@
loadRobotDescription();
if (loadedRobot())
{
- ros::Duration d(1.0);
- d.sleep();
-
- while (m_nodeHandle.ok())
- {
- ros::Duration d(10.0);
-
- runRightArmToPositionA();
-
- d.sleep();
-
- runRightArmToPositionB();
-
- d.sleep();
- }
-
- /*
- sleep(30);
-
- plan->runRightArmToCoordinates();
- */
-
+ goalTimer_ = m_nodeHandle.createTimer(ros::Duration(10.0), &PlanKinematicPath::changeGoal, this);
ros::spin();
}
}
protected:
-
+
+ void changeGoal(const ros::TimerEvent &e)
+ {
+ if (goalA_)
+ runRightArmToPositionA();
+ else
+ runRightArmToPositionB();
+ goalA_ = !goalA_;
+ }
+
// handle new status message
void receiveStatus(const motion_planning_msgs::KinematicPlanStatusConstPtr &planStatus)
{
@@ -310,7 +299,9 @@
int plan_id_;
bool robot_stopped_;
-
+ bool goalA_;
+ ros::Timer goalTimer_;
+
ros::Publisher jointCommandPublisher_;
ros::Publisher cartesianCommandPublisher_;
ros::Publisher displayKinematicPathPublisher_;
Modified: pkg/trunk/util/self_watch/self_watch.cpp
===================================================================
--- pkg/trunk/util/self_watch/self_watch.cpp 2009-06-03 23:39:25 UTC (rev 16681)
+++ pkg/trunk/util/self_watch/self_watch.cpp 2009-06-03 23:42:57 UTC (rev 16682)
@@ -35,7 +35,7 @@
/** \author Ioan Sucan */
-#include <ros/node.h>
+#include <ros/ros.h>
#include <collision_space/environmentODE.h>
#include <robot_msgs/MechanismState.h>
@@ -43,39 +43,35 @@
{
public:
- SelfWatch(void) : m_node("self_watch")
+ SelfWatch(void)
{
setupCollisionSpace();
- m_node.subscribe("mechanism_state", m_mechanismState, &SelfWatch::mechanismStateCallback, this, 1);
+ m_mechanismStateSubscriber = m_nodeHandle.subscribe("mechanism_state", 1, &SelfWatch::mechanismStateCallback, this);
}
void run(void)
{
- m_node.spin();
+ ros::spin();
}
protected:
void setupCollisionSpace(void)
{
- m_robotState = NULL;
- m_kmodel = NULL;
- m_collisionSpace = NULL;
-
// increase the robot parts by x%, to detect collisions before they happen
- m_node.param("self_collision_scale_factor", m_scaling, 1.2);
- m_node.param("self_collision_padding", m_padding, 0.05);
+ m_nodeHandle.param("self_collision_scale_factor", m_scaling, 1.2);
+ m_nodeHandle.param("self_collision_padding", m_padding, 0.05);
// load the string description of the robot
std::string content;
- if (m_node.getParam("robot_description", content))
+ if (m_nodeHandle.getParam("robot_description", content))
{
// parse the description
robot_desc::URDF *file = new robot_desc::URDF();
if (file->loadString(content.c_str()))
{
// create a kinematic model out of the parsed description
- m_kmodel = new planning_models::KinematicModel();
+ m_kmodel = boost::shared_ptr<planning_models::KinematicModel>(new planning_models::KinematicModel());
m_kmodel->setVerbose(false);
m_kmodel->build(*file);
@@ -86,7 +82,7 @@
// create a state that can be used to monitor the
// changes in the joints of the kinematic model
- m_robotState = m_kmodel->newStateParams();
+ m_robotState = boost::shared_ptr<planning_models::KinematicModel::StateParams>(m_kmodel->newStateParams());
// make sure the transforms caused by the planar and
// floating joints are identity, to be compatible with
@@ -95,17 +91,17 @@
m_robotState->setInRobotFrame();
// create a new collision space
- m_collisionSpace = new collision_space::EnvironmentModelODE();
+ m_collisionSpace = boost::shared_ptr<collision_space::EnvironmentModel>(new collision_space::EnvironmentModelODE());
// enable self collision checking (just in case default is disabled)
m_collisionSpace->setSelfCollision(true);
-
+
// get the list of links that are enabled for collision checking
std::vector<std::string> links;
robot_desc::URDF::Group *g = file->getGroup("collision_check");
if (g && g->hasFlag("collision"))
links = g->linkNames;
-
+
// print some info, just to easily double-check the loaded data
if (links.empty())
ROS_WARN("No links have been enabled for collision checking");
@@ -136,7 +132,7 @@
if (nscgroups == 0)
ROS_WARN("No self-collision checking enabled");
-
+
ROS_INFO("Self-collision monitor is active, with scaling %g, padding %g", m_scaling, m_padding);
}
else
@@ -146,29 +142,34 @@
ROS_ERROR("Could not load robot description");
}
- void mechanismStateCallback(void)
+ void mechanismStateCallback(const robot_msgs::MechanismStateConstPtr &mechanismState)
{
bool change = false;
+
if (m_robotState)
{
- unsigned int n = m_mechanismState.get_joint_states_size();
+ static bool firstTime = true;
+ unsigned int n = mechanismState->get_joint_states_size();
for (unsigned int i = 0 ; i < n ; ++i)
{
- planning_models::KinematicModel::Joint* joint = m_kmodel->getJoint(m_mechanismState.joint_states[i].name);
+ planning_models::KinematicModel::Joint* joint = m_kmodel->getJoint(mechanismState->joint_states[i].name);
if (joint)
{
if (joint->usedParams == 1)
{
- double pos = m_mechanismState.joint_states[i].position;
- bool this_changed = m_robotState->setParamsJoint(&pos, m_mechanismState.joint_states[i].name);
+ double pos = mechanismState->joint_states[i].position;
+ bool this_changed = m_robotState->setParamsJoint(&pos, mechanismState->joint_states[i].name);
change = change || this_changed;
}
- // else
- // ROS_WARN("Incorrect number of parameters: %s (expected %d, had 1)", m_mechanismState.joint_states[i].name.c_str(), joint->usedParams);
+ else
+ if (firstTime)
+ ROS_WARN("Incorrect number of parameters: %s (expected %d, had 1)", mechanismState->joint_states[i].name.c_str(), joint->usedParams);
}
else
- ROS_ERROR("Unknown joint: %s", m_mechanismState.joint_states[i].name.c_str());
+ if (firstTime)
+ ROS_ERROR("Unknown joint: %s", mechanismState->joint_states[i].name.c_str());
}
+ firstTime = false;
}
if (change)
stateUpdate();
@@ -204,32 +205,30 @@
}
}
- ros::Node m_node;
-
+ ros::NodeHandle m_nodeHandle;
+ ros::Subscriber m_mechanismStateSubscriber;
+
// we don't want to detect a collision after it happened, but this
// is what collision checkers do, so we scale the robot up by a
// small factor; when a collision is found between the inflated
// parts, the robot should take action to preserve itself
- double m_scaling;
- double m_padding;
+ double m_scaling;
+ double m_padding;
// the complete robot state
- planning_models::KinematicModel::StateParams *m_robotState;
+ boost::shared_ptr<planning_models::KinematicModel::StateParams> m_robotState;
// the kinematic model
- planning_models::KinematicModel *m_kmodel;
+ boost::shared_ptr<planning_models::KinematicModel> m_kmodel;
// the collision space
- collision_space::EnvironmentModel *m_collisionSpace;
-
-
- robot_msgs::MechanismState m_mechanismState;
+ boost::shared_ptr<collision_space::EnvironmentModel> m_collisionSpace;
};
int main(int argc, char **argv)
{
- ros::init(argc, argv);
+ ros::init(argc, argv, "self_watch");
SelfWatch sw;
sw.run();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
