|
From: <ge...@us...> - 2009-01-24 02:44:29
|
Revision: 10117
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10117&view=rev
Author: gerkey
Date: 2009-01-24 02:44:23 +0000 (Sat, 24 Jan 2009)
Log Message:
-----------
Working on a new version of move_arm, and hacking on a Python executive for
a table-object manipulation experiment.
Modified Paths:
--------------
pkg/trunk/highlevel/executive_python/src/exec_table_object_manip.py
pkg/trunk/highlevel/executive_python/src/executive.py
pkg/trunk/highlevel/executive_python/src/navigation_adapter.py
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/ArmControllerAdapter.cc
pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt
pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg
pkg/trunk/pr2_msgs/msg/MoveArmState.msg
Modified: pkg/trunk/highlevel/executive_python/src/exec_table_object_manip.py
===================================================================
--- pkg/trunk/highlevel/executive_python/src/exec_table_object_manip.py 2009-01-24 02:30:21 UTC (rev 10116)
+++ pkg/trunk/highlevel/executive_python/src/exec_table_object_manip.py 2009-01-24 02:44:23 UTC (rev 10117)
@@ -128,6 +128,9 @@
from robot_msgs.msg import *
from highlevel_controllers.msg import *
from navigation_adapter import *
+#from movearm_adapter import *
+#from tiltlaser_adapter import *
+#from gripper_adapter import *
class Executive:
def __init__(self, goals, navigator, cycle_time):
@@ -147,10 +150,10 @@
if self.state == "idle":
if self.navigator.goalReached() or (not self.navigator.active() and self.current_goal == None) or self.navigator.timeUp():
self.current_goal = self.goals[random.randint(0, len(self.goals) - 1)]
- self.navigator.sendGoal(self.current_goal)
+ self.navigator.sendGoal(self.current_goal, "odom")
print "nav --> nav"
elif not self.navigator.active() and self.current_goal != None:
- self.navigator.sendGoal(self.current_goal)
+ self.navigator.sendGoal(self.current_goal, "odom")
print "nav --> nav"
else:
if not self.navigator.legalState():
Modified: pkg/trunk/highlevel/executive_python/src/executive.py
===================================================================
--- pkg/trunk/highlevel/executive_python/src/executive.py 2009-01-24 02:30:21 UTC (rev 10116)
+++ pkg/trunk/highlevel/executive_python/src/executive.py 2009-01-24 02:44:23 UTC (rev 10117)
@@ -77,15 +77,15 @@
print "nav --> recharge"
elif self.batt_monitor.chargeNeeded():
chrg_pts = self.chrg_stations[random.randint(0, len(self.chrg_stations) - 1)]
- self.navigator.sendGoal(chrg_pts)
+ self.navigator.sendGoal(chrg_pts, "map")
self.state = "nav_charge"
print "nav --> nav_charge"
elif self.navigator.goalReached() or (not self.navigator.active() and self.current_goal == None) or self.navigator.timeUp():
self.current_goal = self.goals[random.randint(0, len(self.goals) - 1)]
- self.navigator.sendGoal(self.current_goal)
+ self.navigator.sendGoal(self.current_goal, "map")
print "nav --> nav"
elif not self.navigator.active() and self.current_goal != None:
- self.navigator.sendGoal(self.current_goal)
+ self.navigator.sendGoal(self.current_goal, "map")
print "nav --> nav"
elif self.state == "recharge":
"""
@@ -94,7 +94,7 @@
"""
if self.recharger.doneCharging():
#resume the current goal
- self.navigator.sendGoal(self.current_goal)
+ self.navigator.sendGoal(self.current_goal, "map")
self.state = "nav"
print "recharge --> nav"
elif self.state == "nav_charge":
@@ -109,7 +109,7 @@
print "nav_charge --> recharge"
elif not self.navigator.active() or self.navigator.timeUp():
chrg_pts = self.chrg_stations[random.randint(0, len(self.chrg_stations) - 1)]
- self.navigator.sendGoal(chrg_pts)
+ self.navigator.sendGoal(chrg_pts, "map")
print "nav_charge --> nav_charge"
else:
if not self.navigator.legalState():
Modified: pkg/trunk/highlevel/executive_python/src/navigation_adapter.py
===================================================================
--- pkg/trunk/highlevel/executive_python/src/navigation_adapter.py 2009-01-24 02:30:21 UTC (rev 10116)
+++ pkg/trunk/highlevel/executive_python/src/navigation_adapter.py 2009-01-24 02:44:23 UTC (rev 10117)
@@ -76,10 +76,10 @@
return [self.state.goal.x, self.state.goal.y, self.state.goal.th]
#Send a new goal to the move base node
- def sendGoal(self, goal_pts):
+ def sendGoal(self, goal_pts, frame):
self.start_time = rospy.get_time()
goal = Planner2DGoal()
- goal.header.frame_id = "map"
+ goal.header.frame_id = frame
goal.goal.x = goal_pts[0]
goal.goal.y = goal_pts[1]
goal.goal.th = goal_pts[2]
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/ArmControllerAdapter.cc
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/ArmControllerAdapter.cc 2009-01-24 02:30:21 UTC (rev 10116)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/ArmControllerAdapter.cc 2009-01-24 02:44:23 UTC (rev 10117)
@@ -18,28 +18,40 @@
protected:
void fillActiveObservationParameters(ObservationByValue* obs){
+ ROS_FATAL("Disabled, pending update to new MoveArm interface");
+ ROS_BREAK();
+ /*
for(unsigned int i = 0; i < stateMsg.get_goal_size(); i++){
unsigned int ind;
if(rosIndex(stateMsg.goal[i].name, ind))
obs->push_back(nddlNames()[ind], new IntervalDomain(stateMsg.goal[i].position));
}
+ */
}
void fillInactiveObservationParameters(ObservationByValue* obs){
+ ROS_FATAL("Disabled, pending update to new MoveArm interface");
+ ROS_BREAK();
+ /*
for(unsigned int i = 0; i < stateMsg.get_configuration_size(); i++){
unsigned int ind;
if(rosIndex(stateMsg.configuration[i].name, ind))
obs->push_back(nddlNames()[ind], new IntervalDomain(stateMsg.configuration[i].position));
}
+ */
}
void fillRequestParameters(pr2_msgs::MoveArmGoal& goalMsg, const TokenId& goalToken){
+ ROS_FATAL("Disabled, pending update to new MoveArm interface");
+ ROS_BREAK();
+ /*
goalMsg.set_configuration_size(nddlNames().size());
for(unsigned int i = 0; i<nddlNames().size(); i++){
const IntervalDomain& dom = goalToken->getVariable(nddlNames()[i])->lastDomain();
goalMsg.configuration[i].name = rosNames()[i];
goalMsg.configuration[i].position = (dom.isSingleton() ? dom.getSingletonValue() : (dom.getUpperBound() + dom.getLowerBound()) / 2);
}
+ */
}
};
Modified: pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt 2009-01-24 02:30:21 UTC (rev 10116)
+++ pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt 2009-01-24 02:44:23 UTC (rev 10117)
@@ -3,7 +3,7 @@
### If you're trying to debug include directives or so, this here
### might help:
#SET (CMAKE_VERBOSE_MAKEFILE ON)
-set(ROS_BUILD_TYPE Release)
+set(ROS_BUILD_TYPE Debug)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
add_definitions(-Wall)
rospack(highlevel_controllers)
@@ -15,7 +15,9 @@
target_link_libraries(highlevel_controllers trajectory_rollout costmap_2d)
# MoveArm
-rospack_add_executable(move_arm src/move_arm.cpp)
+# Eventually move_arm2 will replace move_arm
+#rospack_add_executable(move_arm src/move_arm.cpp)
+rospack_add_executable(move_arm2 src/move_arm2.cpp)
# move_base_sbpl
rospack_add_executable(move_base_sbpl src/move_base_sbpl.cpp)
@@ -39,4 +41,5 @@
rospack_add_executable(recharge_controller src/recharge_controller.cpp)
#CLI
-rospack_add_executable(cli src/control_cli.cpp)
+# Disabled, pending update to new message formats
+#rospack_add_executable(cli src/control_cli.cpp)
Modified: pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg 2009-01-24 02:30:21 UTC (rev 10116)
+++ pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg 2009-01-24 02:44:23 UTC (rev 10117)
@@ -1,7 +1,13 @@
Header header
-# The desired joint configuratin
-robot_msgs/JointState[] configuration
+# Only one of goal_state or goal_constraints is used. implicit_goal ==
+# 1 means use goal_constraints, 0 means use goal_state
+byte implicit_goal
+# The desired joint state
+robot_msgs/KinematicState goal_state
+# Goal constraints (implicit definition of goal)
+robot_msgs/PoseConstraint[] goal_constraints
+
# Indicate if the goal is being enabled or disabled
byte enable
Modified: pkg/trunk/pr2_msgs/msg/MoveArmState.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveArmState.msg 2009-01-24 02:30:21 UTC (rev 10116)
+++ pkg/trunk/pr2_msgs/msg/MoveArmState.msg 2009-01-24 02:44:23 UTC (rev 10117)
@@ -10,6 +10,11 @@
#Was the planner told to stop?
byte preempted
#Current arm configuration
-robot_msgs/JointState[] configuration
-#Goal arm configuration
-robot_msgs/JointState[] goal
+robot_msgs/KinematicState configuration
+# Only one of goal_state or goal_constraints is used. implicit_goal ==
+# 1 means use goal_constraints, 0 means use goal_state
+byte implicit_goal
+# The desired joint state
+robot_msgs/KinematicState goal_state
+# Goal constraints (implicit definition of goal)
+robot_msgs/PoseConstraint[] goal_constraints
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-01-24 03:01:25
|
Revision: 10123
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10123&view=rev
Author: isucan
Date: 2009-01-24 03:01:21 +0000 (Sat, 24 Jan 2009)
Log Message:
-----------
new message/service structure for motion planning
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestState.h
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/kinematic_planning/test/avoid_monster.cpp
pkg/trunk/prcore/robot_srvs/srv/KinematicPlanLinkPosition.srv
pkg/trunk/prcore/robot_srvs/srv/KinematicPlanState.srv
Added Paths:
-----------
pkg/trunk/prcore/robot_msgs/msg/KinematicPlanLinkPositionRequest.msg
pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStateRequest.msg
pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStatus.msg
pkg/trunk/prcore/robot_srvs/srv/KinematicReplanLinkPosition.srv
pkg/trunk/prcore/robot_srvs/srv/KinematicReplanState.srv
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-01-24 02:58:23 UTC (rev 10122)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-01-24 03:01:21 UTC (rev 10123)
@@ -39,6 +39,7 @@
#include "kinematic_planning/RKPBasicRequest.h"
#include <robot_srvs/KinematicPlanLinkPosition.h>
+#include <robot_srvs/KinematicReplanLinkPosition.h>
namespace kinematic_planning
{
@@ -123,7 +124,7 @@
/** Validate request for planning towards a link position */
template<>
- bool RKPBasicRequest<robot_srvs::KinematicPlanLinkPosition::request>::isRequestValid(ModelMap &models, robot_srvs::KinematicPlanLinkPosition::request &req)
+ bool RKPBasicRequest<robot_msgs::KinematicPlanLinkPositionRequest>::isRequestValid(ModelMap &models, robot_msgs::KinematicPlanLinkPositionRequest &req)
{
if (!areSpaceParamsValid(models, req.params))
return false;
@@ -141,7 +142,7 @@
/** Set the goal using a destination link position */
template<>
- void RKPBasicRequest<robot_srvs::KinematicPlanLinkPosition::request>::setupGoalState(RKPModel *model, RKPPlannerSetup *psetup, robot_srvs::KinematicPlanLinkPosition::request &req)
+ void RKPBasicRequest<robot_msgs::KinematicPlanLinkPositionRequest>::setupGoalState(RKPModel *model, RKPPlannerSetup *psetup, robot_msgs::KinematicPlanLinkPositionRequest &req)
{
/* set the goal */
std::vector<robot_msgs::PoseConstraint> pc;
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestState.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestState.h 2009-01-24 02:58:23 UTC (rev 10122)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestState.h 2009-01-24 03:01:21 UTC (rev 10123)
@@ -39,13 +39,14 @@
#include "kinematic_planning/RKPBasicRequest.h"
#include <robot_srvs/KinematicPlanState.h>
+#include <robot_srvs/KinematicReplanState.h>
namespace kinematic_planning
{
/** Validate request for planning towards a state */
template<>
- bool RKPBasicRequest<robot_srvs::KinematicPlanState::request>::isRequestValid(ModelMap &models, robot_srvs::KinematicPlanState::request &req)
+ bool RKPBasicRequest<robot_msgs::KinematicPlanStateRequest>::isRequestValid(ModelMap &models, robot_msgs::KinematicPlanStateRequest &req)
{
if (!areSpaceParamsValid(models, req.params))
return false;
@@ -70,7 +71,7 @@
/** Set the goal using a destination state */
template<>
- void RKPBasicRequest<robot_srvs::KinematicPlanState::request>::setupGoalState(RKPModel *model, RKPPlannerSetup *psetup, robot_srvs::KinematicPlanState::request &req)
+ void RKPBasicRequest<robot_msgs::KinematicPlanStateRequest>::setupGoalState(RKPModel *model, RKPPlannerSetup *psetup, robot_msgs::KinematicPlanStateRequest &req)
{
/* set the goal */
ompl::SpaceInformationKinematic::GoalStateKinematic_t goal = new ompl::SpaceInformationKinematic::GoalStateKinematic(psetup->si);
Modified: pkg/trunk/motion_planning/kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-01-24 02:58:23 UTC (rev 10122)
+++ pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-01-24 03:01:21 UTC (rev 10123)
@@ -9,6 +9,7 @@
<depend package="boost" />
<depend package="std_msgs" />
+ <depend package="std_srvs" />
<depend package="robot_msgs" />
<depend package="robot_srvs" />
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-01-24 02:58:23 UTC (rev 10122)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-01-24 03:01:21 UTC (rev 10123)
@@ -101,10 +101,10 @@
@section topic ROS topics
Subscribes to (name/type):
-- @b "replan_kinematic_path_state"/KinematicPlanState::request : given a robot model, starting and goal states, this service computes and recomputes a collision free path until the monitored state is actually at the goal or stopping is requested. Changes in the collision model trigger replanning.
+- @b "replan_kinematic_path_state"/KinematicPlanStateRequest : given a robot model, starting and goal states, this service computes and recomputes a collision free path until the monitored state is actually at the goal or stopping is requested. Changes in the collision model trigger replanning.
-- @b "replan_kinematic_path_position"/KinematicPlanState::request : given a robot model, starting state and goal poses of certain links, this service computes a collision free path until the monitored state is actually at the goal or stopping is requested. Changes in the collision model trigger replanning.
+- @b "replan_kinematic_path_position"/KinematicPlanStateRequest : given a robot model, starting state and goal poses of certain links, this service computes a collision free path until the monitored state is actually at the goal or stopping is requested. Changes in the collision model trigger replanning.
- @b "replan_stop"/Empty : signal the planner to stop replanning
@@ -142,10 +142,7 @@
#include <boost/thread/mutex.hpp>
#include <boost/thread/condition.hpp>
-#include <std_msgs/Empty.h>
-#include <std_msgs/String.h>
-#include <robot_srvs/PlanNames.h>
-#include <robot_srvs/NamedKinematicPlanState.h>
+#include <std_srvs/Empty.h>
using namespace kinematic_planning;
@@ -158,22 +155,24 @@
CollisionSpaceMonitor(dynamic_cast<ros::Node*>(this),
robot_model)
{
- advertiseService("plan_kinematic_path_state", &KinematicPlanning::planToState);
+ advertiseService("plan_kinematic_path_state", &KinematicPlanning::planToState);
advertiseService("plan_kinematic_path_position", &KinematicPlanning::planToPosition);
- // to be removed
- advertiseService("plan_kinematic_path_named", &KinematicPlanning::planToStateNamed);
- advertiseService("plan_joint_state_names", &KinematicPlanning::planJointNames);
-
-
+ m_replanID = 0;
m_replanning = false;
m_replanningThread = NULL;
m_collisionMonitorChange = false;
-
- advertise<robot_msgs::KinematicPath>("path_to_goal", 1);
- subscribe("replan_kinematic_path_state", m_planToStateRequest, &KinematicPlanning::planToStateRequest, this, 1);
- subscribe("replan_kinematic_path_position", m_planToPositionRequest, &KinematicPlanning::planToPositionRequest, this, 1);
- subscribe("replan_stop", m_replanStop, &KinematicPlanning::stopReplanning, this, 1);
+
+ m_currentPlanStatus.id = -1;
+ m_currentPlanStatus.distance = -1.0;
+ m_currentPlanStatus.done = 1;
+ m_currentPlanStatus.valid = 1;
+
+ advertiseService("replan_kinematic_path_state", &KinematicPlanning::replanToState);
+ advertiseService("replan_kinematic_path_position", &KinematicPlanning::replanToPosition);
+ advertiseService("replan_stop", &KinematicPlanning::stopReplanning);
+
+ advertise<robot_msgs::KinematicPlanStatus>("kinematic_planning_status", 1);
}
/** Free the memory */
@@ -186,6 +185,13 @@
void stopReplanning(void)
{
+ std_srvs::Empty::request dummy1;
+ std_srvs::Empty::response dummy2;
+ stopReplanning(dummy1, dummy2);
+ }
+
+ bool stopReplanning(std_srvs::Empty::request &req, std_srvs::Empty::response &res)
+ {
m_replanningLock.lock();
bool stop = false;
m_continueReplanningLock.lock();
@@ -209,18 +215,21 @@
m_replanningLock.unlock();
ROS_INFO("Replanning stopped");
+ return true;
}
- void planToStateRequest(void)
+ bool replanToState(robot_srvs::KinematicReplanState::request &req, robot_srvs::KinematicReplanState::response &res)
{
ROS_INFO("Request for replanning to a state");
+ bool st = false;
+ res.id = -1;
stopReplanning();
if (m_robotState)
{
// back up the request
- m_currentPlanToStateRequest = m_planToStateRequest;
+ m_currentPlanToStateRequest = req.value;
// allocate memory for starting state, if needed
m_currentPlanToStateRequest.start_state.set_vals_size(m_kmodel->stateDimension);
@@ -228,23 +237,36 @@
// start planning thread
m_replanningLock.lock();
m_replanning = true;
- m_replanningThread = new boost::thread(boost::bind(&KinematicPlanning::replanToState, this));
+
+ m_currentPlanStatus.id = ++m_replanID;
+ m_currentPlanStatus.valid = 1;
+ m_currentPlanStatus.path.set_states_size(0);
+ m_currentPlanStatus.done = 0;
+ m_currentPlanStatus.distance = -1.0;
+ res.id = m_currentPlanStatus.id;
+
+ m_replanningThread = new boost::thread(boost::bind(&KinematicPlanning::replanToStateThread, this));
m_replanningLock.unlock();
+ st = true;
}
else
ROS_ERROR("Current robot state is unknown. Cannot start replanning.");
+
+ return st;
}
- void planToPositionRequest(void)
+ bool replanToPosition(robot_srvs::KinematicReplanLinkPosition::request &req, robot_srvs::KinematicReplanLinkPosition::response &res)
{
ROS_INFO("Request for replanning to a position");
-
+ bool st = false;
+ res.id = -1;
+
stopReplanning();
if (m_robotState)
{
// back up the request
- m_currentPlanToPositionRequest = m_planToPositionRequest;
+ m_currentPlanToPositionRequest = req.value;
// allocate memory for starting state, if needed
m_currentPlanToPositionRequest.start_state.set_vals_size(m_kmodel->stateDimension);
@@ -252,15 +274,37 @@
// start planning thread
m_replanningLock.lock();
m_replanning = true;
- m_replanningThread = new boost::thread(boost::bind(&KinematicPlanning::replanToPosition, this));
- m_replanningLock.unlock();
+
+ m_currentPlanStatus.id = ++m_replanID;
+ m_currentPlanStatus.valid = 1;
+ m_currentPlanStatus.path.set_states_size(0);
+ m_currentPlanStatus.done = 0;
+ m_currentPlanStatus.distance = -1.0;
+ res.id = m_currentPlanStatus.id;
+
+ m_replanningThread = new boost::thread(boost::bind(&KinematicPlanning::replanToPositionThread, this));
+ m_replanningLock.unlock();
+ st = true;
}
else
ROS_ERROR("Current robot state is unknown. Cannot start replanning.");
+
+ return st;
}
+ bool planToState(robot_srvs::KinematicPlanState::request &req, robot_srvs::KinematicPlanState::response &res)
+ {
+ ROS_INFO("Request for planning to a state");
+ bool trivial = false;
+ bool result = m_requestState.execute(m_models, req.value, res.value.path, res.value.distance, trivial);
+ res.value.id = -1;
+ res.value.done = trivial ? 1 : 0;
+ res.value.valid = res.value.path.get_states_size() > 0;
+ return result;
+ }
+
/** Wait for a change in the environment and recompute the motion plan */
- void replanToState(void)
+ void replanToStateThread(void)
{
robot_msgs::KinematicPath solution;
unsigned int step = 0;
@@ -277,7 +321,12 @@
for (unsigned int i = 0 ; i < m_kmodel->stateDimension ; ++i)
m_currentPlanToStateRequest.start_state.vals[i] = start_state[i];
m_requestState.execute(m_models, m_currentPlanToStateRequest, solution, distance, trivial);
- publish("path_to_goal", solution);
+
+ m_currentPlanStatus.path = solution;
+ m_currentPlanStatus.distance = distance;
+ m_currentPlanStatus.done = trivial ? 1 : 0;
+ m_currentPlanStatus.valid = solution.get_states_size() > 0 ? 1 : 0;
+
if (trivial)
break;
while (!m_collisionMonitorChange)
@@ -285,8 +334,19 @@
}
}
+ bool planToPosition(robot_srvs::KinematicPlanLinkPosition::request &req, robot_srvs::KinematicPlanLinkPosition::response &res)
+ {
+ ROS_INFO("Request for planning to a position");
+ bool trivial = false;
+ bool result = m_requestLinkPosition.execute(m_models, req.value, res.value.path, res.value.distance, trivial);
+ res.value.id = -1;
+ res.value.done = trivial ? 1 : 0;
+ res.value.valid = res.value.path.get_states_size() > 0;
+ return result;
+ }
+
/** Wait for a change in the environment and recompute the motion plan */
- void replanToPosition(void)
+ void replanToPositionThread(void)
{
robot_msgs::KinematicPath solution;
unsigned int step = 0;
@@ -320,200 +380,9 @@
m_continueReplanningLock.lock();
m_collisionMonitorChange = true;
m_continueReplanningLock.unlock();
- ROS_INFO("Notifying replanning threads");
m_collisionMonitorCondition.notify_all();
}
- bool planToStateNamed(robot_srvs::NamedKinematicPlanState::request &reqn, robot_srvs::NamedKinematicPlanState::response &resn)
- {
- robot_srvs::KinematicPlanState::request req;
- robot_srvs::KinematicPlanState::response res;
-
-
-
-
- //Use name lookups to create the state vector.
- //Find the number of parameters needed and print errors.
- unsigned int nparam = 0;
- for (unsigned int i = 0; i < reqn.start_state.get_joints_size(); i++) {
- if (!m_kmodel->getJoint(reqn.start_state.names[i])) {
- std::cerr << "Non-existant joint ignored: " << reqn.start_state.names[i] << std::endl;
- continue;
- }
- nparam += m_kmodel->getJoint(reqn.start_state.names[i])->usedParams;
- if (reqn.start_state.joints[i].get_vals_size() != m_kmodel->getJoint(reqn.start_state.names[i])->usedParams) {
- std::cerr << "You do not have the right number of axes for a the joint: "
- << reqn.start_state.names[i] << " (" << reqn.start_state.joints[i].get_vals_size()
- << " != " << m_kmodel->getJoint(reqn.start_state.names[i])->usedParams << "). "
- << "Extra axes will be ignored, non-specified ones will be set to zero.\n";
-
- }
- }
- //std::cout << "Name set up\nNeed " << nparam << " parameters.\n";
- //Create the state vector and set it to zero.
- req.start_state.set_vals_size(nparam);
- for (unsigned int i = 0; i < req.start_state.get_vals_size(); i++) {
- req.start_state.vals[i] = 0;
- }
- //Iterate over all the joints and populate the state vector.
- for (unsigned int i = 0; i < reqn.start_state.get_joints_size(); i++) {
- if (m_kmodel->parameterNames.find(reqn.start_state.names[i]) == m_kmodel->parameterNames.end()) {
- continue;
- }
- unsigned int p = m_kmodel->parameterNames.find(reqn.start_state.names[i])->second;
-
- for (unsigned int k = 0; k < m_kmodel->getJoint(reqn.start_state.names[i])->usedParams; k++) {
- req.start_state.vals[p + k] = reqn.start_state.joints[i].vals[k];
- }
-
- //std::cout << reqn.start_state.names[i] << " [" << p << "-"
- // << p + m_kmodel->getJoint(reqn.start_state.names[i])->usedParams - 1 << "]\n";
- }
-
-
-
- //Use name lookups to populate the joint group.
-
- unsigned int group = m_kmodel->getGroupID(reqn.params.model_id);
-
-
- //std::cout << "Joints:\n";
-
- //Create a vector with the joint map for the planning subspace.
- std::vector<planning_models::KinematicModel::Joint*> subspaceJointMapSparse;
- std::vector<planning_models::KinematicModel::Joint*> subspaceJointMap;
-
- //Fill it with nulls.
- for (unsigned int i = 0; i < req.start_state.get_vals_size(); i++) {
- subspaceJointMapSparse.push_back(NULL);
- }
-
- //Iterate over the joint parameter names table, and fill up the subspace joint
- //map with the joints in the target group.
- for (std::map<std::string, unsigned int>::iterator it = m_kmodel->parameterNames.begin();
- it != m_kmodel->parameterNames.end(); it++) {
- if (m_kmodel->getJoint(it->first)->inGroup[group]) {
- subspaceJointMapSparse[it->second] = m_kmodel->getJoint(it->first);
- }
- }
-
- //Create a non-sparse subspace joint map, and get nparam.
- nparam = 0;
- for (unsigned int i = 0; i < subspaceJointMapSparse.size(); i++) {
- if (subspaceJointMapSparse[i]) {
- subspaceJointMap.push_back(subspaceJointMapSparse[i]);
- nparam += subspaceJointMapSparse[i]->usedParams;
- std::cout << subspaceJointMapSparse[i]->name << std::endl;
- }
- }
-
-
-
- //Copy the goal state.
-
- //Zero the output messages.
- req.goal_state.set_vals_size(nparam);
- for (unsigned int i = 0; i < nparam; i++) {
- req.goal_state.vals[i] = 0.0;
- }
-
- //Copy the input into the message using the jointmap.
- nparam = 0;
- for (unsigned int i = 0; i < subspaceJointMap.size(); i++) {
- bool found = false;
- for (unsigned int j = 0; j < reqn.goal_state.get_names_size() && j < reqn.goal_state.get_joints_size(); j++) {
- if (reqn.goal_state.names[j] == subspaceJointMap[i]->name) {
- if (subspaceJointMap[i]->usedParams != reqn.goal_state.joints[j].get_vals_size()) {
- std::cerr << "You do not have the right number of axes for a the joint: "
- << subspaceJointMap[i]->name << " (" << subspaceJointMap[i]->usedParams
- << " != " << reqn.goal_state.joints[j].get_vals_size() << "). "
- << "Extra axes will be ignored, non-specified ones will be set to zero.\n";
- }
- for (unsigned int k = 0; k < subspaceJointMap[i]->usedParams && k < reqn.goal_state.joints[j].get_vals_size(); k++) {
- std::cout << "Writing " << nparam << " for " << subspaceJointMap[i]->name << std::endl;
- req.goal_state.vals[nparam] = reqn.goal_state.joints[j].vals[k];
- nparam++;
- }
- found = true;
- break;
- }
- }
- if (!found) {
- std::cerr << "You did not specify a goal value for the joint:" << subspaceJointMap[i]->name << std::endl;
- }
- }
-
-
- //Blind copy for the rest.
- req.params = reqn.params;
- req.constraints = reqn.constraints;
- req.times = reqn.times;
- req.interpolate = reqn.interpolate;
- req.allowed_time = reqn.allowed_time;
- req.threshold = reqn.threshold;
-
-
-
- //Acutally run the service.
- bool trivial = false;
- bool r = m_requestState.execute(m_models, req, res.path, res.distance, trivial);
-
-
- //Copy the path.
- resn.path.set_states_size(res.path.get_states_size());
-
- for (unsigned int j = 0; j < resn.path.get_states_size(); j++) {
- nparam = 0;
- resn.path.states[j].set_names_size(subspaceJointMap.size());
- resn.path.states[j].set_joints_size(subspaceJointMap.size());
- for (unsigned int i = 0; i < subspaceJointMap.size(); i++) {
- resn.path.states[j].names[i] = subspaceJointMap[i]->name;
- resn.path.states[j].joints[i].set_vals_size(subspaceJointMap[i]->usedParams);
- for (unsigned int k = 0; k < subspaceJointMap[i]->usedParams; k++) {
- resn.path.states[j].joints[i].vals[k] = res.path.states[j].vals[nparam];
- nparam++;
- }
- }
- }
-
-
-
- //Simply copy the other results.
- resn.distance = res.distance;
- return r;
- }
-
- bool planJointNames(robot_srvs::PlanNames::request &req, robot_srvs::PlanNames::response &res) {
- std::vector<planning_models::KinematicModel::Joint*> joints;
- m_kmodel->getJoints(joints);
- res.set_names_size(joints.size());
- res.set_num_values_size(joints.size());
- for (unsigned int i = 0; i < joints.size(); i++) {
- res.names[i] = joints[i]->name;
- res.num_values[i] = joints[i]->usedParams;
- }
- return true;
- }
-
-
- bool planToState(robot_srvs::KinematicPlanState::request &req, robot_srvs::KinematicPlanState::response &res)
- {
- ROS_INFO("Request for planning to a state");
- bool trivial = false;
- bool result = m_requestState.execute(m_models, req, res.path, res.distance, trivial);
- res.trivial = trivial ? 1 : 0;
- return result;
- }
-
- bool planToPosition(robot_srvs::KinematicPlanLinkPosition::request &req, robot_srvs::KinematicPlanLinkPosition::response &res)
- {
- ROS_INFO("Request for planning to a position");
- bool trivial = false;
- bool result = m_requestLinkPosition.execute(m_models, req, res.path, res.distance, trivial);
- res.trivial = trivial ? 1 : 0;
- return result;
- }
-
virtual void setRobotDescription(robot_desc::URDF *file)
{
CollisionSpaceMonitor::setRobotDescription(file);
@@ -604,18 +473,18 @@
}
ModelMap m_models;
- RKPBasicRequest<robot_srvs::KinematicPlanState::request> m_requestState;
- RKPBasicRequest<robot_srvs::KinematicPlanLinkPosition::request> m_requestLinkPosition;
+ RKPBasicRequest<robot_msgs::KinematicPlanStateRequest> m_requestState;
+ RKPBasicRequest<robot_msgs::KinematicPlanLinkPositionRequest> m_requestLinkPosition;
- // received request for replanning
- robot_srvs::KinematicPlanState::request m_planToStateRequest;
- robot_srvs::KinematicPlanLinkPosition::request m_planToPositionRequest;
- // flag received for stopping replanning
- std_msgs::Empty m_replanStop;
-
// currently considered request
- robot_srvs::KinematicPlanState::request m_currentPlanToStateRequest;
- robot_srvs::KinematicPlanLinkPosition::request m_currentPlanToPositionRequest;
+ robot_msgs::KinematicPlanStateRequest m_currentPlanToStateRequest;
+ robot_msgs::KinematicPlanLinkPositionRequest m_currentPlanToPositionRequest;
+
+ // current status of the motion planner
+ robot_msgs::KinematicPlanStatus m_currentPlanStatus;
+
+ // the ID of the current replanning task
+ int m_replanID;
// flag indicating whether a replanning thread is active
bool m_replanning;
Modified: pkg/trunk/motion_planning/kinematic_planning/test/avoid_monster.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/test/avoid_monster.cpp 2009-01-24 02:58:23 UTC (rev 10122)
+++ pkg/trunk/motion_planning/kinematic_planning/test/avoid_monster.cpp 2009-01-24 03:01:21 UTC (rev 10123)
@@ -60,7 +60,7 @@
void runTestBase(void)
{
- robot_srvs::KinematicPlanState::request req;
+ robot_msgs::KinematicPlanStateRequest req;
req.params.model_id = "pr2::base";
req.params.distance_metric = "L2Square";
@@ -81,8 +81,11 @@
req.params.volumeMin.x = -10.0; req.params.volumeMin.y = -10.0; req.params.volumeMin.z = 0.0;
req.params.volumeMax.x = 10.0; req.params.volumeMax.y = 10.0; req.params.volumeMax.z = 0.0;
+
+ robot_srvs::KinematicPlanState::request r;
+ r.value = req;
- performCall(req);
+ performCall(r);
}
@@ -92,8 +95,8 @@
if (ros::service::call("plan_kinematic_path_state", req, res))
{
- EXPECT_TRUE(res.path.get_states_size() > 2);
- EXPECT_TRUE(res.distance == 0.0);
+ EXPECT_TRUE(res.value.path.get_states_size() > 2);
+ EXPECT_TRUE(res.value.distance == 0.0);
}
else
{
@@ -104,7 +107,7 @@
void runTestLeftEEf(void)
{
- robot_srvs::KinematicPlanLinkPosition::request req;
+ robot_msgs::KinematicPlanLinkPositionRequest req;
req.params.model_id = "pr2::right_arm";
req.params.distance_metric = "L2Square";
@@ -133,7 +136,10 @@
req.params.volumeMin.x = -5.0; req.params.volumeMin.y = -5.0; req.params.volumeMin.z = 0.0;
req.params.volumeMax.x = 5.0; req.params.volumeMax.y = 5.0; req.params.volumeMax.z = 0.0;
- performCall(req);
+ robot_srvs::KinematicPlanLinkPosition::request r;
+ r.value = req;
+
+ performCall(r);
}
void performCall(robot_srvs::KinematicPlanLinkPosition::request &req)
@@ -141,8 +147,8 @@
robot_srvs::KinematicPlanLinkPosition::response res;
if (ros::service::call("plan_kinematic_path_position", req, res))
{
- EXPECT_TRUE(res.path.get_states_size() > 0);
- EXPECT_TRUE(res.distance >= 0.0);
+ EXPECT_TRUE(res.value.path.get_states_size() > 0);
+ EXPECT_TRUE(res.value.distance >= 0.0);
}
else
{
Added: pkg/trunk/prcore/robot_msgs/msg/KinematicPlanLinkPositionRequest.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/KinematicPlanLinkPositionRequest.msg (rev 0)
+++ pkg/trunk/prcore/robot_msgs/msg/KinematicPlanLinkPositionRequest.msg 2009-01-24 03:01:21 UTC (rev 10123)
@@ -0,0 +1,36 @@
+# This message contains the definition for a request to the motion
+# planner
+
+
+# Parameters for the state space
+robot_msgs/KinematicSpaceParameters params
+
+
+# The starting state for the robot. This is eth complete state of the
+# robot, all joint values, everything that needs to be specified to
+# completely define where each part of the robot is in space. The
+# meaning for each element in the state vector can be extracted from
+# viewing the output of the robot model construction (the kinematic
+# model constructed from the parsed URDF model) in verbose mode
+robot_msgs/KinematicState start_state
+
+
+# The goal state for the model to plan for. The state is not explicit.
+# The goal is considered achieved if all the constraints are satisfied.
+robot_msgs/PoseConstraint[] goal_constraints
+
+
+# No state in the produced motion plan will violate these constraints
+robot_msgs/KinematicConstraints constraints
+
+
+# The number of times this plan is to be computed. Shortest solution
+# will be reported.
+int32 times
+
+# Boolean flag: if true, the returned path will contain interpolated
+# states as well
+byte interpolate
+
+# The maximum amount of time the motion planner is allowed to plan for
+float64 allowed_time
Added: pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStateRequest.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStateRequest.msg (rev 0)
+++ pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStateRequest.msg 2009-01-24 03:01:21 UTC (rev 10123)
@@ -0,0 +1,38 @@
+# This message contains the definition for a request to the motion
+# planner
+
+# Parameters for the state space
+robot_msgs/KinematicSpaceParameters params
+
+# The starting state for the robot. This is the complete state of the
+# robot, all joint values, everything that needs to be specified to
+# completely define where each part of the robot is in space. The
+# meaning for each element in the state vector can be extracted from
+# viewing the output of the robot model construction (the kinematic
+# model constructed from the parsed URDF model) in verbose mode
+robot_msgs/KinematicState start_state
+
+
+# The goal state for the model to plan for. The dimension of this
+# state must be equal to the dimension of the state space
+# characterizing the model (group) to plan for.
+robot_msgs/KinematicState goal_state
+
+# No state in the produced motion plan will violate these constraints
+robot_msgs/KinematicConstraints constraints
+
+# The number of times this plan is to be computed. Shortest solution
+# will be reported.
+int32 times
+
+# Boolean flag: if true, the returned path will contain interpolated
+# states as well
+byte interpolate
+
+# The maximum amount of time the motion planner is allowed to plan for
+float64 allowed_time
+
+
+# The threshold (distance) that is allowed between the returned
+# solution and the actual goal
+float64 threshold
Added: pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStatus.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStatus.msg (rev 0...
[truncated message content] |
|
From: <jfa...@us...> - 2009-01-24 06:07:14
|
Revision: 10133
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10133&view=rev
Author: jfaustwg
Date: 2009-01-24 06:07:08 +0000 (Sat, 24 Jan 2009)
Log Message:
-----------
Updated for removal of boost and log4cxx as 3rdparty packages
Modified Paths:
--------------
pkg/trunk/3rdparty/bfl/Makefile
pkg/trunk/3rdparty/bfl/manifest.xml
pkg/trunk/3rdparty/gazebo/Makefile
pkg/trunk/3rdparty/gazebo/manifest.xml
pkg/trunk/3rdparty/ijg_libjpeg/CMakeLists.txt
pkg/trunk/3rdparty/ijg_libjpeg/manifest.xml
pkg/trunk/3rdparty/kdl/Makefile
pkg/trunk/3rdparty/kdl/manifest.xml
pkg/trunk/3rdparty/kni-3.9.2/lm_boost_cxxflags.patch
pkg/trunk/3rdparty/kni-3.9.2/manifest.xml
pkg/trunk/3rdparty/libsunflower/Makefile
pkg/trunk/3rdparty/nepumuk/Makefile
pkg/trunk/3rdparty/nepumuk/manifest.xml
pkg/trunk/3rdparty/ogre/Makefile
pkg/trunk/3rdparty/ogre/manifest.xml
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/3rdparty/openrave/manifest.xml
pkg/trunk/calibration/kinematic_calibration/CMakeLists.txt
pkg/trunk/calibration/kinematic_calibration/manifest.xml
pkg/trunk/calibration/wheel_odometry_calibration/CMakeLists.txt
pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml
pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
pkg/trunk/deprecated/cv_wrappers/CMakeLists.txt
pkg/trunk/deprecated/cv_wrappers/manifest.xml
pkg/trunk/deprecated/libTF/CMakeLists.txt
pkg/trunk/deprecated/libTF/manifest.xml
pkg/trunk/deprecated/misc_utils/CMakeLists.txt
pkg/trunk/deprecated/misc_utils/manifest.xml
pkg/trunk/deprecated/random_utils/CMakeLists.txt
pkg/trunk/deprecated/random_utils/manifest.xml
pkg/trunk/deprecated/resource_locator/CMakeLists.txt
pkg/trunk/deprecated/resource_locator/manifest.xml
pkg/trunk/deprecated/scan_utils/CMakeLists.txt
pkg/trunk/deprecated/scan_utils/manifest.xml
pkg/trunk/drivers/cam/axis_cam/CMakeLists.txt
pkg/trunk/drivers/cam/axis_cam/manifest.xml
pkg/trunk/drivers/laser/laser_scan_annotator/CMakeLists.txt
pkg/trunk/drivers/laser/laser_scan_annotator/manifest.xml
pkg/trunk/drivers/motor/ethercat_hardware/CMakeLists.txt
pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml
pkg/trunk/drivers/robot/ipdcmot/CMakeLists.txt
pkg/trunk/drivers/robot/ipdcmot/manifest.xml
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/CMakeLists.txt
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/manifest.xml
pkg/trunk/drivers/robot/pr2/pr2_power_board/manifest.xml
pkg/trunk/drivers/robot/pr2/pr2_power_board/src/power_node/CMakeLists.txt
pkg/trunk/drivers/robot/segway_apox/CMakeLists.txt
pkg/trunk/drivers/robot/segway_apox/manifest.xml
pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
pkg/trunk/estimation/robot_pose_ekf/CMakeLists.txt
pkg/trunk/estimation/robot_pose_ekf/manifest.xml
pkg/trunk/hardware_test/self_test/CMakeLists.txt
pkg/trunk/hardware_test/self_test/manifest.xml
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/CMakeLists.txt
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml
pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt
pkg/trunk/highlevel/highlevel_controllers/manifest.xml
pkg/trunk/mechanism/mechanism_control/CMakeLists.txt
pkg/trunk/mechanism/mechanism_control/manifest.xml
pkg/trunk/motion_planning/kinematic_planning/CMakeLists.txt
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
pkg/trunk/nav/nav_view_sdl/CMakeLists.txt
pkg/trunk/nav/nav_view_sdl/manifest.xml
pkg/trunk/nav/trajectory_rollout/CMakeLists.txt
pkg/trunk/nav/trajectory_rollout/manifest.xml
pkg/trunk/nav/wavefront_player/CMakeLists.txt
pkg/trunk/nav/wavefront_player/manifest.xml
pkg/trunk/openrave_planning/openraveros/CMakeLists.txt
pkg/trunk/openrave_planning/openraveros/manifest.xml
pkg/trunk/openrave_planning/ormanipulation/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/CMakeLists.txt
pkg/trunk/openrave_planning/orrosplanning/manifest.xml
pkg/trunk/prcore/tf/CMakeLists.txt
pkg/trunk/prcore/tf/manifest.xml
pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt
pkg/trunk/robot_descriptions/openrave_robot_description/manifest.xml
pkg/trunk/simulators/rosstage/CMakeLists.txt
pkg/trunk/simulators/rosstage/manifest.xml
pkg/trunk/util/laser_scan/CMakeLists.txt
pkg/trunk/util/laser_scan/manifest.xml
pkg/trunk/util/message_sequencing/CMakeLists.txt
pkg/trunk/util/message_sequencing/manifest.xml
pkg/trunk/util/realtime_tools/CMakeLists.txt
pkg/trunk/util/realtime_tools/manifest.xml
pkg/trunk/util/robot_self_filter/CMakeLists.txt
pkg/trunk/util/robot_self_filter/manifest.xml
pkg/trunk/util/topic_synchronizer/manifest.xml
pkg/trunk/vision/calib_converter/CMakeLists.txt
pkg/trunk/vision/calib_converter/manifest.xml
pkg/trunk/vision/calonder_descriptor/CMakeLists.txt
pkg/trunk/vision/calonder_descriptor/manifest.xml
pkg/trunk/vision/checkerboard_detector/CMakeLists.txt
pkg/trunk/vision/checkerboard_detector/manifest.xml
pkg/trunk/vision/color_calib/CMakeLists.txt
pkg/trunk/vision/color_calib/manifest.xml
pkg/trunk/vision/outlet_detection/CMakeLists.txt
pkg/trunk/vision/outlet_detection/manifest.xml
pkg/trunk/vision/people/CMakeLists.txt
pkg/trunk/vision/people/manifest.xml
pkg/trunk/vision/place_recognition/CMakeLists.txt
pkg/trunk/vision/place_recognition/manifest.xml
pkg/trunk/vision/stereo_blob_tracker/CMakeLists.txt
pkg/trunk/vision/stereo_blob_tracker/manifest.xml
pkg/trunk/vision/visual_odometry/CMakeLists.txt
pkg/trunk/vision/visual_odometry/manifest.xml
pkg/trunk/visualization/cloud_viewer/CMakeLists.txt
pkg/trunk/visualization/cloud_viewer/manifest.xml
pkg/trunk/visualization/ogre_tools/CMakeLists.txt
pkg/trunk/visualization/ogre_tools/manifest.xml
pkg/trunk/visualization/ogre_visualizer/CMakeLists.txt
pkg/trunk/visualization/ogre_visualizer/manifest.xml
pkg/trunk/visualization/wx_camera_panel/CMakeLists.txt
pkg/trunk/visualization/wx_camera_panel/manifest.xml
pkg/trunk/world_models/costmap_2d/CMakeLists.txt
pkg/trunk/world_models/costmap_2d/manifest.xml
pkg/trunk/world_models/robot_models/collision_space/CMakeLists.txt
pkg/trunk/world_models/robot_models/collision_space/manifest.xml
pkg/trunk/world_models/topological_map/CMakeLists.txt
pkg/trunk/world_models/topological_map/manifest.xml
pkg/trunk/world_models/world_3d_map/CMakeLists.txt
pkg/trunk/world_models/world_3d_map/manifest.xml
Added Paths:
-----------
pkg/trunk/3rdparty/ogre/boost.patch
pkg/trunk/drivers/input/joy_annotator/joy_node.cpp
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:9624
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10106
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/3rdparty/bfl/Makefile
===================================================================
--- pkg/trunk/3rdparty/bfl/Makefile 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/3rdparty/bfl/Makefile 2009-01-24 06:07:08 UTC (rev 10133)
@@ -6,9 +6,9 @@
SVN_PATCH = mcpdf.patch warning.patch
INSTALL_DIR = bfl-boost
CMAKE = cmake
-BOOST_ROOT =$(shell rospack find boost)/boost
+BOOST_INCLUDE =$(shell rosboost-cfg --include_dirs)
CMAKE_ARGS = -DCMAKE_INSTALL_PREFIX=$(PWD)/$(INSTALL_DIR)/ \
- -DMATRIX_LIB=boost -DRNG_LIB=boost -DBOOST_FOUND=`rospack find boost`/boost/include/
+ -DMATRIX_LIB=boost -DRNG_LIB=boost -DBOOST_FOUND=$(BOOST_INCLUDE)
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/3rdparty/bfl/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/bfl/manifest.xml 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/3rdparty/bfl/manifest.xml 2009-01-24 06:07:08 UTC (rev 10133)
@@ -17,7 +17,6 @@
<doxymaker external="http://orocos.org/bfl" />
</export>
-<depend package="boost" />
<sysdepend os="ubuntu" version="7.04-feisty" package="subversion"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="subversion"/>
<sysdepend os="ubuntu" version="7.04-feisty" package="libcppunit-dev"/>
Modified: pkg/trunk/3rdparty/gazebo/Makefile
===================================================================
--- pkg/trunk/3rdparty/gazebo/Makefile 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/3rdparty/gazebo/Makefile 2009-01-24 06:07:08 UTC (rev 10133)
@@ -19,19 +19,18 @@
ROOT = $(shell rospack find gazebo)/gazebo
TESTROOT = $(shell rospack find gazebo)/test-gazebo
-BOOST_ROOT = $(shell rospack find boost)/boost
+BOOST_CFLAGS = $(shell rosboost-cfg --cflags)
+BOOST_LFLAGS = $(shell rosboost-cfg --lflags signals,thread)
-CCFLAGS=-I${BOOST_ROOT}/include
-
build: SVN_UP_REVERT_PATCH gazebo
.PHONY: gazebo
gazebo: $(SVN_DIR)
if [ ! -d gazebo ]; then mkdir -p gazebo; mkdir -p gazebo/share; mkdir -p gazebo/share/gazebo; mkdir -p gazebo/share/gazebo/worlds; mkdir -p gazebo/share/gazebo/Media; fi
cd $(SVN_DIR) && PKG_CONFIG_PATH=${CV_PKG}:${ODE_PKG}:${FI_PKG}:${PLAYER_PKG}:${OGRE_PKG}:${PKG_CONFIG_PATH} PATH=${ODE_PATH}:${PATH} \
- scons mode=optimized prefix=$(ROOT) boost_cflags='-I${BOOST_ROOT}/include' boost_lflags='-lboost_system-mt -lboost_graph -lboost_regex -lboost_signals -lboost_thread -L${BOOST_ROOT}/lib'
+ scons mode=optimized prefix=$(ROOT) boost_cflags="$(BOOST_CFLAGS)" boost_lflags="$(BOOST_LFLAGS)"
-cd $(SVN_DIR) && PKG_CONFIG_PATH=${CV_PKG}:${ODE_PKG}:${FI_PKG}:${PLAYER_PKG}:${OGRE_PKG}:${PKG_CONFIG_PATH} PATH=${ODE_PATH}:${PATH} \
- scons mode=optimized prefix=$(ROOT) boost_cflags='-I${BOOST_ROOT}/include' boost_lflags='-lboost_system-mt -lboost_graph -lboost_regex -lboost_signals -lboost_thread -L${BOOST_ROOT}/lib' install
+ scons mode=optimized prefix=$(ROOT) boost_cflags="$(BOOST_CFLAGS)" boost_lflags="$(BOOST_LFLAGS)" install
touch gazebo
ROOT_ORIG = $(shell rospack find gazebo)/gazebo-original
Modified: pkg/trunk/3rdparty/gazebo/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/gazebo/manifest.xml 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/3rdparty/gazebo/manifest.xml 2009-01-24 06:07:08 UTC (rev 10133)
@@ -21,10 +21,6 @@
<sysdepend os="ubuntu" version="8.04-hardy" package="libltdl3-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libfltk1.1"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libfltk1.1-dev"/>
- <!--sysdepend os="ubuntu" version="7.04-feisty" package="libboost-signals1.34.1"/>
- <sysdepend os="ubuntu" version="7.04-feisty" package="libboost-signals-dev"/>
- <sysdepend os="ubuntu" version="8.04-hardy" package="libboost-signals1.34.1"/>
- <sysdepend os="ubuntu" version="8.04-hardy" package="libboost-signals-dev"/-->
</package>
Modified: pkg/trunk/3rdparty/ijg_libjpeg/CMakeLists.txt
===================================================================
--- pkg/trunk/3rdparty/ijg_libjpeg/CMakeLists.txt 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/3rdparty/ijg_libjpeg/CMakeLists.txt 2009-01-24 06:07:08 UTC (rev 10133)
@@ -21,6 +21,9 @@
COMMAND sed -e '/HAVE_STDLIB_H/d' -e '/HAVE_STDDEF_H/d' <${PROJECT_SOURCE_DIR}/build/jpeg-6b/jconfig.h >${PROJECT_SOURCE_DIR}/include/ijg_libjpeg/jconfig.h
)
+rospack_add_boost_directories()
+
rospack_add_library(ros_jpeg_mutex src/ros_jpeg_mutex.cpp)
+rospack_link_boost(ros_jpeg_mutex thread)
add_file_dependencies(src/ros_jpeg_mutex.cpp ${PROJECT_SOURCE_DIR}/lib/libjpeg.a)
Modified: pkg/trunk/3rdparty/ijg_libjpeg/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/ijg_libjpeg/manifest.xml 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/3rdparty/ijg_libjpeg/manifest.xml 2009-01-24 06:07:08 UTC (rev 10133)
@@ -15,7 +15,6 @@
<license>BSD-style</license>
<review status="3rdparty" notes=""/>
<url>http://www.ijg.org</url>
-<depend package="boost"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -ljpeg -lros_jpeg_mutex"/>
<doxymaker external="http://www.ijg.org"/>
Modified: pkg/trunk/3rdparty/kdl/Makefile
===================================================================
--- pkg/trunk/3rdparty/kdl/Makefile 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/3rdparty/kdl/Makefile 2009-01-24 06:07:08 UTC (rev 10133)
@@ -2,12 +2,13 @@
SOURCE_DIR = kdl-willow
CMAKE = cmake
-BOOST_ROOT =$(shell rospack find boost)/boost
+BOOST_INCLUDE_DIR = $(shell rosboost-cfg --include_dirs)
+BOOST_LIBRARY_DIR = $(shell rosboost-cfg --library_dirs)
CMAKE_ARGS = -DCMAKE_INSTALL_PREFIX=$(PWD)/kdl/ \
-DPYTHON_BINDINGS=OFF \
-DKDL_PYTHON_INSTALL_PATH=$(PWD)/kdl/lib/ \
- -DBOOST_INCLUDEDIR:STRING=$(BOOST_ROOT)/include \
- -DBOOST_LIBRARYDIR:STRING=$(BOOST_ROOT)/lib
+ -DBOOST_INCLUDEDIR:STRING=$(BOOST_INCLUDE_DIR) \
+ -DBOOST_LIBRARYDIR:STRING=$(BOOST_LIBRARY_DIR)
installed: wiped
-mkdir -p $(SOURCE_DIR)/build
Modified: pkg/trunk/3rdparty/kdl/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/kdl/manifest.xml 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/3rdparty/kdl/manifest.xml 2009-01-24 06:07:08 UTC (rev 10133)
@@ -4,7 +4,6 @@
<author>Advait Jain, Sachin Chitta</author>
<license>BSD</license>
<review status="3rdparty" notes=""/>
-<depend package="boost" />
<url>http://pr.willowgarage.net</url>
<export>
<cpp cflags="-I${prefix}/kdl/include" lflags="-Wl,-rpath,${prefix}/kdl/lib -L${prefix}/kdl/lib -lorocos-kdl"/>
Modified: pkg/trunk/3rdparty/kni-3.9.2/lm_boost_cxxflags.patch
===================================================================
--- pkg/trunk/3rdparty/kni-3.9.2/lm_boost_cxxflags.patch 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/3rdparty/kni-3.9.2/lm_boost_cxxflags.patch 2009-01-24 06:07:08 UTC (rev 10133)
@@ -5,7 +5,7 @@
CXX = g++
-CXXFLAGS += -fPIC -g -O2 -Wall
-+CXXFLAGS += -fPIC -g -O2 -Wall `rospack export/cpp/cflags boost`
++CXXFLAGS += -fPIC -g -O2 -Wall `rosboost-cfg --cflags`
MAJOR_VERSION = 3.9
MINOR_VERSION = 0
Modified: pkg/trunk/3rdparty/kni-3.9.2/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/kni-3.9.2/manifest.xml 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/3rdparty/kni-3.9.2/manifest.xml 2009-01-24 06:07:08 UTC (rev 10133)
@@ -3,7 +3,6 @@
<description>Control library and demos for the Katana arm</description>
<license>GPL</license>
<review status="3rdparty" notes=""/>
- <depend package="boost"/>
<url>http://www.neuronics.ch</url>
<export>
<cpp cflags="-I${prefix}/KNI_3.9.2/include" lflags="-L${prefix}/KNI_3.9.2/lib/linux -lKNI_LM -lKNI_InvKin -lKNIBase"/>
Modified: pkg/trunk/3rdparty/libsunflower/Makefile
===================================================================
--- pkg/trunk/3rdparty/libsunflower/Makefile 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/3rdparty/libsunflower/Makefile 2009-01-24 06:07:08 UTC (rev 10133)
@@ -28,7 +28,7 @@
BUILD_DIR= $(PWD)/build
INST_DIR= $(PWD)/local
ESTAR_DIR= `rospack find estar`/local
-BOOST_DIR= `rospack cflags-only-I boost`
+BOOST_DIR= `rosboost-cfg --include_dirs`
SVN_DIR= $(PWD)/build/sunflower-svn
SVN_URL= https://libsunflower.svn.sourceforge.net/svnroot/libsunflower/trunk/sunflower
Modified: pkg/trunk/3rdparty/nepumuk/Makefile
===================================================================
--- pkg/trunk/3rdparty/nepumuk/Makefile 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/3rdparty/nepumuk/Makefile 2009-01-24 06:07:08 UTC (rev 10133)
@@ -32,7 +32,7 @@
INST_DIR= $(PWD)/local
ESTAR_DIR= `rospack find estar`/local
SFL_DIR= `rospack find libsunflower`/local
-BOOST_DIR= `rospack cflags-only-I boost`
+BOOST_DIR= `rosboost-cfg --include_dirs`
SVN_DIR= $(PWD)/build/nepumuk-svn
SVN_URL= https://libsunflower.svn.sourceforge.net/svnroot/libsunflower/trunk/nepumuk
Modified: pkg/trunk/3rdparty/nepumuk/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/nepumuk/manifest.xml 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/3rdparty/nepumuk/manifest.xml 2009-01-24 06:07:08 UTC (rev 10133)
@@ -11,7 +11,6 @@
<url>http://libsunflower.sourceforge.net/</url>
<depend package="roscpp"/>
<depend package="std_msgs"/>
- <depend package="boost"/>
<depend package="libsunflower"/>
<sysdepend os="debian" version="4.0-etch" package="libpng12-dev"/>
<sysdepend os="debian" version="4.0-etch" package="autoconf"/>
Modified: pkg/trunk/3rdparty/ogre/Makefile
===================================================================
--- pkg/trunk/3rdparty/ogre/Makefile 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/3rdparty/ogre/Makefile 2009-01-24 06:07:08 UTC (rev 10133)
@@ -5,7 +5,7 @@
TARBALL_URL = http://pr.willowgarage.com/downloads/$(OGRE_VERSION).tar.bz2
SOURCE_DIR = build/$(OGRE_VERSION)
UNPACK_CMD = tar xf
-TARBALL_PATCH = warnings.patch
+TARBALL_PATCH = warnings.patch boost.patch
INITIAL_DIR = build/ogre
include $(shell rospack find mk)/download_unpack_build.mk
@@ -16,12 +16,12 @@
$(shell rospack --lang=cpp --attrib=cflags export freeimage) \
$(shell rospack --lang=cpp --attrib=cflags export opende) \
$(shell rospack --lang=cpp --attrib=cflags export Cg) \
- $(shell rospack --lang=cpp --attrib=cflags export boost)
+ $(shell rosboost-cfg --cflags)
LFLAGS = $(shell rospack --lang=cpp --attrib=lflags export freeimage) \
$(shell rospack --lang=cpp --attrib=lflags export opende) \
$(shell rospack --lang=cpp --attrib=lflags export Cg) \
- $(shell rospack --lang=cpp --attrib=lflags export boost) \
+ $(shell rosboost-cfg --lflags thread) \
-lpthread
CONFIGURE_FLAGS = --prefix=$(OGREROOT) --enable-threading --with-pic --with-platform=GLX --with-gui=gtk --disable-ogre-demos --with-arch=nocona --enable-release
Added: pkg/trunk/3rdparty/ogre/boost.patch
===================================================================
--- pkg/trunk/3rdparty/ogre/boost.patch (rev 0)
+++ pkg/trunk/3rdparty/ogre/boost.patch 2009-01-24 06:07:08 UTC (rev 10133)
@@ -0,0 +1,14 @@
+--- configure 2009-01-22 16:02:27.000000000 -0800
++++ configure-noboosterror 2009-01-22 16:02:22.000000000 -0800
+@@ -25777,11 +25777,6 @@
+ echo "${ECHO_T}$ac_cv_lib_boost_thread_mt_main" >&6
+ if test $ac_cv_lib_boost_thread_mt_main = yes; then
+ OGRE_THREAD_LIBS="-lboost_thread-mt"
+-else
+-
+- { { echo "$as_me:$LINENO: error: You need the C++ boost libraries." >&5
+-echo "$as_me: error: You need the C++ boost libraries." >&2;}
+- { (exit 1); exit 1; }; }
+ fi
+
+ fi
Modified: pkg/trunk/3rdparty/ogre/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/ogre/manifest.xml 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/3rdparty/ogre/manifest.xml 2009-01-24 06:07:08 UTC (rev 10133)
@@ -10,7 +10,6 @@
<url>http://ogre3d.org</url>
<depend package="freeimage"/>
<depend package="Cg"/>
-<depend package="boost"/>
<export>
<cpp lflags="-Wl,-rpath,${prefix}/ogre/lib -L${prefix}/ogre/lib/OGRE `PKG_CONFIG_PATH=${prefix}/ogre/lib/pkgconfig:$PKG_CONFIG_PATH pkg-config --libs OGRE`" cflags="`PKG_CONFIG_PATH=${prefix}/ogre/lib/pkgconfig:$PKG_CONFIG_PATH pkg-config --cflags OGRE`"/>
<doxymaker external="http://ogre3d.org"/>
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-01-24 06:07:08 UTC (rev 10133)
@@ -8,8 +8,11 @@
.PHONY: openrave
+BOOST_INCLUDE_DIRS=$(shell rosboost-cfg --include_dirs)
+BOOST_LIBRARY_DIRS=$(shell rosboost-cfg --lib_dirs)
+
installed: $(SVN_DIR) patched
- @cd $(SVN_DIR) && export PATH="$(shell rospack find soqt)/bin:$(shell rospack find opende)/opende/bin:$(PATH)" && export CPATH="$(shell rospack find bullet)/include" && export LD_LIBRARY_PATH=$(shell rospack find bullet)/lib:$(LD_LIBRARY_PATH) && export BOOST_ROOT=$(shell rospack find boost)/boost && mkdir -p build && cd build && cmake -DCMAKE_INSTALL_PREFIX=$(PWD) -DCMAKE_BUILD_TYPE=Release .. && make $(PARALLEL_JOBS) install
+ @cd $(SVN_DIR) && export PATH="$(shell rospack find soqt)/bin:$(shell rospack find opende)/opende/bin:$(PATH)" && export CPATH="$(shell rospack find bullet)/include:$(CPATH)" && export LD_LIBRARY_PATH=$(shell rospack find bullet)/lib:$(LD_LIBRARY_PATH) && mkdir -p build && cd build && cmake -DCMAKE_INSTALL_PREFIX=$(PWD) -DCMAKE_BUILD_TYPE=Release -DBoost_INCLUDE_DIR=$(BOOST_INCLUDE_DIRS) -DBoost_LIBRARY_DIR=$(BOOST_LIBRARY_DIRS) .. && make $(PARALLEL_JOBS) install
touch installed
clean:
Modified: pkg/trunk/3rdparty/openrave/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/openrave/manifest.xml 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/3rdparty/openrave/manifest.xml 2009-01-24 06:07:08 UTC (rev 10133)
@@ -14,7 +14,6 @@
<depend package="bullet"/>
<depend package="opende"/>
<depend package="std_msgs"/>
- <depend package="boost"/>
<versioncontrol type="svn" url="https://openrave.svn.sourceforge.net/svnroot/openrave"/>
<sysdepend os="ubuntu" version="7.04-feisty" package="libglew-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libglew1.5-dev"/>
Modified: pkg/trunk/calibration/kinematic_calibration/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/CMakeLists.txt 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/calibration/kinematic_calibration/CMakeLists.txt 2009-01-24 06:07:08 UTC (rev 10133)
@@ -12,11 +12,15 @@
src/unittest_verification.cpp
)
+rospack_add_boost_directories()
+
rospack_add_executable(arm_phasespace_grabber src/arm_phasespace_grabber.cpp)
target_link_libraries(arm_phasespace_grabber kinematic_calibration)
+rospack_link_boost(arm_phasespace_grabber thread)
rospack_add_executable(sensor_kinematics_grabber src/sensor_kinematics_grabber.cpp)
target_link_libraries(sensor_kinematics_grabber kinematic_calibration)
+rospack_link_boost(sensor_kinematics_grabber thread)
# ********** Tests **********
rospack_add_gtest(test/jacobian_unittest test/jacobian_unittest.cpp)
Modified: pkg/trunk/calibration/kinematic_calibration/manifest.xml
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/manifest.xml 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/calibration/kinematic_calibration/manifest.xml 2009-01-24 06:07:08 UTC (rev 10133)
@@ -11,7 +11,6 @@
<depend package="topic_synchronizer" />
<depend package="image_msgs" />
<depend package="newmat10"/>
- <depend package="boost"/>
<depend package="pr2_mechanism_controllers" />
# For Scripts
Modified: pkg/trunk/calibration/wheel_odometry_calibration/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/CMakeLists.txt 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/calibration/wheel_odometry_calibration/CMakeLists.txt 2009-01-24 06:07:08 UTC (rev 10133)
@@ -2,4 +2,7 @@
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(wheel_odometry_calibration)
genmsg()
+
rospack_add_executable(wheel_odometry_calibration src/odom_calib.cpp)
+rospack_add_boost_directories()
+rospack_link_boost(wheel_odometry_calibration thread)
Modified: pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml 2009-01-24 06:07:08 UTC (rev 10133)
@@ -7,5 +7,4 @@
<depend package="std_msgs" />
<depend package="roscpp" />
<depend package="pr2_mechanism_controllers" />
- <depend package="boost" />
</package>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2009-01-24 06:07:08 UTC (rev 10133)
@@ -5,6 +5,8 @@
gensrv()
#rospack_add_library(pr2_controllers src/base_controller.cpp src/laser_scanner_controller.cpp src/arm_position_controller.cpp src/arm_velocity_controller.cpp src/arm_pd_controller.cpp src/head_pan_tilt_controller.cpp)
+rospack_add_boost_directories()
+
rospack_add_library( pr2_mechanism_controllers
src/base_controller.cpp
src/base_controller_pos.cpp
@@ -23,6 +25,7 @@
src/gripper_calibration_controller.cpp
src/arm_trajectory_controller.cpp
)
+rospack_link_boost(pr2_mechanism_controllers thread)
#rospack_add_executable(testBaseController test/test_base_controller.cpp)
#target_link_libraries(testBaseController pr2_mechanism_controllers)
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-01-24 06:07:08 UTC (rev 10133)
@@ -18,7 +18,6 @@
<depend package="robot_kinematics" />
<depend package="rosconsole" />
<depend package="trajectory" />
- <depend package="boost" />
<depend package="angles" />
<depend package="control_toolbox" />
<depend package="filters" />
Modified: pkg/trunk/deprecated/cv_wrappers/CMakeLists.txt
===================================================================
--- pkg/trunk/deprecated/cv_wrappers/CMakeLists.txt 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/deprecated/cv_wrappers/CMakeLists.txt 2009-01-24 06:07:08 UTC (rev 10133)
@@ -3,14 +3,18 @@
rospack(cv_wrappers)
+rospack_add_boost_directories()
+
genmsg()
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
# cv_view* (note: does not include cv_view in service/)
rospack_add_executable(cv_view src/cv_view.cpp)
+rospack_link_boost(cv_view thread)
rospack_add_executable(cv_view_minimal minimal/cv_view.cpp)
rospack_add_executable(cv_view_array src/cv_view_array.cpp)
+rospack_link_boost(cv_view_array thread)
rospack_add_executable(cv_view_dcam dcam/cv_view.cpp)
# cv_movie_streamer
Modified: pkg/trunk/deprecated/cv_wrappers/manifest.xml
===================================================================
--- pkg/trunk/deprecated/cv_wrappers/manifest.xml 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/deprecated/cv_wrappers/manifest.xml 2009-01-24 06:07:08 UTC (rev 10133)
@@ -25,7 +25,6 @@
<depend package="image_msgs"/>
<depend package="image_utils"/>
<depend package="roscpp"/>
- <depend package="boost"/>
<depend package="color_calib"/>
<export>
<cpp cflags="-I${prefix}/msg/cpp" />
Modified: pkg/trunk/deprecated/libTF/CMakeLists.txt
===================================================================
--- pkg/trunk/deprecated/libTF/CMakeLists.txt 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/deprecated/libTF/CMakeLists.txt 2009-01-24 06:07:08 UTC (rev 10133)
@@ -2,7 +2,10 @@
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(libTF)
+rospack_add_boost_directories()
+
rospack_add_library(oldTF src/libTF.cpp src/Pose3DCache.cpp src/Pose3D.cpp)
+rospack_link_boost(oldTF thread)
rospack_add_executable(simpletest src/test/main.cpp)
target_link_libraries(simpletest oldTF)
rospack_add_executable(testtf src/test/testtf.cc)
Modified: pkg/trunk/deprecated/libTF/manifest.xml
===================================================================
--- pkg/trunk/deprecated/libTF/manifest.xml 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/deprecated/libTF/manifest.xml 2009-01-24 06:07:08 UTC (rev 10133)
@@ -10,11 +10,10 @@
<url>http://pr.willowgarage.com</url>
<depend package="newmat10"/>
<depend package="ros_exception"/>
-<depend package="boost"/>
<depend package="angles"/>
<depend package="std_msgs"/>
<depend package="roscpp"/>
<export>
-<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -loldTF"/>
+<cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -loldTF `rosboost-cfg --lflags thread`"/>
</export>
</package>
Modified: pkg/trunk/deprecated/misc_utils/CMakeLists.txt
===================================================================
--- pkg/trunk/deprecated/misc_utils/CMakeLists.txt 2009-01-24 06:03:57 UTC (rev 10132)
+++ pkg/trunk/deprecated/misc_utils/CMakeLists.txt 2009-01-24 06:07:08 UTC (rev 10133)
@@ -1,6 +1,7 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(misc_utils)
+
add_definitions(-Wall)
rospack_add_gtest(ring_buffer_test test/test_ring_buffer.cpp)
Modified: pkg/trunk/deprecated/misc_utils/manifest.xml
===========================...
[truncated message content] |
|
From: <is...@us...> - 2009-01-24 08:56:41
|
Revision: 10138
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10138&view=rev
Author: isucan
Date: 2009-01-24 08:56:35 +0000 (Sat, 24 Jan 2009)
Log Message:
-----------
verbosity + args bugfix
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
pkg/trunk/motion_planning/kinematic_planning/kinematic_planning.launch
pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
pkg/trunk/motion_planning/kinematic_planning/src/state_validity_monitor.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp
pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-01-24 08:36:18 UTC (rev 10137)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-01-24 08:56:35 UTC (rev 10138)
@@ -263,8 +263,6 @@
{
}
- private:
-
double radiusOfBox(std_msgs::Point32 &point)
{
return std::max(std::max(point.x, point.y), point.z) * 1.732050808;
Modified: pkg/trunk/motion_planning/kinematic_planning/kinematic_planning.launch
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/kinematic_planning.launch 2009-01-24 08:36:18 UTC (rev 10137)
+++ pkg/trunk/motion_planning/kinematic_planning/kinematic_planning.launch 2009-01-24 08:56:35 UTC (rev 10138)
@@ -1,4 +1,4 @@
<launch>
- <node pkg="kinematic_planning" type="kinematic_planning" args="robotdesc/pr2" respawn="true" output="screen" />
- <node pkg="kinematic_planning" type="motion_validator" args="robotdesc/pr2" respawn="true" />
+ <node pkg="kinematic_planning" type="kinematic_planning" args="robotdesc/pr2 collision_map:=collision_map_buffer" respawn="true" output="screen" />
+ <node pkg="kinematic_planning" type="motion_validator" args="robotdesc/pr2 collision_map:=collision_map_buffer" respawn="true" />
</launch>
Modified: pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-01-24 08:36:18 UTC (rev 10137)
+++ pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-01-24 08:56:35 UTC (rev 10138)
@@ -41,7 +41,8 @@
@htmlinclude ../manifest.html
@b MotionValidator is a node capable of verifying if a path is valid
-or not (collides or does not collide).
+or not (collides or does not collide). The path is consider to be a
+straight line between two states.
<hr>
@@ -269,7 +270,7 @@
int main(int argc, char **argv)
{
- if (argc == 2)
+ if (argc >= 2)
{
ros::init(argc, argv);
Modified: pkg/trunk/motion_planning/kinematic_planning/src/state_validity_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/state_validity_monitor.cpp 2009-01-24 08:36:18 UTC (rev 10137)
+++ pkg/trunk/motion_planning/kinematic_planning/src/state_validity_monitor.cpp 2009-01-24 08:56:35 UTC (rev 10138)
@@ -89,6 +89,7 @@
#include "kinematic_planning/CollisionSpaceMonitor.h"
#include <std_msgs/Byte.h>
+#include <std_msgs/VisualizationMarker.h>
#include <iostream>
#include <sstream>
@@ -104,12 +105,13 @@
StateValidityMonitor(const std::string &robot_model) : ros::Node("state_validity_monitor"),
CollisionSpaceMonitor(dynamic_cast<ros::Node*>(this),
robot_model),
- last_(-1)
+ last_(-1),
+ id_(0)
{
advertise<std_msgs::Byte>("state_validity", 1);
+ advertise<std_msgs::VisualizationMarker>("visualizationMarker", 10240);
}
- /** Free the memory */
virtual ~StateValidityMonitor(void)
{
}
@@ -119,6 +121,17 @@
void afterWorldUpdate(void)
{
CollisionSpaceMonitor::afterWorldUpdate();
+
+ unsigned int n = m_collisionMap.get_boxes_size();
+ for (unsigned int i = 0 ; i < n ; ++i)
+ {
+ sendPoint(m_collisionMap.boxes[i].center.x,
+ m_collisionMap.boxes[i].center.y,
+ m_collisionMap.boxes[i].center.z,
+ radiusOfBox(m_collisionMap.boxes[i].extents),
+ m_collisionMap.header);
+ }
+
last_ = -1;
}
@@ -149,7 +162,36 @@
private:
+ void sendPoint(double x, double y, double z, double radius, const rostools::Header &header)
+ {
+ std_msgs::VisualizationMarker mk;
+ mk.header = header;
+
+ mk.id = id_++;
+ mk.type = std_msgs::VisualizationMarker::SPHERE;
+ mk.action = std_msgs::VisualizationMarker::ADD;
+ mk.x = x;
+ mk.y = y;
+ mk.z = z;
+
+ mk.roll = 0;
+ mk.pitch = 0;
+ mk.yaw = 0;
+
+ mk.xScale = radius * 2.0;
+ mk.yScale = radius * 2.0;
+ mk.zScale = radius * 2.0;
+
+ mk.alpha = 255;
+ mk.r = 255;
+ mk.g = 10;
+ mk.b = 10;
+
+ publish("visualizationMarker", mk);
+ }
+
int last_;
+ int id_;
};
@@ -161,7 +203,7 @@
int main(int argc, char **argv)
{
- if (argc == 2)
+ if (argc >= 2)
{
ros::init(argc, argv);
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-01-24 08:36:18 UTC (rev 10137)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-01-24 08:56:35 UTC (rev 10138)
@@ -242,9 +242,9 @@
{
printPath(path, distance);
sendDisplay(req.start_state, path, req.params.model_id);
- if (verifyPath(req.start_state, req.constraints, path, req.params.model_id))
- if (controller == C_ARM)
- sendArmCommand(path, req.params.model_id);
+ verifyPath(req.start_state, req.constraints, path, req.params.model_id);
+ if (controller == C_ARM)
+ sendArmCommand(path, req.params.model_id);
}
void executePath(robot_msgs::KinematicPlanStateRequest &req,
@@ -253,9 +253,9 @@
{
printPath(path, distance);
sendDisplay(req.start_state, path, req.params.model_id);
- if (verifyPath(req.start_state, req.constraints, path, req.params.model_id))
- if (controller == C_ARM)
- sendArmCommand(path, req.params.model_id);
+ verifyPath(req.start_state, req.constraints, path, req.params.model_id);
+ if (controller == C_ARM)
+ sendArmCommand(path, req.params.model_id);
}
protected:
@@ -266,10 +266,9 @@
for (unsigned int i = 0 ; i < path.get_states_size() ; ++i)
{
- traj.points[i].set_positions_size(path.states[i].get_vals_size() + 1);
+ traj.points[i].set_positions_size(path.states[i].get_vals_size());
for (unsigned int j = 0 ; j < path.states[i].get_vals_size() ; ++j)
traj.points[i].positions[j] = path.states[i].vals[j];
- traj.points[i].positions[path.states[i].get_vals_size()] = m_gripPos;
traj.points[i].time = 0.0;
}
}
@@ -372,9 +371,18 @@
req.params.volumeMin.x = -5.0 + m_basePos[0]; req.params.volumeMin.y = -5.0 + m_basePos[1]; req.params.volumeMin.z = 0.0;
req.params.volumeMax.x = 5.0 + m_basePos[0]; req.params.volumeMax.y = 5.0 + m_basePos[1]; req.params.volumeMax.z = 0.0;
- robot_srvs::KinematicPlanState::request r;
- r.value = req;
- m_pr.performCall(r, PlanningRequest::C_ARM);
+ if (replan)
+ {
+ robot_srvs::KinematicReplanState::request r;
+ r.value = req;
+ m_pr.performCall(r, PlanningRequest::C_ARM);
+ }
+ else
+ {
+ robot_srvs::KinematicPlanState::request r;
+ r.value = req;
+ m_pr.performCall(r, PlanningRequest::C_ARM);
+ }
}
void runRightArmTo0(bool replan = false)
@@ -400,10 +408,19 @@
req.params.volumeMin.x = -5.0 + m_basePos[0]; req.params.volumeMin.y = -5.0 + m_basePos[1]; req.params.volumeMin.z = 0.0;
req.params.volumeMax.x = 5.0 + m_basePos[0]; req.params.volumeMax.y = 5.0 + m_basePos[1]; req.params.volumeMax.z = 0.0;
-
- robot_srvs::KinematicPlanState::request r;
- r.value = req;
- m_pr.performCall(r, PlanningRequest::C_ARM);
+
+ if (replan)
+ {
+ robot_srvs::KinematicReplanState::request r;
+ r.value = req;
+ m_pr.performCall(r, PlanningRequest::C_ARM);
+ }
+ else
+ {
+ robot_srvs::KinematicPlanState::request r;
+ r.value = req;
+ m_pr.performCall(r, PlanningRequest::C_ARM);
+ }
}
void printLinkPoses(void)
@@ -461,9 +478,18 @@
req.params.volumeMax.y = 5.0 + m_basePos[1];
req.params.volumeMax.z = 0.0;
- robot_srvs::KinematicPlanLinkPosition::request r;
- r.value = req;
- m_pr.performCall(r, PlanningRequest::C_ARM);
+ if (replan)
+ {
+ robot_srvs::KinematicReplanLinkPosition::request r;
+ r.value = req;
+ m_pr.performCall(r, PlanningRequest::C_ARM);
+ }
+ else
+ {
+ robot_srvs::KinematicPlanLinkPosition::request r;
+ r.value = req;
+ m_pr.performCall(r, PlanningRequest::C_ARM);
+ }
}
@@ -488,9 +514,9 @@
mk.pitch = 0;
mk.yaw = 0;
- mk.xScale = radius;
- mk.yScale = radius;
- mk.zScale = radius;
+ mk.xScale = radius * 2.0;
+ mk.yScale = radius * 2.0;
+ mk.zScale = radius * 2.0;
mk.alpha = 255;
mk.r = 255;
Modified: pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h 2009-01-24 08:36:18 UTC (rev 10137)
+++ pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h 2009-01-24 08:56:35 UTC (rev 10138)
@@ -38,7 +38,7 @@
#define COLLISION_SPACE_ENVIRONMENT_MODEL_
#include <planning_models/kinematic.h>
-#include "boost/thread/mutex.hpp"
+#include <boost/thread/mutex.hpp>
#include <vector>
#include <string>
@@ -68,6 +68,7 @@
EnvironmentModel(void)
{
m_selfCollision = true;
+ m_verbose = false;
}
virtual ~EnvironmentModel(void)
@@ -76,6 +77,9 @@
delete m_models[i];
}
+ /** Enable/disable verbosity */
+ void setVerbose(bool verbose);
+
/** Check if a model is in collision */
virtual bool isCollision(unsigned int model_id) = 0;
@@ -128,6 +132,7 @@
boost::mutex m_lock;
bool m_selfCollision;
+ bool m_verbose;
/** List of loaded robot models */
std::vector<planning_models::KinematicModel*> m_models;
Modified: pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp 2009-01-24 08:36:18 UTC (rev 10137)
+++ pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp 2009-01-24 08:56:35 UTC (rev 10138)
@@ -36,6 +36,11 @@
#include <collision_space/environment.h>
+void collision_space::EnvironmentModel::setVerbose(bool verbose)
+{
+ m_verbose = verbose;
+}
+
unsigned int collision_space::EnvironmentModel::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links)
{
unsigned int pos = m_models.size();
Modified: pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp 2009-01-24 08:36:18 UTC (rev 10137)
+++ pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp 2009-01-24 08:56:35 UTC (rev 10138)
@@ -36,6 +36,7 @@
#include <collision_space/environmentODE.h>
#include <cassert>
+#include <cstdio>
#include <algorithm>
#include <map>
@@ -364,7 +365,12 @@
dSpaceCollide2(g1, g2, reinterpret_cast<void*>(&cdata), nearCallbackFn);
if (cdata.collides)
+ {
+ if (m_verbose)
+ printf("Self-collision between '%s' and '%s'\n",
+ m_modelsGeom[model_id].linkGeom[vec[j]]->link->name.c_str(), m_modelsGeom[model_id].linkGeom[vec[k]]->link->name.c_str());
goto OUT1;
+ }
}
}
}
@@ -399,7 +405,13 @@
dSpaceCollide2(g1, g2, reinterpret_cast<void*>(&cdata), nearCallbackFn);
if (cdata.collides)
+ {
+ if (m_verbose)
+ printf("Collision between static body and link '%s'\n",
+ m_modelsGeom[model_id].linkGeom[i]->link->name.c_str());
goto OUT2;
+ }
+
}
}
}
@@ -415,8 +427,17 @@
if (m_modelsGeom[model_id].linkGeom[i]->enabled)
{
const unsigned int ng = m_modelsGeom[model_id].linkGeom[i]->geom.size();
- for (unsigned int ig = 0 ; ig < ng && !cdata.collides ; ++ig)
+ for (unsigned int ig = 0 ; ig < ng ; ++ig)
+ {
m_collide2.collide(m_modelsGeom[model_id].linkGeom[i]->geom[ig], reinterpret_cast<void*>(&cdata), nearCallbackFn);
+ if (cdata.collides)
+ {
+ if (m_verbose)
+ printf("Collision between dynamic body and link '%s'\n",
+ m_modelsGeom[model_id].linkGeom[i]->link->name.c_str());
+ break;
+ }
+ }
}
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-01-24 09:40:24
|
Revision: 10139
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10139&view=rev
Author: tfoote
Date: 2009-01-24 09:40:17 +0000 (Sat, 24 Jan 2009)
Log Message:
-----------
r16462@axx (orig r10136): tfoote | 2009-01-24 00:08:55 -0800
review status reflecting deprecation
Modified Paths:
--------------
pkg/trunk/deprecated/cv_wrappers/manifest.xml
pkg/trunk/deprecated/misc_utils/manifest.xml
pkg/trunk/deprecated/mp_benchmarks/manifest.xml
pkg/trunk/deprecated/robot_model/manifest.xml
pkg/trunk/deprecated/sbpl_util/manifest.xml
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10106
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/deprecated/cv_wrappers/manifest.xml
===================================================================
--- pkg/trunk/deprecated/cv_wrappers/manifest.xml 2009-01-24 08:56:35 UTC (rev 10138)
+++ pkg/trunk/deprecated/cv_wrappers/manifest.xml 2009-01-24 09:40:17 UTC (rev 10139)
@@ -17,7 +17,7 @@
<author>Morgan Quigley</author>
<license>BSD</license>
<url>http://pr.willowgarage.com/wiki/cv_wrappers</url>
- <review status="unreviewed" notes=""/>
+ <review status="deprecated" notes=""/>
<depend package="roscpp"/>
<depend package="opencv_latest"/>
<depend package="std_msgs"/>
Modified: pkg/trunk/deprecated/misc_utils/manifest.xml
===================================================================
--- pkg/trunk/deprecated/misc_utils/manifest.xml 2009-01-24 08:56:35 UTC (rev 10138)
+++ pkg/trunk/deprecated/misc_utils/manifest.xml 2009-01-24 09:40:17 UTC (rev 10139)
@@ -6,7 +6,7 @@
</description>
<author>Stuart Glaser <sg...@wi...></author>
<license>BSD</license>
- <review status="unreviewed" notes="API review in progress (Stu)"/>
+ <review status="deprecated" notes=""/>
<url>http://pr.willowgarage.com</url>
<depend package="roscpp" />
<export>
Modified: pkg/trunk/deprecated/mp_benchmarks/manifest.xml
===================================================================
--- pkg/trunk/deprecated/mp_benchmarks/manifest.xml 2009-01-24 08:56:35 UTC (rev 10138)
+++ pkg/trunk/deprecated/mp_benchmarks/manifest.xml 2009-01-24 09:40:17 UTC (rev 10139)
@@ -3,7 +3,7 @@
</description>
<author>Roland Philippsen</author>
<license>BSD</license>
- <review status="unreviewed" notes=""/>
+ <review status="deprecated" notes=""/>
<url>http://pr.willowgarage.com</url>
<depend package="libsunflower"/>
<depend package="nepumuk"/>
Modified: pkg/trunk/deprecated/robot_model/manifest.xml
===================================================================
--- pkg/trunk/deprecated/robot_model/manifest.xml 2009-01-24 08:56:35 UTC (rev 10138)
+++ pkg/trunk/deprecated/robot_model/manifest.xml 2009-01-24 09:40:17 UTC (rev 10139)
@@ -6,7 +6,7 @@
<author>Ioan Sucan/is...@wi...</author>
<license>BSD</license>
- <review status="unreviewed" notes=""/>
+ <review status="deprecated" notes=""/>
<depend package="roscpp"/>
<depend package="std_msgs"/>
Modified: pkg/trunk/deprecated/sbpl_util/manifest.xml
===================================================================
--- pkg/trunk/deprecated/sbpl_util/manifest.xml 2009-01-24 08:56:35 UTC (rev 10138)
+++ pkg/trunk/deprecated/sbpl_util/manifest.xml 2009-01-24 09:40:17 UTC (rev 10139)
@@ -3,7 +3,7 @@
</description>
<author>Roland Philippsen</author>
<license>BSD</license>
- <review status="unreviewed" notes=""/>
+ <review status="deprecated" notes=""/>
<url>http://pr.willowgarage.com</url>
<depend package="std_msgs"/>
<depend package="roscpp"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ve...@us...> - 2009-01-25 06:06:52
|
Revision: 10158
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10158&view=rev
Author: veedee
Date: 2009-01-25 06:06:47 +0000 (Sun, 25 Jan 2009)
Log Message:
-----------
moved the CollisionMap messages into robot_msgs
Modified Paths:
--------------
pkg/trunk/mapping/collision_map/manifest.xml
Added Paths:
-----------
pkg/trunk/mapping/collision_map/collision_map.xml
pkg/trunk/prcore/robot_msgs/msg/CollisionMap.msg
pkg/trunk/prcore/robot_msgs/msg/OrientedBoundingBox.msg
Removed Paths:
-------------
pkg/trunk/mapping/collision_map/collision_map.launch
pkg/trunk/mapping/collision_map/msg/CollisionMap.msg
pkg/trunk/mapping/collision_map/msg/OrientedBoundingBox.msg
Deleted: pkg/trunk/mapping/collision_map/collision_map.launch
===================================================================
--- pkg/trunk/mapping/collision_map/collision_map.launch 2009-01-25 06:05:13 UTC (rev 10157)
+++ pkg/trunk/mapping/collision_map/collision_map.launch 2009-01-25 06:06:47 UTC (rev 10158)
@@ -1,11 +0,0 @@
-<launch>
- <node pkg="collision_map" type="collision_map_node" name="collision_map" respawn="true">
- <remap from="tilt_laser_cloud" to="cloud_1_raw" />
- <param name="object_type" type="int" value="0" />
- <param name="sphere_radius" type="double" value="0.015" />
-
- <param name="robot_max_x" type="double" value="1.5" />
- <param name="robot_max_y" type="double" value="1.5" />
- <param name="robot_max_z" type="double" value="1.5" />
- </node>
-</launch>
Added: pkg/trunk/mapping/collision_map/collision_map.xml
===================================================================
--- pkg/trunk/mapping/collision_map/collision_map.xml (rev 0)
+++ pkg/trunk/mapping/collision_map/collision_map.xml 2009-01-25 06:06:47 UTC (rev 10158)
@@ -0,0 +1,23 @@
+<!-- <launch>
+ <node pkg="collision_map" type="collision_map_node" name="collision_map" respawn="true">
+ <remap from="tilt_laser_cloud" to="cloud_1_raw" />
+
+ <param name="robot_max_x" type="double" value="1.5" />
+ <param name="robot_max_y" type="double" value="1.5" />
+ <param name="robot_max_z" type="double" value="1.5" />
+ </node>
+</launch>-->
+
+<launch>
+ <node pkg="collision_map" type="collision_map_buffer_node" name="collision_map_buffer" respawn="true" output="screen">
+<!-- <remap from="full_cloud" to="cloud_1_raw" />-->
+
+ <param name="robot_max_x" type="double" value="1.5" />
+ <param name="robot_max_y" type="double" value="1.5" />
+ <param name="robot_max_z" type="double" value="1.5" />
+
+ <param name="leaf_width_x" type="double" value="0.015" />
+ <param name="leaf_width_y" type="double" value="0.015" />
+ <param name="leaf_width_z" type="double" value="0.015" />
+ </node>
+</launch>
Modified: pkg/trunk/mapping/collision_map/manifest.xml
===================================================================
--- pkg/trunk/mapping/collision_map/manifest.xml 2009-01-25 06:05:13 UTC (rev 10157)
+++ pkg/trunk/mapping/collision_map/manifest.xml 2009-01-25 06:06:47 UTC (rev 10158)
@@ -13,6 +13,8 @@
<depend package="tf" />
<depend package="cloud_geometry" />
<depend package="eigen" />
+ <depend package="robot_msgs" />
+
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp"/>
</export>
Deleted: pkg/trunk/mapping/collision_map/msg/CollisionMap.msg
===================================================================
--- pkg/trunk/mapping/collision_map/msg/CollisionMap.msg 2009-01-25 06:05:13 UTC (rev 10157)
+++ pkg/trunk/mapping/collision_map/msg/CollisionMap.msg 2009-01-25 06:06:47 UTC (rev 10158)
@@ -1,2 +0,0 @@
-Header header
-OrientedBoundingBox[] boxes
Deleted: pkg/trunk/mapping/collision_map/msg/OrientedBoundingBox.msg
===================================================================
--- pkg/trunk/mapping/collision_map/msg/OrientedBoundingBox.msg 2009-01-25 06:05:13 UTC (rev 10157)
+++ pkg/trunk/mapping/collision_map/msg/OrientedBoundingBox.msg 2009-01-25 06:06:47 UTC (rev 10158)
@@ -1,4 +0,0 @@
-std_msgs/Point32 center
-std_msgs/Point32 extents
-std_msgs/Point32 axis
-float32 angle
Copied: pkg/trunk/prcore/robot_msgs/msg/CollisionMap.msg (from rev 10135, pkg/trunk/mapping/collision_map/msg/CollisionMap.msg)
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/CollisionMap.msg (rev 0)
+++ pkg/trunk/prcore/robot_msgs/msg/CollisionMap.msg 2009-01-25 06:06:47 UTC (rev 10158)
@@ -0,0 +1,2 @@
+Header header
+OrientedBoundingBox[] boxes
Copied: pkg/trunk/prcore/robot_msgs/msg/OrientedBoundingBox.msg (from rev 10135, pkg/trunk/mapping/collision_map/msg/OrientedBoundingBox.msg)
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/OrientedBoundingBox.msg (rev 0)
+++ pkg/trunk/prcore/robot_msgs/msg/OrientedBoundingBox.msg 2009-01-25 06:06:47 UTC (rev 10158)
@@ -0,0 +1,4 @@
+std_msgs/Point32 center
+std_msgs/Point32 extents
+std_msgs/Point32 axis
+float32 angle
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ve...@us...> - 2009-01-25 06:18:14
|
Revision: 10161
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10161&view=rev
Author: veedee
Date: 2009-01-25 06:18:05 +0000 (Sun, 25 Jan 2009)
Log Message:
-----------
moved the service
Modified Paths:
--------------
pkg/trunk/mapping/collision_map/manifest.xml
Added Paths:
-----------
pkg/trunk/prcore/robot_srvs/srv/RecordStaticMapTrigger.srv
Removed Paths:
-------------
pkg/trunk/mapping/collision_map/srv/RememberMap.srv
Modified: pkg/trunk/mapping/collision_map/manifest.xml
===================================================================
--- pkg/trunk/mapping/collision_map/manifest.xml 2009-01-25 06:17:41 UTC (rev 10160)
+++ pkg/trunk/mapping/collision_map/manifest.xml 2009-01-25 06:18:05 UTC (rev 10161)
@@ -14,6 +14,7 @@
<depend package="cloud_geometry" />
<depend package="eigen" />
<depend package="robot_msgs" />
+ <depend package="robot_srvs" />
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp"/>
Deleted: pkg/trunk/mapping/collision_map/srv/RememberMap.srv
===================================================================
--- pkg/trunk/mapping/collision_map/srv/RememberMap.srv 2009-01-25 06:17:41 UTC (rev 10160)
+++ pkg/trunk/mapping/collision_map/srv/RememberMap.srv 2009-01-25 06:18:05 UTC (rev 10161)
@@ -1,3 +0,0 @@
-time map_time
----
-uint8 status
Copied: pkg/trunk/prcore/robot_srvs/srv/RecordStaticMapTrigger.srv (from rev 10158, pkg/trunk/mapping/collision_map/srv/RememberMap.srv)
===================================================================
--- pkg/trunk/prcore/robot_srvs/srv/RecordStaticMapTrigger.srv (rev 0)
+++ pkg/trunk/prcore/robot_srvs/srv/RecordStaticMapTrigger.srv 2009-01-25 06:18:05 UTC (rev 10161)
@@ -0,0 +1,3 @@
+time map_time
+---
+uint8 status
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-01-25 06:52:06
|
Revision: 10173
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10173&view=rev
Author: isucan
Date: 2009-01-25 06:52:02 +0000 (Sun, 25 Jan 2009)
Log Message:
-----------
fixed moved msgs error
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/CMakeLists.txt
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
Added Paths:
-----------
pkg/trunk/prcore/robot_msgs/msg/AttachedObject.msg
Removed Paths:
-------------
pkg/trunk/motion_planning/kinematic_planning/msg/
Modified: pkg/trunk/motion_planning/kinematic_planning/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/CMakeLists.txt 2009-01-25 06:43:52 UTC (rev 10172)
+++ pkg/trunk/motion_planning/kinematic_planning/CMakeLists.txt 2009-01-25 06:52:02 UTC (rev 10173)
@@ -4,8 +4,6 @@
rospack_add_boost_directories()
-genmsg()
-
rospack_add_executable(kinematic_planning src/kinematic_planning.cpp)
rospack_add_executable(motion_validator src/motion_validator.cpp)
rospack_add_executable(state_validity_monitor src/state_validity_monitor.cpp)
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-01-25 06:43:52 UTC (rev 10172)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-01-25 06:52:02 UTC (rev 10173)
@@ -38,9 +38,9 @@
#include <collision_space/environmentODE.h>
#include <std_msgs/PointCloud.h>
+#include <robot_msgs/CollisionMap.h>
+#include <robot_msgs/AttachedObject.h>
#include <robot_srvs/CollisionCheckState.h>
-#include <collision_map/CollisionMap.h>
-#include <kinematic_planning/AttachedObject.h>
/** Main namespace */
namespace kinematic_planning
@@ -199,11 +199,11 @@
protected:
- collision_map::CollisionMap m_collisionMap;
+ robot_msgs::CollisionMap m_collisionMap;
collision_space::EnvironmentModel *m_collisionSpace;
// add or remove objects to be attached to a link
- kinematic_planning::AttachedObject m_attachedObject;
+ robot_msgs::AttachedObject m_attachedObject;
void addSelfCollisionGroups(unsigned int cid, robot_desc::URDF *model)
{
Modified: pkg/trunk/motion_planning/kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-01-25 06:43:52 UTC (rev 10172)
+++ pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-01-25 06:52:02 UTC (rev 10173)
@@ -17,7 +17,6 @@
<depend package="wg_robot_description_parser"/>
<depend package="planning_models"/>
<depend package="collision_space"/>
- <depend package="collision_map"/>
<depend package="ompl" />
Added: pkg/trunk/prcore/robot_msgs/msg/AttachedObject.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/AttachedObject.msg (rev 0)
+++ pkg/trunk/prcore/robot_msgs/msg/AttachedObject.msg 2009-01-25 06:52:02 UTC (rev 10173)
@@ -0,0 +1,7 @@
+# This mesage defines what objects are attached to a link. The objects
+# are assumed to be defined in the reference frame of the link they
+# are attached to
+
+string robot_name
+string link_name
+CollisionMap objects
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-01-25 20:16:06
|
Revision: 10177
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10177&view=rev
Author: isucan
Date: 2009-01-25 20:15:59 +0000 (Sun, 25 Jan 2009)
Log Message:
-----------
option to go to robot frame
Modified Paths:
--------------
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.cpp
pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.cpp 2009-01-25 08:11:24 UTC (rev 10176)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.cpp 2009-01-25 20:15:59 UTC (rev 10177)
@@ -165,6 +165,7 @@
kinematic_model_->setVerbose( false );
kinematic_model_->build( file );
kinematic_model_->defaultState();
+ kinematic_model_->reduceToRobotFrame();
robot_->update( kinematic_model_, target_frame_ );
}
Modified: pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-01-25 08:11:24 UTC (rev 10176)
+++ pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-01-25 20:15:59 UTC (rev 10177)
@@ -548,7 +548,10 @@
void defaultState(void);
void computeTransforms(const double *params, int groupID = -1);
-
+
+ /** Add thansforms to the rootTransform such that the robot is in its planar/floating link frame */
+ bool reduceToRobotFrame(void);
+
void printModelInfo(std::ostream &out = std::cout);
void printLinkPoses(std::ostream &out = std::cout) const;
Modified: pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp 2009-01-25 08:11:24 UTC (rev 10176)
+++ pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp 2009-01-25 20:15:59 UTC (rev 10177)
@@ -322,6 +322,20 @@
{
return new StateParams(this);
}
+
+bool planning_models::KinematicModel::reduceToRobotFrame(void)
+{
+ if (m_robots.size() == 1 && m_robots[0]->floatingJoints.size() + m_robots[0]->planarJoints.size() == 1)
+ {
+ if (m_robots[0]->planarJoints.size())
+ rootTransform *= m_jointMap[parameterValues[m_robots[0]->planarJoints[0]]]->after->constTrans.inverse();
+ else
+ rootTransform *= m_jointMap[parameterValues[m_robots[0]->floatingJoints[0]]]->after->constTrans.inverse();
+ return true;
+ }
+ else
+ return false;
+}
void planning_models::KinematicModel::build(const robot_desc::URDF &model, bool ignoreSensors)
{
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-01-26 08:23:32
|
Revision: 10194
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10194&view=rev
Author: isucan
Date: 2009-01-26 08:23:29 +0000 (Mon, 26 Jan 2009)
Log Message:
-----------
fixed bug in state retrieval
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h
pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-01-26 07:54:56 UTC (rev 10193)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-01-26 08:23:29 UTC (rev 10194)
@@ -156,7 +156,8 @@
m_kmodel->reduceToRobotFrame();
m_robotState = m_kmodel->newStateParams();
-
+ m_robotState->setInRobotFrame();
+
m_haveMechanismState = false;
m_haveBasePos = false;
}
@@ -251,9 +252,22 @@
unsigned int n = m_mechanismState.get_joint_states_size();
for (unsigned int i = 0 ; i < n ; ++i)
{
- double pos = m_mechanismState.joint_states[i].position;
- change = change || m_robotState->setParams(&pos, m_mechanismState.joint_states[i].name);
+ planning_models::KinematicModel::Joint* joint = m_kmodel->getJoint(m_mechanismState.joint_states[i].name);
+ if (joint)
+ {
+ if (joint->usedParams == 1)
+ {
+ double pos = m_mechanismState.joint_states[i].position;
+ bool this_changed = m_robotState->setParams(&pos, m_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
+ ROS_ERROR("Unknown joint: %s", m_mechanismState.joint_states[i].name.c_str());
}
+
if (!m_haveMechanismState)
m_haveMechanismState = m_robotState->seenAll();
}
Modified: pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-01-26 07:54:56 UTC (rev 10193)
+++ pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-01-26 08:23:29 UTC (rev 10194)
@@ -470,6 +470,9 @@
/** Set all the parameters to a given value */
void setAll(const double value);
+ /** Set all planar & floating joints to 0, so that the robot is in its own frame */
+ void setInRobotFrame(void);
+
/** Set the parameters for a given group. Return true if
any change was observed in either of the set
values. */
Modified: pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp 2009-01-26 07:54:56 UTC (rev 10193)
+++ pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp 2009-01-26 08:23:29 UTC (rev 10194)
@@ -669,6 +669,21 @@
}
}
+void planning_models::KinematicModel::StateParams::setInRobotFrame(void)
+{
+ for (unsigned int j = 0 ; j < m_owner->floatingJoints.size() ; ++j)
+ {
+ double vals[7] = {0, 0, 0, 0, 0, 0, 1};
+ setParams(vals, m_name[m_owner->floatingJoints[j]]);
+ }
+
+ for (unsigned int j = 0 ; j < m_owner->planarJoints.size() ; ++j)
+ {
+ double vals[3] = {0, 0, 0};
+ setParams(vals, m_name[m_owner->planarJoints[j]]);
+ }
+}
+
const double* planning_models::KinematicModel::StateParams::getParams(void) const
{
return m_params;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ve...@us...> - 2009-01-26 21:16:25
|
Revision: 10203
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10203&view=rev
Author: veedee
Date: 2009-01-26 21:16:19 +0000 (Mon, 26 Jan 2009)
Log Message:
-----------
added a frame_id to the header
Modified Paths:
--------------
pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp
pkg/trunk/prcore/robot_msgs/msg/ObjectOnTable.msg
Modified: pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp
===================================================================
--- pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp 2009-01-26 19:31:58 UTC (rev 10202)
+++ pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp 2009-01-26 21:16:19 UTC (rev 10203)
@@ -387,6 +387,7 @@
object_indices[nr_p] = object_idx.at (j);
nr_p++;
}
+ table.objects[i].frame_id = table.header.frame_id;
cloud_geometry::getMinMax (points, &object_idx, table.objects[i].min_bound, table.objects[i].max_bound);
cloud_geometry::nearest::computeCentroid (points, &object_idx, table.objects[i].center);
}
Modified: pkg/trunk/prcore/robot_msgs/msg/ObjectOnTable.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/ObjectOnTable.msg 2009-01-26 19:31:58 UTC (rev 10202)
+++ pkg/trunk/prcore/robot_msgs/msg/ObjectOnTable.msg 2009-01-26 21:16:19 UTC (rev 10203)
@@ -1,3 +1,4 @@
+string frame_id
std_msgs/Point32 center
std_msgs/Point32 min_bound
std_msgs/Point32 max_bound
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-26 21:19:25
|
Revision: 10204
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10204&view=rev
Author: sfkwc
Date: 2009-01-26 21:19:22 +0000 (Mon, 26 Jan 2009)
Log Message:
-----------
#890: Deprecating world_3d_map
Added Paths:
-----------
pkg/trunk/deprecated/world_3d_map/
Removed Paths:
-------------
pkg/trunk/world_models/world_3d_map/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-01-27 18:05:58
|
Revision: 10261
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10261&view=rev
Author: hsujohnhsu
Date: 2009-01-27 18:05:54 +0000 (Tue, 27 Jan 2009)
Log Message:
-----------
update to handle mesh as collision model.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp
pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp 2009-01-27 17:41:10 UTC (rev 10260)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp 2009-01-27 18:05:54 UTC (rev 10261)
@@ -153,6 +153,16 @@
*sizeCount = 1;
sizeVals[0] = static_cast<robot_desc::URDF::Link::Geometry::Sphere*>(geometry->shape)->radius;
break;
+ case robot_desc::URDF::Link::Geometry::MESH:
+ type = "trimesh";
+ *sizeCount = 3;
+ {
+ robot_desc::URDF::Link::Geometry::Mesh* mesh = static_cast<robot_desc::URDF::Link::Geometry::Mesh*>(geometry->shape);
+ sizeVals[0] = mesh->scale[0];
+ sizeVals[1] = mesh->scale[1];
+ sizeVals[2] = mesh->scale[2];
+ }
+ break;
default:
*sizeCount = 0;
printf("Unknown body type: %d in geometry '%s'\n", geometry->type, geometry->name.c_str());
@@ -210,8 +220,21 @@
for (int j = 0 ; j < 3 ; ++j)
addKeyValue(geom, tagList2[j], values2str(1, link->inertial->com + j));
- /* set geometry size */
- addKeyValue(geom, "size", values2str(linkGeomSize, linkSize));
+ if (link->collision->geometry->type == robot_desc::URDF::Link::Geometry::MESH)
+ {
+ robot_desc::URDF::Link::Geometry::Mesh* mesh = static_cast<robot_desc::URDF::Link::Geometry::Mesh*>(link->collision->geometry->shape);
+ /* set mesh size or scale */
+ addKeyValue(geom, "scale", values2str(3, mesh->scale));
+
+ /* set mesh file */
+ addKeyValue(geom, "mesh", "models/pr2/" + mesh->filename + ".mesh");
+
+ }
+ else
+ {
+ /* set geometry size */
+ addKeyValue(geom, "size", values2str(linkGeomSize, linkSize));
+ }
/* set additional data */
copyGazeboMap(link->collision->data, geom);
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp 2009-01-27 17:41:10 UTC (rev 10260)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp 2009-01-27 18:05:54 UTC (rev 10261)
@@ -120,7 +120,8 @@
std::string getGeometrySize(robot_desc::URDF::Link::Geometry* geometry, int *sizeCount, double *sizeVals)
{
std::string type;
-
+
+ std::cout << " geometry type: " << geometry->type << std::endl;
switch (geometry->type)
{
case robot_desc::URDF::Link::Geometry::BOX:
@@ -147,6 +148,16 @@
*sizeCount = 1;
sizeVals[0] = static_cast<robot_desc::URDF::Link::Geometry::Sphere*>(geometry->shape)->radius;
break;
+ case robot_desc::URDF::Link::Geometry::MESH:
+ type = "trimesh";
+ *sizeCount = 3;
+ {
+ robot_desc::URDF::Link::Geometry::Mesh* mesh = static_cast<robot_desc::URDF::Link::Geometry::Mesh*>(geometry->shape);
+ sizeVals[0] = mesh->scale[0];
+ sizeVals[1] = mesh->scale[1];
+ sizeVals[2] = mesh->scale[2];
+ }
+ break;
default:
*sizeCount = 0;
printf("Unknown body type: %d in geometry '%s'\n", geometry->type, geometry->name.c_str());
@@ -204,8 +215,21 @@
for (int j = 0 ; j < 3 ; ++j)
addKeyValue(geom, tagList2[j], values2str(1, link->inertial->com + j));
- /* set geometry size */
- addKeyValue(geom, "size", values2str(linkGeomSize, linkSize));
+ if (link->collision->geometry->type == robot_desc::URDF::Link::Geometry::MESH)
+ {
+ robot_desc::URDF::Link::Geometry::Mesh* mesh = static_cast<robot_desc::URDF::Link::Geometry::Mesh*>(link->collision->geometry->shape);
+ /* set mesh size or scale */
+ addKeyValue(geom, "scale", values2str(3, mesh->scale));
+
+ /* set mesh file */
+ addKeyValue(geom, "mesh", "models/pr2/" + mesh->filename + ".mesh");
+
+ }
+ else
+ {
+ /* set geometry size */
+ addKeyValue(geom, "size", values2str(linkGeomSize, linkSize));
+ }
/* set additional data */
copyGazeboMap(link->collision->data, geom);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-01-27 19:30:36
|
Revision: 10266
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10266&view=rev
Author: tfoote
Date: 2009-01-27 19:30:31 +0000 (Tue, 27 Jan 2009)
Log Message:
-----------
switching to 3D particle cloud
Modified Paths:
--------------
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/amcl_player/manifest.xml
pkg/trunk/nav/fake_localization/fake_localization.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
pkg/trunk/nav/nav_view_sdl/nav_view.cpp
Added Paths:
-----------
pkg/trunk/prcore/robot_msgs/msg/ParticleCloud.msg
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2009-01-27 18:09:20 UTC (rev 10265)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2009-01-27 19:30:31 UTC (rev 10266)
@@ -60,7 +60,7 @@
Publishes to (name / type):
- @b "localizedpose"/RobotBase2DOdom : robot's localized map pose. Only the position information is set (no velocity).
-- @b "particlecloud"/ParticleCloud2D : the set of particles being maintained by the filter.
+- @b "particlecloud"/ParticleCloud : the set of particles being maintained by the filter.
<hr>
@@ -99,7 +99,7 @@
// Messages that I need
#include "std_msgs/LaserScan.h"
#include "std_msgs/RobotBase2DOdom.h"
-#include "std_msgs/ParticleCloud2D.h"
+#include "robot_msgs/ParticleCloud.h"
#include "std_msgs/Pose2DFloat32.h"
#include "std_srvs/StaticMap.h"
@@ -145,7 +145,7 @@
// incoming messages
std_msgs::RobotBase2DOdom localizedOdomMsg;
- std_msgs::ParticleCloud2D particleCloudMsg;
+ robot_msgs::ParticleCloud particleCloudMsg;
std_msgs::RobotBase2DOdom odomMsg;
std_msgs::LaserScan laserMsg;
std_msgs::Pose2DFloat32 initialPoseMsg;
@@ -438,7 +438,7 @@
this->tfL = new tf::TransformListener(*this);
advertise<std_msgs::RobotBase2DOdom>("localizedpose",2);
- advertise<std_msgs::ParticleCloud2D>("particlecloud",2);
+ advertise<robot_msgs::ParticleCloud>("particlecloud",2);
//subscribe("odom", odomMsg, &AmclNode::odomReceived,2);
subscribe("scan", laserMsg, &AmclNode::laserReceived,2);
subscribe("initialpose", initialPoseMsg, &AmclNode::initialPoseReceived,2);
@@ -540,9 +540,13 @@
particleCloudMsg.set_particles_size(resp->particles_count);
for(unsigned int i=0;i<resp->particles_count;i++)
{
- particleCloudMsg.particles[i].x = resp->particles[i].pose.px;
+ tf::PoseTFToMsg(tf::Pose(btQuaternion(resp->particles[i].pose.pa, 0, 0), btVector3(resp->particles[i].pose.px, resp->particles[i].pose.py, 0)),
+ particleCloudMsg.particles[i]);
+
+ /* particleCloudMsg.particles[i].x = resp->particles[i].pose.px;
particleCloudMsg.particles[i].y = resp->particles[i].pose.py;
particleCloudMsg.particles[i].th = resp->particles[i].pose.pa;
+ */
}
publish("particlecloud", particleCloudMsg);
delete msg;
Modified: pkg/trunk/nav/amcl_player/manifest.xml
===================================================================
--- pkg/trunk/nav/amcl_player/manifest.xml 2009-01-27 18:09:20 UTC (rev 10265)
+++ pkg/trunk/nav/amcl_player/manifest.xml 2009-01-27 19:30:31 UTC (rev 10266)
@@ -7,4 +7,6 @@
<depend package="player" />
<depend package="tf" />
<depend package="std_srvs" />
+ <depend package="std_msgs" />
+ <depend package="robot_msgs" />
</package>
Modified: pkg/trunk/nav/fake_localization/fake_localization.cpp
===================================================================
--- pkg/trunk/nav/fake_localization/fake_localization.cpp 2009-01-27 18:09:20 UTC (rev 10265)
+++ pkg/trunk/nav/fake_localization/fake_localization.cpp 2009-01-27 19:30:31 UTC (rev 10266)
@@ -59,7 +59,7 @@
Publishes to (name / type):
- @b "localizedpose"/RobotBase2DOdom : robot's localized map pose. Only the position information is set (no velocity).
-- @b "particlecloud"/ParticleCloud2D : fake set of particles being maintained by the filter (one paricle only).
+- @b "particlecloud"/ParticleCloud : fake set of particles being maintained by the filter (one paricle only).
<hr>
@@ -74,7 +74,7 @@
#include <std_msgs/RobotBase2DOdom.h>
#include <std_msgs/PoseWithRatesStamped.h>
-#include <std_msgs/ParticleCloud2D.h>
+#include <robot_msgs/ParticleCloud.h>
#include <std_msgs/Pose2DFloat32.h>
#include <angles/angles.h>
@@ -90,7 +90,7 @@
FakeOdomNode(void) : ros::Node("fake_localization")
{
advertise<std_msgs::RobotBase2DOdom>("localizedpose",1);
- advertise<std_msgs::ParticleCloud2D>("particlecloud",1);
+ advertise<robot_msgs::ParticleCloud>("particlecloud",1);
m_tfServer = new tf::TransformBroadcaster(*this);
@@ -129,7 +129,7 @@
bool m_base_pos_received;
std_msgs::PoseWithRatesStamped m_basePosMsg;
- std_msgs::ParticleCloud2D m_particleCloud;
+ robot_msgs::ParticleCloud m_particleCloud;
std_msgs::RobotBase2DOdom m_currentPos;
std_msgs::Pose2DFloat32 m_iniPos;
@@ -180,7 +180,10 @@
publish("localizedpose", m_currentPos);
// The particle cloud is the current position. Quite convenient.
- m_particleCloud.particles[0] = m_currentPos.pos;
+ std_msgs::Pose pos;
+ tf::PoseTFToMsg(tf::Pose(tf::Quaternion(m_currentPos.pos.th, 0, 0), tf::Vector3(m_currentPos.pos.x, m_currentPos.pos.y, 0)),
+ pos);
+ m_particleCloud.particles[0] = pos;
publish("particlecloud", m_particleCloud);
}
};
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-01-27 18:09:20 UTC (rev 10265)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-01-27 19:30:31 UTC (rev 10266)
@@ -474,8 +474,12 @@
cloud_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
for( int i=0; i < num_particles; ++i)
{
- Ogre::Vector3 pos( cloud_.particles[i].x, cloud_.particles[i].y, 0.0f );
- Ogre::Quaternion orient( Ogre::Quaternion( Ogre::Radian( cloud_.particles[i].th ), Ogre::Vector3::UNIT_Z ) );
+ Ogre::Vector3 pos( cloud_.particles[i].position.x, cloud_.particles[i].position.y, cloud_.particles[i].position.z );
+ tf::Quaternion orientation;
+ tf::QuaternionMsgToTF(cloud_.particles[i].orientation, orientation);
+ double yaw, pitch, roll;
+ btMatrix3x3(orientation).getEulerZYX(yaw, pitch, roll);
+ Ogre::Quaternion orient( Ogre::Quaternion( Ogre::Radian( yaw ), Ogre::Vector3::UNIT_Z ) );
Ogre::Vector3 vertices[8];
vertices[0] = pos;
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-01-27 18:09:20 UTC (rev 10265)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-01-27 19:30:31 UTC (rev 10266)
@@ -32,7 +32,7 @@
#include "nav_view_panel_generated.h"
-#include "std_msgs/ParticleCloud2D.h"
+#include "robot_msgs/ParticleCloud.h"
#include "robot_msgs/Planner2DGoal.h"
#include "std_msgs/Polyline2D.h"
#include "std_msgs/Pose2DFloat32.h"
@@ -86,7 +86,7 @@
@section topic ROS topics
Subscribes to (name/type):
-- @b "particlecloud"/ParticleCloud2D : a set particles from a probabilistic localization system. Rendered is a set of small arrows.
+- @b "particlecloud"robot_msgs/ParticleCloud : a set particles from a probabilistic localization system. Rendered is a set of small arrows.
- @b "gui_path"/Polyline2D : a path from a planner. Rendered as a dashed line.
- @b "gui_laser"/Polyline2D : re-projected laser scan from a planner. Rendered as a set of points.
- @b "local_path"/Polyline2D : local path from a planner. Rendered as a dashed line.
@@ -206,7 +206,7 @@
int map_height_;
Ogre::TexturePtr map_texture_;
- std_msgs::ParticleCloud2D cloud_;
+ robot_msgs::ParticleCloud cloud_;
robot_msgs::Planner2DGoal goal_;
std_msgs::Polyline2D path_line_;
std_msgs::Polyline2D local_path_;
Modified: pkg/trunk/nav/nav_view_sdl/nav_view.cpp
===================================================================
--- pkg/trunk/nav/nav_view_sdl/nav_view.cpp 2009-01-27 18:09:20 UTC (rev 10265)
+++ pkg/trunk/nav/nav_view_sdl/nav_view.cpp 2009-01-27 19:30:31 UTC (rev 10266)
@@ -67,7 +67,7 @@
Subscribes to (name/type):
- @b "odom"/RobotBase2DOdom : the robot's 2D pose. Rendered as a circle with a heading line.
-- @b "particlecloud"/ParticleCloud2D : a set particles from a probabilistic localization system. Rendered is a set of small arrows.
+- @b "particlecloud"robot_msgs/ParticleCloud : a set particles from a probabilistic localization system. Rendered is a set of small arrows.
- @b "gui_path"/Polyline2D : a path from a planner. Rendered as a dashed line.
- @b "gui_laser"/Polyline2D : re-projected laser scan from a planner. Rendered as a set of points.
@@ -97,7 +97,7 @@
#include "tf/transform_listener.h"
// messages and services
-#include "std_msgs/ParticleCloud2D.h"
+#include "robot_msgs/ParticleCloud.h"
#include "robot_msgs/Planner2DGoal.h"
#include "std_msgs/Polyline2D.h"
#include "std_msgs/Pose2DFloat32.h"
@@ -114,7 +114,7 @@
class NavView : public ros::Node, public ros::SDLGL
{
public:
- std_msgs::ParticleCloud2D cloud;
+ robot_msgs::ParticleCloud cloud;
robot_msgs::Planner2DGoal goal;
std_msgs::Polyline2D pathline;
std_msgs::Polyline2D local_path;
@@ -388,8 +388,13 @@
for(unsigned int i=0;i<cloud.get_particles_size();i++)
{
glPushMatrix();
- glTranslatef(cloud.particles[i].x, cloud.particles[i].y, 0);
- glRotatef(cloud.particles[i].th * 180 / M_PI, 0, 0, 1);
+ glTranslatef(cloud.particles[i].position.x, cloud.particles[i].position.y, cloud.particles[i].position.z);
+ tf::Quaternion orientation;
+ tf::QuaternionMsgToTF(cloud.particles[i].orientation, orientation);
+ double yaw, pitch, roll;
+ btMatrix3x3(orientation).getEulerZYX(yaw, pitch, roll);
+
+ glRotatef(yaw * 180 / M_PI, 0, 0, 1);
glColor3f(1.0, 0.0, 0.0);
glBegin(GL_LINE_LOOP);
glVertex2f(0,0);
Added: pkg/trunk/prcore/robot_msgs/msg/ParticleCloud.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/ParticleCloud.msg (rev 0)
+++ pkg/trunk/prcore/robot_msgs/msg/ParticleCloud.msg 2009-01-27 19:30:31 UTC (rev 10266)
@@ -0,0 +1 @@
+std_msgs/Pose[] particles
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-01-27 20:57:09
|
Revision: 10270
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10270&view=rev
Author: tfoote
Date: 2009-01-27 20:57:04 +0000 (Tue, 27 Jan 2009)
Log Message:
-----------
moving VisualizationMarker message to robot msgs from std_msgs ros#447
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp
pkg/trunk/motion_planning/kinematic_planning/src/state_validity_monitor.cpp
pkg/trunk/motion_planning/test_collision_space/manifest.xml
pkg/trunk/motion_planning/test_collision_space/src/test_cs.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.h
pkg/trunk/visualization/ogre_visualizer/src/test/marker_test.cpp
Added Paths:
-----------
pkg/trunk/prcore/robot_msgs/msg/VisualizationMarker.msg
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h 2009-01-27 20:41:49 UTC (rev 10269)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h 2009-01-27 20:57:04 UTC (rev 10270)
@@ -49,7 +49,7 @@
#include <robot_msgs/JointCmd.h>
#include <robot_srvs/GetJointCmd.h>
#include <std_msgs/PointStamped.h>
-#include <std_msgs/VisualizationMarker.h>
+#include <robot_msgs/VisualizationMarker.h>
// Math utils
#include <math.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h 2009-01-27 20:41:49 UTC (rev 10269)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h 2009-01-27 20:57:04 UTC (rev 10270)
@@ -49,7 +49,7 @@
#include <robot_msgs/JointCmd.h>
#include <robot_srvs/GetJointCmd.h>
#include <std_msgs/PointStamped.h>
-#include <std_msgs/VisualizationMarker.h>
+#include <robot_msgs/VisualizationMarker.h>
// Math utils
#include <math.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2009-01-27 20:41:49 UTC (rev 10269)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2009-01-27 20:57:04 UTC (rev 10270)
@@ -194,7 +194,7 @@
//services
node_->advertiseService(service_prefix_ + "/get_command_array", &HeadPanTiltControllerNode::getJointCmd, this);
guard_get_command_array_.set(service_prefix_ + "/get_command_array");
- node_->advertise<std_msgs::VisualizationMarker>( "visualizationMarker", 0 );
+ node_->advertise<robot_msgs::VisualizationMarker>( "visualizationMarker", 0 );
return true;
}
@@ -340,7 +340,7 @@
c_->setJointCmd(pos,names);
- std_msgs::VisualizationMarker marker;
+ robot_msgs::VisualizationMarker marker;
marker.header.frame_id = "stereo";
marker.id = 0;
marker.type = 0;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp 2009-01-27 20:41:49 UTC (rev 10269)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp 2009-01-27 20:57:04 UTC (rev 10270)
@@ -219,7 +219,7 @@
//services
node_->advertiseService(service_prefix_ + "/get_command_array", &HeadServoingControllerNode::getJointCmd, this);
guard_get_command_array_.set(service_prefix_ + "/get_command_array");
- node_->advertise<std_msgs::VisualizationMarker>( "visualizationMarker", 0 );
+ node_->advertise<robot_msgs::VisualizationMarker>( "visualizationMarker", 0 );
return true;
}
@@ -357,7 +357,7 @@
c_->setJointCmd(pos,names);
- std_msgs::VisualizationMarker marker;
+ robot_msgs::VisualizationMarker marker;
marker.header.frame_id = "stereo";
marker.id = 0;
marker.type = 0;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-01-27 20:41:49 UTC (rev 10269)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-01-27 20:57:04 UTC (rev 10270)
@@ -47,7 +47,7 @@
#include "Eigen/LU"
#include "Eigen/Core"
#include "robot_kinematics/robot_kinematics.h"
-#include <std_msgs/VisualizationMarker.h>
+#include <robot_msgs/VisualizationMarker.h>
namespace controller {
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-01-27 20:41:49 UTC (rev 10269)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-01-27 20:57:04 UTC (rev 10270)
@@ -319,7 +319,7 @@
node_->subscribe(topic_ + "/command", wrench_msg_,
&EndeffectorConstraintControllerNode::command, this, 1);
guard_command_.set(topic_ + "/command");
- node_->advertise<std_msgs::VisualizationMarker>( "visualizationMarker", 0 );
+ node_->advertise<robot_msgs::VisualizationMarker>( "visualizationMarker", 0 );
return true;
@@ -334,10 +334,10 @@
if (count%100==0)
{
- std_msgs::VisualizationMarker marker;
+ robot_msgs::VisualizationMarker marker;
marker.header.frame_id = "torso_lift_link";
marker.id = 0;
- marker.type = std_msgs::VisualizationMarker::CUBE;
+ marker.type = robot_msgs::VisualizationMarker::CUBE;
marker.action = 0;
marker.x = 0.6;
marker.y = 0;
@@ -358,10 +358,10 @@
if (count%100==0)
{
- std_msgs::VisualizationMarker marker;
+ robot_msgs::VisualizationMarker marker;
marker.header.frame_id = "torso_lift_link";
marker.id = 1;
- marker.type = std_msgs::VisualizationMarker::SPHERE;
+ marker.type = robot_msgs::VisualizationMarker::SPHERE;
marker.action = 0;
marker.x = 0.6;
marker.y = 0;
Modified: pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp 2009-01-27 20:41:49 UTC (rev 10269)
+++ pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp 2009-01-27 20:57:04 UTC (rev 10270)
@@ -46,7 +46,7 @@
#include <std_msgs/Point.h>
#include <std_msgs/PointCloud.h>
#include <std_msgs/PoseStamped.h>
-#include <std_msgs/VisualizationMarker.h>
+#include <robot_msgs/VisualizationMarker.h>
#include <Eigen/Core>
#include <cloud_geometry/point.h>
@@ -184,7 +184,7 @@
subtract_object_ = false;
m_id_ = 0;
- advertise<std_msgs::VisualizationMarker>("visualizationMarker", 100);
+ advertise<robot_msgs::VisualizationMarker>("visualizationMarker", 100);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/motion_planning/kinematic_planning/src/state_validity_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/state_validity_monitor.cpp 2009-01-27 20:41:49 UTC (rev 10269)
+++ pkg/trunk/motion_planning/kinematic_planning/src/state_validity_monitor.cpp 2009-01-27 20:57:04 UTC (rev 10270)
@@ -89,7 +89,7 @@
#include "kinematic_planning/CollisionSpaceMonitor.h"
#include <std_msgs/Byte.h>
-#include <std_msgs/VisualizationMarker.h>
+#include <robot_msgs/VisualizationMarker.h>
#include <iostream>
#include <sstream>
@@ -109,7 +109,7 @@
id_(0)
{
advertise<std_msgs::Byte>("state_validity", 1);
- advertise<std_msgs::VisualizationMarker>("visualizationMarker", 10240);
+ advertise<robot_msgs::VisualizationMarker>("visualizationMarker", 10240);
}
virtual ~StateValidityMonitor(void)
@@ -168,12 +168,12 @@
void sendPoint(double x, double y, double z, double radius, const rostools::Header &header)
{
- std_msgs::VisualizationMarker mk;
+ robot_msgs::VisualizationMarker mk;
mk.header = header;
mk.id = id_++;
- mk.type = std_msgs::VisualizationMarker::SPHERE;
- mk.action = std_msgs::VisualizationMarker::ADD;
+ mk.type = robot_msgs::VisualizationMarker::SPHERE;
+ mk.action = robot_msgs::VisualizationMarker::ADD;
mk.x = x;
mk.y = y;
mk.z = z;
@@ -196,12 +196,12 @@
void delPoint(int id, const rostools::Header &header)
{
- std_msgs::VisualizationMarker mk;
+ robot_msgs::VisualizationMarker mk;
mk.header = header;
mk.id = id;
- mk.type = std_msgs::VisualizationMarker::SPHERE;
- mk.action = std_msgs::VisualizationMarker::DELETE;
+ mk.type = robot_msgs::VisualizationMarker::SPHERE;
+ mk.action = robot_msgs::VisualizationMarker::DELETE;
publish("visualizationMarker", mk);
}
Modified: pkg/trunk/motion_planning/test_collision_space/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/test_collision_space/manifest.xml 2009-01-27 20:41:49 UTC (rev 10269)
+++ pkg/trunk/motion_planning/test_collision_space/manifest.xml 2009-01-27 20:57:04 UTC (rev 10270)
@@ -12,6 +12,7 @@
<depend package="rostime"/>
<depend package="tf"/>
<depend package="std_msgs"/>
+ <depend package="robot_msgs"/>
<depend package="collision_space"/>
<depend package="random_utils"/>
Modified: pkg/trunk/motion_planning/test_collision_space/src/test_cs.cpp
===================================================================
--- pkg/trunk/motion_planning/test_collision_space/src/test_cs.cpp 2009-01-27 20:41:49 UTC (rev 10269)
+++ pkg/trunk/motion_planning/test_collision_space/src/test_cs.cpp 2009-01-27 20:57:04 UTC (rev 10270)
@@ -39,7 +39,7 @@
#include <ros/time.h>
#include <collision_space/environmentODE.h>
#include <algorithm>
-#include <std_msgs/VisualizationMarker.h>
+#include <robot_msgs/VisualizationMarker.h>
#include <rostools/Time.h>
#include <tf/transform_broadcaster.h>
#include <collision_space/util.h>
@@ -55,7 +55,7 @@
TestVM(void) : ros::Node("TVM")
{
- std_msgs::VisualizationMarker mk;
+ robot_msgs::VisualizationMarker mk;
advertise("visualizationMarker",
mk,
&TestVM::subCb,
@@ -86,15 +86,15 @@
void sendPoint(double x, double y, double z)
{
- std_msgs::VisualizationMarker mk;
+ robot_msgs::VisualizationMarker mk;
mk.header.stamp = m_tm;
mk.header.frame_id = "map";
mk.id = m_id++;
- mk.type = std_msgs::VisualizationMarker::SPHERE;
- mk.action = std_msgs::VisualizationMarker::ADD;
+ mk.type = robot_msgs::VisualizationMarker::SPHERE;
+ mk.action = robot_msgs::VisualizationMarker::ADD;
mk.x = x;
mk.y = y;
mk.z = z;
@@ -129,7 +129,7 @@
}
void setShapeTransformAndMarker(collision_space::bodies::Shape *s,
- std_msgs::VisualizationMarker &mk)
+ robot_msgs::VisualizationMarker &mk)
{
btTransform t;
@@ -151,7 +151,7 @@
mk.header.frame_id = "map";
mk.id = m_id++;
- mk.action = std_msgs::VisualizationMarker::ADD;
+ mk.action = robot_msgs::VisualizationMarker::ADD;
mk.x = x;
mk.y = y;
@@ -175,10 +175,10 @@
for (int i = 0 ; i < TEST_TIMES ; ++i)
{
- std_msgs::VisualizationMarker mk;
+ robot_msgs::VisualizationMarker mk;
setShapeTransformAndMarker(s, mk);
- mk.type = std_msgs::VisualizationMarker::SPHERE;
+ mk.type = robot_msgs::VisualizationMarker::SPHERE;
mk.xScale = radius[0]*2.0;
mk.yScale = radius[0]*2.0;
mk.zScale = radius[0]*2.0;
@@ -199,10 +199,10 @@
for (int i = 0 ; i < TEST_TIMES ; ++i)
{
- std_msgs::VisualizationMarker mk;
+ robot_msgs::VisualizationMarker mk;
setShapeTransformAndMarker(s, mk);
- mk.type = std_msgs::VisualizationMarker::CUBE;
+ mk.type = robot_msgs::VisualizationMarker::CUBE;
mk.xScale = dims[0]; // length
mk.yScale = dims[1]; // width
mk.zScale = dims[2]; // height
@@ -223,10 +223,10 @@
for (int i = 0 ; i < TEST_TIMES ; ++i)
{
- std_msgs::VisualizationMarker mk;
+ robot_msgs::VisualizationMarker mk;
setShapeTransformAndMarker(s, mk);
- mk.type = std_msgs::VisualizationMarker::CUBE;
+ mk.type = robot_msgs::VisualizationMarker::CUBE;
mk.xScale = dims[1] * 2.0; // radius
mk.yScale = dims[1] * 2.0; // radius
mk.zScale = dims[0]; //length
Added: pkg/trunk/prcore/robot_msgs/msg/VisualizationMarker.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/VisualizationMarker.msg (rev 0)
+++ pkg/trunk/prcore/robot_msgs/msg/VisualizationMarker.msg 2009-01-27 20:57:04 UTC (rev 10270)
@@ -0,0 +1,35 @@
+#
+# Visualization Marker message
+# For inserting pre-made objects into the visualization
+#
+
+byte ARROW=0
+byte CUBE=1
+byte SPHERE=2
+byte ROBOT=3
+byte LINE_STRIP=4
+byte CYLINDER=5
+
+byte ADD=0
+byte MODIFY=0
+byte DELETE=2
+
+Header header # header for time/frame information
+int32 id # object ID useful for manipulating and deleting the object later
+int32 type # Type of object
+int32 action # 0 add/modify an object, 1 (deprecated), 2 deletes an object
+float32 x # x location of the object
+float32 y # y location of the object
+float32 z # z location of the object
+float32 roll # roll of the object
+float32 pitch # pitch of the object
+float32 yaw # yaw of the object
+float32 xScale # scale of the object in the x direction (1 does not change size) (default sizes are usually 1 meter square)
+float32 yScale # scale in y direction
+float32 zScale # scale in z direction
+int32 alpha # alpha (0-255) (amount of transparency, 255 is opaque)
+int32 r # red color value (0-255) (if available)
+int32 g # green color value
+int32 b # blue color value
+#These are only used if the object type is LINE_STRIP
+std_msgs/Position[] points
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.cpp 2009-01-27 20:41:49 UTC (rev 10269)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.cpp 2009-01-27 20:57:04 UTC (rev 10270)
@@ -59,7 +59,7 @@
kinematic_model_ = new planning_models::KinematicModel();
kinematic_model_->setVerbose( false );
- notifier_ = new tf::MessageNotifier<std_msgs::VisualizationMarker>(tf_, ros_node_, boost::bind(&MarkerDisplay::incomingMarker, this, _1), "", "", 100);
+ notifier_ = new tf::MessageNotifier<robot_msgs::VisualizationMarker>(tf_, ros_node_, boost::bind(&MarkerDisplay::incomingMarker, this, _1), "", "", 100);
}
MarkerDisplay::~MarkerDisplay()
@@ -138,11 +138,11 @@
{
switch ( message->action )
{
- case std_msgs::VisualizationMarker::ADD:
+ case robot_msgs::VisualizationMarker::ADD:
processAdd( message );
break;
- case std_msgs::VisualizationMarker::DELETE:
+ case robot_msgs::VisualizationMarker::DELETE:
processDelete( message );
break;
@@ -178,7 +178,7 @@
{
switch ( message->type )
{
- case std_msgs::VisualizationMarker::CUBE:
+ case robot_msgs::VisualizationMarker::CUBE:
{
ogre_tools::Shape* cube = new ogre_tools::Shape( ogre_tools::Shape::Cube, scene_manager_, scene_node_ );
@@ -186,7 +186,7 @@
}
break;
- case std_msgs::VisualizationMarker::CYLINDER:
+ case robot_msgs::VisualizationMarker::CYLINDER:
{
ogre_tools::Shape* cylinder = new ogre_tools::Shape( ogre_tools::Shape::Cylinder, scene_manager_, scene_node_ );
@@ -194,7 +194,7 @@
}
break;
- case std_msgs::VisualizationMarker::SPHERE:
+ case robot_msgs::VisualizationMarker::SPHERE:
{
ogre_tools::Shape* sphere = new ogre_tools::Shape( ogre_tools::Shape::Sphere, scene_manager_, scene_node_ );
@@ -202,13 +202,13 @@
}
break;
- case std_msgs::VisualizationMarker::ARROW:
+ case robot_msgs::VisualizationMarker::ARROW:
{
object = new ogre_tools::Arrow( scene_manager_, scene_node_, 0.8, 0.5, 0.2, 1.0 );
}
break;
- case std_msgs::VisualizationMarker::ROBOT:
+ case robot_msgs::VisualizationMarker::ROBOT:
{
Robot* robot = new Robot( scene_manager_ );
robot->load( urdf_, false, true );
@@ -218,7 +218,7 @@
}
break;
- case std_msgs::VisualizationMarker::LINE_STRIP:
+ case robot_msgs::VisualizationMarker::LINE_STRIP:
{
ogre_tools::BillboardLine* line = new ogre_tools::BillboardLine( scene_manager_, scene_node_ );
object = line;
@@ -292,7 +292,7 @@
object->setColor( message->r / 255.0f, message->g / 255.0f, message->b / 255.0f, message->alpha / 255.0f );
object->setUserData( Ogre::Any( (void*)this ) );
- if ( message->type == std_msgs::VisualizationMarker::LINE_STRIP )
+ if ( message->type == robot_msgs::VisualizationMarker::LINE_STRIP )
{
ogre_tools::BillboardLine* line = dynamic_cast<ogre_tools::BillboardLine*>(object);
ROS_ASSERT( line );
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.h 2009-01-27 20:41:49 UTC (rev 10269)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.h 2009-01-27 20:57:04 UTC (rev 10270)
@@ -34,7 +34,7 @@
#include <map>
-#include <std_msgs/VisualizationMarker.h>
+#include <robot_msgs/VisualizationMarker.h>
#include <boost/thread/mutex.hpp>
#include <boost/shared_ptr.hpp>
@@ -76,7 +76,7 @@
* \class MarkerDisplay
* \brief Displays "markers" sent in by other ROS nodes on the "visualizationMarker" topic
*
- * Markers come in as std_msgs::VisualizationMarker messages. See the VisualizationMarker message for more information.
+ * Markers come in as robot_msgs::VisualizationMarker messages. See the VisualizationMarker message for more information.
*/
class MarkerDisplay : public Display
{
@@ -114,7 +114,7 @@
*/
void clearMarkers();
- typedef boost::shared_ptr<std_msgs::VisualizationMarker> MarkerPtr;
+ typedef boost::shared_ptr<robot_msgs::VisualizationMarker> MarkerPtr;
/**
* \brief Processes a marker message
@@ -150,7 +150,7 @@
, message_(message)
{}
ogre_tools::Object* object_;
- boost::shared_ptr<std_msgs::VisualizationMarker> message_;
+ boost::shared_ptr<robot_msgs::VisualizationMarker> message_;
};
typedef std::map<int, MarkerInfo> M_IDToMarker;
@@ -166,7 +166,7 @@
robot_desc::URDF* urdf_;
planning_models::KinematicModel* kinematic_model_;
- tf::MessageNotifier<std_msgs::VisualizationMarker>* notifier_;
+ tf::MessageNotifier<robot_msgs::VisualizationMarker>* notifier_;
};
} // namespace ogre_vis
Modified: pkg/trunk/visualization/ogre_visualizer/src/test/marker_test.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/test/marker_test.cpp 2009-01-27 20:41:49 UTC (rev 10269)
+++ pkg/trunk/visualization/ogre_visualizer/src/test/marker_test.cpp 2009-01-27 20:57:04 UTC (rev 10270)
@@ -1,6 +1,6 @@
#include "ros/node.h"
-#include "std_msgs/VisualizationMarker.h"
+#include "robot_msgs/VisualizationMarker.h"
int main( int argc, char** argv )
{
@@ -13,17 +13,17 @@
usleep( 10000 );
}
- node->advertise<std_msgs::VisualizationMarker>( "visualizationMarker", 0 );
+ node->advertise<robot_msgs::VisualizationMarker>( "visualizationMarker", 0 );
usleep( 1000000 );
for ( int i = -50; i < 50; ++i )
{
- std_msgs::VisualizationMarker marker;
+ robot_msgs::VisualizationMarker marker;
marker.header.frame_id = "map";
marker.header.stamp = ros::Time();
marker.id = i;
- marker.type = std_msgs::VisualizationMarker::ARROW;
+ marker.type = robot_msgs::VisualizationMarker::ARROW;
marker.action = 0;
marker.x = 1;
marker.y = (i*2);
@@ -41,11 +41,11 @@
node->publish( "visualizationMarker", marker );
}
- std_msgs::VisualizationMarker line_marker;
+ robot_msgs::VisualizationMarker line_marker;
line_marker.header.frame_id = "map";
line_marker.header.stamp = ros::Time();
line_marker.id = 1000;
- line_marker.type = std_msgs::VisualizationMarker::LINE_STRIP;
+ line_marker.type = robot_msgs::VisualizationMarker::LINE_STRIP;
line_marker.action = 0;
line_marker.x = 0;
line_marker.y = 0;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-01-27 23:30:19
|
Revision: 10295
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10295&view=rev
Author: hsujohnhsu
Date: 2009-01-27 23:30:15 +0000 (Tue, 27 Jan 2009)
Log Message:
-----------
* udpate gripper limit, friction parameters.
* update grasp sim demo.
* added a 10khz world.
* update gripper controller gains in sim.
* update table object friction coefs and mass.
Modified Paths:
--------------
pkg/trunk/demos/arm_gazebo/grasp_preprogrammed.py
pkg/trunk/demos/arm_gazebo/l_arm_default_controller.xml
pkg/trunk/demos/arm_gazebo/r_arm_default_controller.xml
pkg/trunk/demos/examples_gazebo/table_defs/table_defs.xml
pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml
Added Paths:
-----------
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/empty_10000hz.world
Modified: pkg/trunk/demos/arm_gazebo/grasp_preprogrammed.py
===================================================================
--- pkg/trunk/demos/arm_gazebo/grasp_preprogrammed.py 2009-01-27 23:09:00 UTC (rev 10294)
+++ pkg/trunk/demos/arm_gazebo/grasp_preprogrammed.py 2009-01-27 23:30:15 UTC (rev 10295)
@@ -57,9 +57,10 @@
CMD_POS_5 = 0.0
CMD_POS_6 = 0.0
CMD_POS_7 = 0.0
-CMD_POS_8 = 0.5
LOWER_Z = 0.1
PAN_RAD = 0.2
+G_OPEN = 0.5;
+G_CLOSE = 0.25;
p3d_received = False
@@ -81,45 +82,45 @@
#open gripper
pub_r_arm.publish(JointPosCmd(['r_shoulder_pan_joint','r_shoulder_lift_joint','r_upper_arm_roll_joint','r_elbow_flex_joint','r_forearm_roll_joint','r_wrist_flex_joint','r_wrist_roll_joint'],[CMD_POS_1,CMD_POS_2,CMD_POS_3,CMD_POS_4,CMD_POS_5,CMD_POS_6,CMD_POS_7],[0,0,0,0,0,0,0],0))
- pub_r_gripper.publish(Float64(CMD_POS_8))
+ pub_r_gripper.publish(Float64(G_OPEN))
time.sleep(8)
#lower arm
pub_r_arm.publish(JointPosCmd(['r_shoulder_pan_joint','r_shoulder_lift_joint','r_upper_arm_rolr_joint','r_elbow_flex_joint','r_forearm_roll_joint','r_wrist_flex_joint','r_wrist_roll_joint'],[CMD_POS_1,CMD_POS_2+LOWER_Z,CMD_POS_3,CMD_POS_4,CMD_POS_5,CMD_POS_6,CMD_POS_7],[0,0,0,0,0,0,0],0))
- pub_r_gripper.publish(Float64(CMD_POS_8))
+ pub_r_gripper.publish(Float64(G_OPEN))
time.sleep(2)
#close gripper
pub_r_arm.publish(JointPosCmd(['r_shoulder_pan_joint','r_shoulder_lift_joint','r_upper_arm_rolr_joint','r_elbow_flex_joint','r_forearm_roll_joint','r_wrist_flex_joint','r_wrist_roll_joint'],[CMD_POS_1,CMD_POS_2+LOWER_Z,CMD_POS_3,CMD_POS_4,CMD_POS_5,CMD_POS_6,CMD_POS_7],[0,0,0,0,0,0,0],0))
- pub_r_gripper.publish(Float64(0.0))
+ pub_r_gripper.publish(Float64(G_CLOSE))
time.sleep(8)
#raise arm
pub_r_arm.publish(JointPosCmd(['r_shoulder_pan_joint','r_shoulder_lift_joint','r_upper_arm_roll_joint','r_elbow_flex_joint','r_forearm_roll_joint','r_wrist_flex_joint','r_wrist_roll_joint'],[CMD_POS_1,CMD_POS_2,CMD_POS_3,CMD_POS_4,CMD_POS_5,CMD_POS_6,CMD_POS_7],[0,0,0,0,0,0,0],0))
- pub_r_gripper.publish(Float64(0.0))
+ pub_r_gripper.publish(Float64(G_CLOSE))
time.sleep(2)
#pan arm
pub_r_arm.publish(JointPosCmd(['r_shoulder_pan_joint','r_shoulder_lift_joint','r_upper_arm_roll_joint','r_elbow_flex_joint','r_forearm_roll_joint','r_wrist_flex_joint','r_wrist_roll_joint'],[CMD_POS_1+PAN_RAD,CMD_POS_2,CMD_POS_3,CMD_POS_4,CMD_POS_5,CMD_POS_6,CMD_POS_7],[0,0,0,0,0,0,0],0))
- pub_r_gripper.publish(Float64(0.0))
+ pub_r_gripper.publish(Float64(G_CLOSE))
time.sleep(2)
#lower arm
pub_r_arm.publish(JointPosCmd(['r_shoulder_pan_joint','r_shoulder_lift_joint','r_upper_arm_roll_joint','r_elbow_flex_joint','r_forearm_roll_joint','r_wrist_flex_joint','r_wrist_roll_joint'],[CMD_POS_1+PAN_RAD,CMD_POS_2+LOWER_Z,CMD_POS_3,CMD_POS_4,CMD_POS_5,CMD_POS_6,CMD_POS_7],[0,0,0,0,0,0,0],0))
- pub_r_gripper.publish(Float64(0.0))
+ pub_r_gripper.publish(Float64(G_CLOSE))
time.sleep(2)
#open gripper
pub_r_arm.publish(JointPosCmd(['r_shoulder_pan_joint','r_shoulder_lift_joint','r_upper_arm_rolr_joint','r_elbow_flex_joint','r_forearm_roll_joint','r_wrist_flex_joint','r_wrist_roll_joint'],[CMD_POS_1+PAN_RAD,CMD_POS_2+LOWER_Z,CMD_POS_3,CMD_POS_4,CMD_POS_5,CMD_POS_6,CMD_POS_7],[0,0,0,0,0,0,0],0))
- pub_r_gripper.publish(Float64(CMD_POS_8))
+ pub_r_gripper.publish(Float64(G_OPEN))
time.sleep(8)
#raise arm
pub_r_arm.publish(JointPosCmd(['r_shoulder_pan_joint','r_shoulder_lift_joint','r_upper_arm_roll_joint','r_elbow_flex_joint','r_forearm_roll_joint','r_wrist_flex_joint','r_wrist_roll_joint'],[CMD_POS_1+PAN_RAD,CMD_POS_2,CMD_POS_3,CMD_POS_4,CMD_POS_5,CMD_POS_6,CMD_POS_7],[0,0,0,0,0,0,0],0))
- pub_r_gripper.publish(Float64(CMD_POS_8))
+ pub_r_gripper.publish(Float64(G_OPEN))
time.sleep(2)
#return arm
pub_r_arm.publish(JointPosCmd(['r_shoulder_pan_joint','r_shoulder_lift_joint','r_upper_arm_roll_joint','r_elbow_flex_joint','r_forearm_roll_joint','r_wrist_flex_joint','r_wrist_roll_joint'],[CMD_POS_1,CMD_POS_2,CMD_POS_3,CMD_POS_4,CMD_POS_5,CMD_POS_6,CMD_POS_7],[0,0,0,0,0,0,0],0))
- pub_r_gripper.publish(Float64(CMD_POS_8))
+ pub_r_gripper.publish(Float64(G_OPEN))
Modified: pkg/trunk/demos/arm_gazebo/l_arm_default_controller.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/l_arm_default_controller.xml 2009-01-27 23:09:00 UTC (rev 10294)
+++ pkg/trunk/demos/arm_gazebo/l_arm_default_controller.xml 2009-01-27 23:30:15 UTC (rev 10295)
@@ -64,7 +64,7 @@
</controller>
<controller name="l_gripper_controller" topic="l_gripper_controller" type="JointPositionControllerNode">
<joint name="l_gripper_l_finger_joint">
- <pid p="0.5" d="0.000001" i="0.001" iClamp="0.01" />
+ <pid p="10.0" d="0.0" i="0.01" iClamp="0.1" />
</joint>
</controller>
</controllers>
Modified: pkg/trunk/demos/arm_gazebo/r_arm_default_controller.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm_default_controller.xml 2009-01-27 23:09:00 UTC (rev 10294)
+++ pkg/trunk/demos/arm_gazebo/r_arm_default_controller.xml 2009-01-27 23:30:15 UTC (rev 10295)
@@ -64,7 +64,7 @@
</controller>
<controller name="r_gripper_controller" topic="r_gripper_controller" type="JointPositionControllerNode">
<joint name="r_gripper_l_finger_joint">
- <pid p="0.5" d="0.000001" i="0.001" iClamp="0.01" />
+ <pid p="10.0" d="0.0" i="0.01" iClamp="0.1" />
</joint>
</controller>
</controllers>
Modified: pkg/trunk/demos/examples_gazebo/table_defs/table_defs.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/table_defs/table_defs.xml 2009-01-27 23:09:00 UTC (rev 10294)
+++ pkg/trunk/demos/examples_gazebo/table_defs/table_defs.xml 2009-01-27 23:30:15 UTC (rev 10295)
@@ -21,10 +21,12 @@
<property name="table_y" value="0.0" />
<property name="table_z" value="0.0" />
- <property name="object_1_height" value="0.12" />
- <property name="object_1_radius" value="0.02" />
+ <property name="object_1_height" value="0.12" />
+ <property name="object_1_radius" value="0.02" />
<property name="object_1_base_height" value="0.02" />
- <property name="object_1_base_width" value="0.15" />
+ <property name="object_1_base_width" value="0.15" />
+ <property name="object_1_mass" value="0.1" />
+ <property name="object_1_mu" value="50.0" />
<property name="M_PI" value="3.1415926535897931" />
@@ -201,11 +203,11 @@
<origin xyz="0 0 ${object_1_base_height}" rpy="0 0 0" />
<joint name="object_1_joint" />
<inertial>
- <mass value="0.01" />
+ <mass value="${object_1_mass/2.0}" />
<com xyz="0 0 ${object_1_height/2}" />
- <inertia ixx="0.001" ixy="0" ixz="0"
- iyy="0.001" iyz="0"
- izz="0.0001" />
+ <inertia ixx="${object_1_mass/20.0}" ixy="0" ixz="0"
+ iyy="${object_1_mass/20.0}" iyz="0"
+ izz="${object_1_mass/200.}" />
</inertial>
<visual>
<origin xyz="0.0 0.0 ${object_1_height/2}" rpy="0 0 0" />
@@ -223,8 +225,9 @@
</geometry>
</collision>
<map name="object_1_collision" flag="gazebo">
- <elem key="mu1" value="5000.0" />
- <elem key="mu2" value="5000.0" />
+ <elem key="selfCollide">true</elem>
+ <elem key="mu1" value="object_1_mu" />
+ <elem key="mu2" value="object_1_mu" />
<elem key="kp" value="100000000.0" />
<elem key="kd" value="1.0" />
</map>
@@ -234,11 +237,11 @@
<origin xyz="${table_x} ${table_y} ${table_z+table_height+0.03}" rpy="0 0 0" />
<joint name="object_1_base_joint" />
<inertial>
- <mass value="0.01" />
+ <mass value="${object_1_mass/2.0}" />
<com xyz="0 0 ${object_1_base_height/2}" />
- <inertia ixx="0.001" ixy="0" ixz="0"
- iyy="0.001" iyz="0"
- izz="0.0001" />
+ <inertia ixx="${object_1_mass/20.0}" ixy="0" ixz="0"
+ iyy="${object_1_mass/20.0}" iyz="0"
+ izz="${object_1_mass/200.}" />
</inertial>
<visual>
<origin xyz="0.0 0.0 ${object_1_base_height/2}" rpy="0 0 0" />
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/empty_10000hz.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/empty_10000hz.world (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/empty_10000hz.world 2009-01-27 23:30:15 UTC (rev 10295)
@@ -0,0 +1,130 @@
+<?xml version="1.0"?>
+
+<gazebo:world
+ xmlns:xi="http://www.w3.org/2001/XInclude"
+ xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+ xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+ xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window"
+ xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param"
+ xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+ xmlns:geo="http://willowgarage.com/xmlschema/#geo"
+ xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+ xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+ xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui"
+ xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
+ xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" >
+
+ <verbosity>5</verbosity>
+
+ <!-- cfm is 1e-5 for single precision -->
+ <!-- erp is typically .1-.8 -->
+ <!-- here's the global contact cfm/erp -->
+ <physics:ode>
+ <stepTime>0.0001</stepTime>
+ <gravity>0 0 -9.8</gravity>
+ <cfm>0.0000000001</cfm>
+ <erp>0.2</erp>
+ <quickStep>true</quickStep>
+ <quickStepIters>3</quickStepIters>
+ <quickStepW>1.3</quickStepW>
+ </physics:ode>
+
+ <geo:origin>
+ <lat>37.4270909558</lat><lon>-122.077919338</lon>
+ </geo:origin>
+
+ <rendering:gui>
+ <type>fltk</type>
+ <size>1024 800</size>
+ <pos>0 0</pos>
+ <frames>
+ <row height="100%">
+ <camera width="100%">
+ <xyz>0.3 0 3</xyz>
+ <rpy>0 90 90</rpy>
+ </camera>
+ </row>
+ </frames>
+ </rendering:gui>
+
+
+ <rendering:ogre>
+ <ambient>1.0 1.0 1.0 1.0</ambient>
+ <sky>
+ <material>Gazebo/CloudySky</material>
+ </sky>
+ <gazeboPath>media</gazeboPath>
+ <grid>false</grid>
+ <maxUpdateRate>10.</maxUpdateRate>
+ </rendering:ogre>
+
+ <model:physical name="gplane">
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <static>true</static>
+
+ <body:plane name="plane">
+ <geom:plane name="plane">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
+ <normal>0 0 1</normal>
+ <size>51.3 51.3</size>
+ <material>Gazebo/Rocky</material>
+ </geom:plane>
+ </body:plane>
+ </model:physical>
+
+ <model:empty name="factory_model">
+ <controller:factory name="factory_controller">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <interface:factory name="factory_iface"/>
+ </controller:factory>
+ </model:empty>
+
+<!--
+ <model:physical name="walls">
+ <include embedded="false">
+ <xi:include href="tests/willow-walls.model" />
+ </include>
+ </model:physical>
+-->
+<!--
+ <model:physical name="willow_map">
+ <xyz>-25.65 25.65 1.0</xyz>
+ <rpy>180 0 0</rpy>
+ <static>true</static>
+ <body:map name="willow_map_body">
+ <geom:map name="willow_map_geom">
+ <image>willowMap.png</image>
+ <threshold>200</threshold>
+ <granularity>1</granularity>
+ <negative>false</negative>
+ <scale>0.1</scale>
+ <offset>0 0 0</offset>
+ <material>Gazebo/Rocky</material>
+ </geom:map>
+ </body:map>
+ </model:physical>
+-->
+
+
+ <!-- White Directional light -->
+ <!--
+ <model:renderable name="directional_white">
+ <light>
+ <type>directional</type>
+ <direction>0 -0.5 -0.5</direction>
+ <diffuseColor>0.4 0.4 0.4</diffuseColor>
+ <specularColor>0.0 0.0 0.0</specularColor>
+ <attenuation>1 0.0 1.0 0.4</attenuation>
+ </light>
+ </model:renderable>
+ -->
+
+
+</gazebo:world>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml 2009-01-27 23:09:00 UTC (rev 10294)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml 2009-01-27 23:30:15 UTC (rev 10295)
@@ -8,6 +8,9 @@
<property name="gripper_max_angle" value="0.548" />
<property name="gripper_min_angle" value="0.05" />
+ <property name="finger_tip_mu" value="50.0" />
+ <property name="finger_joint_effort_limit" value="10.0" />
+
<macro name="pr2_finger" params="prefix parent">
<!-- Finger proximal digit -->
@@ -15,10 +18,10 @@
<joint name="${prefix}_l_finger_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
- <calibration reference_position="0" values="1.5 -1.0" />
- <limit effort="10" min="${gripper_min_angle*0}" max="${gripper_max_angle}"
- k_position="1.0" k_velocity="0.1" velocity="0.5"
- safety_length_min="0.05" safety_length_max="0.1" />
+ <calibration reference_position="0" values="1.0 -1.0" />
+ <limit effort="${finger_joint_effort_limit}" min="${gripper_min_angle*0}" max="${gripper_max_angle}"
+ k_position="1.0" k_velocity="1.0" velocity="0.5"
+ safety_length_min="0.0" safety_length_max="0.0" />
<joint_properties damping="0.2" />
</joint>
@@ -79,16 +82,16 @@
<joint name="${prefix}_r_finger_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
- <calibration reference_position="0" values="1.5 -1.0" />
- <limit effort="10" min="${-gripper_max_angle}" max="${-gripper_min_angle*0}"
- k_position="0" k_velocity="0" velocity="0.5"
+ <calibration reference_position="0" values="1.0 -1.0" />
+ <limit effort="${finger_joint_effort_limit}" min="${-gripper_max_angle}" max="${-gripper_min_angle*0}"
+ k_position="0" k_velocity="0" velocity="0.0"
safety_length_min="0.0" safety_length_max="0.0" />
<joint_properties damping="0.2" />
<mimic name="${prefix}_l_finger_joint" mult="${-1}" offset="0"/>
<map name="${prefix}_r_finger_mimic" flag="gazebo">
- <elem key="mimicKp">1.0</elem>
- <elem key="mimicKd">1.0</elem>
- <elem key="mimicFMax">50.0</elem> <!-- ode bug: this force is set for _l_finger_joint -->
+ <elem key="mimicKp" value="20.0" />
+ <elem key="mimicKd" value="10.0" />
+ <elem key="mimicFMax" value="10.0" />
</map>
</joint>
@@ -149,16 +152,16 @@
<joint name="${prefix}_l_finger_tip_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
- <calibration reference_position="0" values="1.5 -1.0" />
- <limit effort="10" min="${-gripper_max_angle}" max="${-gripper_min_angle*0}"
- k_position="0" k_velocity="0" velocity="0.5"
+ <calibration reference_position="0" values="1.0 -1.0" />
+ <limit effort="${finger_joint_effort_limit}" min="${-gripper_max_angle}" max="${-gripper_min_angle*0}"
+ k_position="0" k_velocity="0" velocity="0.0"
safety_length_min="0.0" safety_length_max="0.0" />
<joint_properties damping="0.002" />
<mimic name="${prefix}_l_finger_joint" mult="-1" offset="0"/>
<map name="${prefix}_l_finger_tip_mimic" flag="gazebo">
- <elem key="mimicKp">2.0</elem>
- <elem key="mimicKd">1.0</elem>
- <elem key="mimicFMax">10.0</elem>
+ <elem key="mimicKp" value="20.0" />
+ <elem key="mimicKd" value="10.0" />
+ <elem key="mimicFMax" value="10.0" />
</map>
</joint>
@@ -209,8 +212,8 @@
</map>
<map name="${prefix}_l_finger_tip_gravity" flag="gazebo">
<elem key="turnGravityOff">true</elem>
- <elem key="mu1" value="50.0" />
- <elem key="mu2" value="50.0" />
+ <elem key="mu1" value="${finger_tip_mu}" />
+ <elem key="mu2" value="${finger_tip_mu}" />
<elem key="kp" value="100000000.0" />
<elem key="kd" value="1.0" />
</map>
@@ -221,15 +224,15 @@
<joint name="${prefix}_r_finger_tip_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
- <calibration reference_position="0" values="1.5 -1.0" />
- <limit effort="10" min="${gripper_min_angle*0}" max="${gripper_max_angle}"
- k_position="0" k_velocity="0" velocity="0.5"
+ <calibration reference_position="0" values="1.0 -1.0" />
+ <limit effort="${finger_joint_effort_limit}" min="${gripper_min_angle*0}" max="${gripper_max_angle}"
+ k_position="0" k_velocity="0" velocity="0.0"
safety_length_min="0.0" safety_length_max="0.0" />
<joint_properties damping="0.002" />
<mimic name="${prefix}_l_finger_joint" mult="1" offset="0"/>
<map name="${prefix}_r_finger_tip_mimic" flag="gazebo">
- <elem key="mimicKp" value="2.0" />
- <elem key="mimicKd" value="1.0" />
+ <elem key="mimicKp" value="20.0" />
+ <elem key="mimicKd" value="10.0" />
<elem key="mimicFMax" value="10.0" />
</map>
</joint>
@@ -281,8 +284,8 @@
</map>
<map name="${prefix}_r_finger_tip_gravity" flag="gazebo">
<elem key="turnGravityOff">true</elem>
- <elem key="mu1" value="50.0" />
- <elem key="mu2" value="50.0" />
+ <elem key="mu1" value="${finger_tip_mu}" />
+ <elem key="mu2" value="${finger_tip_mu}" />
<elem key="kp" value="100000000.0" />
<elem key="kd" value="1.0" />
</map>
@@ -299,6 +302,14 @@
<frameName>map</frameName>
<interface:position name="p3d_${prefix}_l_finger_position_iface" />
</controller:ros_p3d>
+ <controller:ros_f3d name="f3d_${prefix}_l_finger_controller" plugin="libros_f3d.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>${prefix}_l_finger_link</bodyName>
+ <topicName>${prefix}_l_finger_force_ground_truth</topicName>
+ <frameName>${prefix}_l_finger_link</frameName>
+ <interface:position name="f3d_${prefix}_l_finger_force_iface" />
+ </controller:ros_f3d>
</verbatim>
</map>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jam...@us...> - 2009-01-28 00:56:27
|
Revision: 10304
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10304&view=rev
Author: jamesbowman
Date: 2009-01-28 00:56:23 +0000 (Wed, 28 Jan 2009)
Log Message:
-----------
New FAST implementation that behaves more predictably wrt threshold. VO switched to new interface.
Modified Paths:
--------------
pkg/trunk/3rdparty/fast_detector/manifest.xml
pkg/trunk/3rdparty/fast_detector/src/fast.h
pkg/trunk/3rdparty/fast_detector/src/fast_9.c
pkg/trunk/3rdparty/fast_detector/src/nonmax.c
pkg/trunk/3rdparty/fast_detector/src/pyfast.c
pkg/trunk/vision/visual_odometry/src/visualodometer.py
Modified: pkg/trunk/3rdparty/fast_detector/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/fast_detector/manifest.xml 2009-01-28 00:25:14 UTC (rev 10303)
+++ pkg/trunk/3rdparty/fast_detector/manifest.xml 2009-01-28 00:56:23 UTC (rev 10304)
@@ -3,7 +3,7 @@
FAST
</description>
<author>Edward Rosten and Tom Drummond</author>
- <license>LGPL</license>
+ <license>BSD</license>
<review status="3rdparty" notes=""/>
</package>
Modified: pkg/trunk/3rdparty/fast_detector/src/fast.h
===================================================================
--- pkg/trunk/3rdparty/fast_detector/src/fast.h 2009-01-28 00:25:14 UTC (rev 10303)
+++ pkg/trunk/3rdparty/fast_detector/src/fast.h 2009-01-28 00:56:23 UTC (rev 10304)
@@ -6,10 +6,11 @@
#endif
typedef struct { int x, y; } xy;
+typedef struct { int x, y, r; } xyr;
typedef unsigned char byte;
-xy* fast_nonmax(const byte* im, int xsize, int ysize, xy* corners, int numcorners, int barrier, int* numnx);
-xy* fast_corner_detect_9(const byte* im, int xsize, int ysize, int barrier, int* numcorners);
+xyr* fast_nonmax(const byte* im, int xsize, int ysize, xyr* corners, int numcorners, int barrier, int* numnx);
+xyr* fast_corner_detect_9(const byte* im, int xsize, int ysize, int barrier, int* numcorners);
xy* fast_corner_detect_10(const byte* im, int xsize, int ysize, int barrier, int* numcorners);
xy* fast_corner_detect_11(const byte* im, int xsize, int ysize, int barrier, int* numcorners);
xy* fast_corner_detect_12(const byte* im, int xsize, int ysize, int barrier, int* numcorners);
Modified: pkg/trunk/3rdparty/fast_detector/src/fast_9.c
===================================================================
--- pkg/trunk/3rdparty/fast_detector/src/fast_9.c 2009-01-28 00:25:14 UTC (rev 10303)
+++ pkg/trunk/3rdparty/fast_detector/src/fast_9.c 2009-01-28 00:56:23 UTC (rev 10304)
@@ -4,12 +4,13 @@
*/
#include <stdlib.h>
#include "fast.h"
-xy* fast_corner_detect_9(const byte* im, int xsize, int ysize, int barrier, int* num)
+xyr* fast_corner_detect_9(const byte* im, int xsize, int ysize, int barrier, int* num)
{
int boundary = 3, y, cb, c_b;
+ int score;
const byte *line_max, *line_min;
int rsize=512, total=0;
- xy *ret = (xy*)malloc(rsize*sizeof(xy));
+ xyr *ret = (xyr*)malloc(rsize*sizeof(xyr));
const byte* cache_0;
const byte* cache_1;
const byte* cache_2;
@@ -2639,10 +2640,11 @@
if(total >= rsize)
{
rsize *=2;
- ret=(xy*)realloc(ret, rsize*sizeof(xy));
+ ret=(xyr*)realloc(ret, rsize*sizeof(xyr));
}
ret[total].x = cache_0-line_min;
- ret[total++].y = y;
+ ret[total].y = y;
+ total++;
}
}
*num = total;
Modified: pkg/trunk/3rdparty/fast_detector/src/nonmax.c
===================================================================
--- pkg/trunk/3rdparty/fast_detector/src/nonmax.c 2009-01-28 00:25:14 UTC (rev 10303)
+++ pkg/trunk/3rdparty/fast_detector/src/nonmax.c 2009-01-28 00:56:23 UTC (rev 10304)
@@ -179,7 +179,7 @@
}
/*void fast_nonmax(const BasicImage<byte>& im, const vector<ImageRef>& corners, int barrier, vector<ReturnType>& nonmax_corners)*/
-xy* fast_nonmax(const byte* im, int xsize, int ysize, xy* corners, int numcorners, int barrier, int* numnx)
+xyr* fast_nonmax(const byte* im, int xsize, int ysize, xyr* corners, int numcorners, int barrier, int* numnx)
{
/*Create a list of integer pointer offstes, corresponding to the */
@@ -187,14 +187,13 @@
int pointer_dir[16];
int* row_start = (int*) malloc(ysize * sizeof(int));
int* scores = (int*) malloc(numcorners * sizeof(int));
- xy* nonmax_corners=(xy*)malloc(numcorners* sizeof(xy));
+ xyr* nonmax_corners=(xyr*)malloc(numcorners* sizeof(xyr));
int num_nonmax=0;
int prev_row = -1;
int i, j;
int point_above = 0;
int point_below = 0;
-
pointer_dir[0] = 0 + 3 * xsize;
pointer_dir[1] = 1 + 3 * xsize;
pointer_dir[2] = 2 + 2 * xsize;
@@ -249,7 +248,7 @@
for(i=0; i < numcorners; i++)
{
int score = scores[i];
- xy pos = corners[i];
+ xyr pos = corners[i];
//Check left
if(i > 0)
@@ -305,6 +304,7 @@
nonmax_corners[num_nonmax].x = corners[i].x;
nonmax_corners[num_nonmax].y = corners[i].y;
+ nonmax_corners[num_nonmax].r = score;
num_nonmax++;
Modified: pkg/trunk/3rdparty/fast_detector/src/pyfast.c
===================================================================
--- pkg/trunk/3rdparty/fast_detector/src/pyfast.c 2009-01-28 00:25:14 UTC (rev 10303)
+++ pkg/trunk/3rdparty/fast_detector/src/pyfast.c 2009-01-28 00:56:23 UTC (rev 10304)
@@ -1,27 +1,113 @@
#include "Python.h"
#include "fast.h"
+static inline int corner_score(const byte* imp, const int *pointer_dir, int barrier)
+{
+ /*The score for a positive feature is sum of the difference between the pixels
+ and the barrier if the difference is positive. Negative is similar.
+ The score is the max of those two.
+
+ B = {x | x = points on the Bresenham circle around c}
+ Sp = { I(x) - t | x E B , I(x) - t > 0 }
+ Sn = { t - I(x) | x E B, t - I(x) > 0}
+
+ Score = max sum(Sp), sum(Sn)*/
+
+ int cb = *imp + barrier;
+ int c_b = *imp - barrier;
+ int sp=0, sn = 0;
+
+ int i=0;
+
+#if 0
+ for(i=0; i<16; i++)
+ {
+ int p = imp[pointer_dir[i]];
+
+ if(p > cb)
+ sp += p-cb;
+ else if(p < c_b)
+ sn += c_b-p;
+ }
+#else
+{
+ int p;
+#define ACC(i) p = imp[pointer_dir[i]]; if (p > cb) sp += p-cb; else sn += c_b-p;
+ ACC(0)
+ ACC(1)
+ ACC(2)
+ ACC(3)
+ ACC(4)
+ ACC(5)
+ ACC(6)
+ ACC(7)
+ ACC(8)
+ ACC(9)
+ ACC(10)
+ ACC(11)
+ ACC(12)
+ ACC(13)
+ ACC(14)
+ ACC(15)
+#undef ACC
+}
+#endif
+
+ if(sp > sn)
+ return sp;
+ else
+ return sn;
+}
+
PyObject *fast(PyObject *self, PyObject *args)
{
int imdata_size;
char *imdata;
- int xsize, ysize, threshold, barrier;
- if (!PyArg_ParseTuple(args, "s#iiii", &imdata, &imdata_size, &xsize, &ysize, &threshold, &barrier))
+ int xsize, ysize, barrier, threshold;
+ if (!PyArg_ParseTuple(args, "s#iiii", &imdata, &imdata_size, &xsize, &ysize, &barrier, &threshold))
return NULL;
int numcorners = 0, num_nonmax = 0;
- xy *corners = fast_corner_detect_9(imdata, xsize, ysize, threshold, &numcorners);
+ xyr *corners = fast_corner_detect_9(imdata, xsize, ysize, barrier, &numcorners);
+#if 0
+ xyr *nm = fast_nonmax(imdata, xsize, ysize, corners, numcorners, 3, &num_nonmax);
+#else
+ num_nonmax = numcorners;
+ xyr *nm = corners;
+ corners = NULL;
+#endif
- xy *nm = fast_nonmax(imdata, xsize, ysize, corners, numcorners, barrier, &num_nonmax);
- PyObject *r = PyList_New(num_nonmax);
+ int pixel[16];
+ pixel[0] = 0 + 3 * xsize;
+ pixel[1] = 1 + 3 * xsize;
+ pixel[2] = 2 + 2 * xsize;
+ pixel[3] = 3 + 1 * xsize;
+ pixel[4] = 3 + 0 * xsize;
+ pixel[5] = 3 + -1 * xsize;
+ pixel[6] = 2 + -2 * xsize;
+ pixel[7] = 1 + -3 * xsize;
+ pixel[8] = 0 + -3 * xsize;
+ pixel[9] = -1 + -3 * xsize;
+ pixel[10] = -2 + -2 * xsize;
+ pixel[11] = -3 + -1 * xsize;
+ pixel[12] = -3 + 0 * xsize;
+ pixel[13] = -3 + 1 * xsize;
+ pixel[14] = -2 + 2 * xsize;
+ pixel[15] = -1 + 3 * xsize;
+
+ PyObject *r = PyList_New(0);
int i;
-
for (i = 0; i < num_nonmax; i++) {
- PyObject *t = PyTuple_New(2);
- PyTuple_SetItem(t, 0, PyInt_FromLong(nm[i].x));
- PyTuple_SetItem(t, 1, PyInt_FromLong(nm[i].y));
- PyList_SetItem(r, i, t);
+ int x = nm[i].x, y = nm[i].y;
+ int s = corner_score(imdata + x + y * xsize, pixel, barrier);
+ if (s > threshold) {
+ PyObject *t = PyTuple_New(3);
+ PyTuple_SetItem(t, 0, PyInt_FromLong(x));
+ PyTuple_SetItem(t, 1, PyInt_FromLong(y));
+ PyTuple_SetItem(t, 2, PyInt_FromLong(s));
+ PyList_Append(r, t);
+ }
}
if (corners)
free(corners);
@@ -31,8 +117,66 @@
return r;
}
+PyObject *nonmax(PyObject *self, PyObject *args)
+{
+ PyObject *ppts;
+ Py_ssize_t i, j;
+ int xi, yi, ri;
+ int xj, yj, rj;
+ int dist2;
+
+ if (!PyArg_ParseTuple(args, "O", &ppts))
+ return NULL;
+ Py_ssize_t sz = PyList_Size(ppts);
+
+ char suppress[sz];
+ for (i = 0; i < sz; i++)
+ suppress[i] = 0;
+ xyr pts[sz];
+
+ for (i = 0; i < sz; i++) {
+ PyObject *t = PyList_GET_ITEM(ppts, i);
+ pts[i].x = PyInt_AsLong(PyTuple_GET_ITEM(t, 0));
+ pts[i].y = PyInt_AsLong(PyTuple_GET_ITEM(t, 1));
+ pts[i].r = PyInt_AsLong(PyTuple_GET_ITEM(t, 2));
+ }
+
+ for (i = 0; i < sz; i++) {
+ xi = pts[i].x;
+ yi = pts[i].y;
+ ri = pts[i].r;
+ for (j = i + 1; j < sz; j++) {
+ yj = pts[j].y;
+ if ((yj - yi) > 1) {
+ continue;
+ }
+ rj = pts[j].r;
+ xj = pts[j].x;
+ dist2 = (xi-xj)*(xi-xj) + (yi-yj)*(yi-yj);
+ if (dist2 < 2) {
+ if (ri < rj)
+ suppress[i] = 1;
+ else
+ suppress[j] = 1;
+ }
+ }
+ }
+
+ PyObject *r = PyList_New(0);
+ for (i = 0; i < sz; i++) {
+ if (!suppress[i]) {
+ PyObject *t = PyList_GET_ITEM(ppts, i);
+ Py_INCREF(t);
+ PyList_Append(r, t);
+ }
+ }
+
+ return r;
+}
+
static PyMethodDef methods[] = {
{"fast", fast, METH_VARARGS},
+ {"nonmax", nonmax, METH_VARARGS},
{NULL, NULL},
};
Modified: pkg/trunk/vision/visual_odometry/src/visualodometer.py
===================================================================
--- pkg/trunk/vision/visual_odometry/src/visualodometer.py 2009-01-28 00:25:14 UTC (rev 10303)
+++ pkg/trunk/vision/visual_odometry/src/visualodometer.py 2009-01-28 00:56:23 UTC (rev 10304)
@@ -185,6 +185,7 @@
hi = self.thresh
if len(features) > target_points:
lo = self.thresh
+ self.thresh = 0.5 * (lo + hi)
# Try to be a bit adaptive for next time
if len(features) > (target_points * 1.1):
@@ -193,20 +194,48 @@
self.thresh *= 0.95
return features
-class FeatureDetectorFast(FeatureDetector):
+# Feature detectors that return features in order (i.e. strongest first)
+# can be simpler. Just always keep the threshold high enough to give
+# too many responses, and take the top N.
-# default_thresh = 10
-# threshrange = (5,127)
+class FeatureDetectorOrdered:
+
+ def name(self):
+ return self.__class__.__name__
+
+ def __init__(self):
+ self.thresh = self.default_thresh
+ self.cold = True
+
+ def detect(self, frame, target_points):
+
+ features = self.get_features(frame, target_points)
+ # Too few features, so lower threshold
+ while (len(features) < target_points) and (self.thresh > self.threshrange[0]):
+ self.thresh = float(max(self.threshrange[0], self.thresh / 2))
+ features = self.get_features(frame, target_points)
+
+ # Try to be a bit adaptive for next time
+ if len(features) > (target_points * 2):
+ self.thresh *= 2
+ if len(features) < (target_points * 1.25):
+ self.thresh *= 0.95
+ return features[:target_points]
+
+def FAST(imdata, xsize, ysize, thresh):
+ kp = fast.fast(imdata, xsize, ysize, 9, thresh)
+ return sorted(fast.nonmax(kp), key = lambda x:(x[2],x[0],x[1]), reverse = True)
+
+class FeatureDetectorFast(FeatureDetectorOrdered):
+
default_thresh = 10
- threshrange = (5,127)
+ threshrange = (0.5,300)
def get_features(self, frame, target_points):
assert len(frame.rawdata) == (frame.size[0] * frame.size[1])
- feat = fast.fast(frame.rawdata, frame.size[0], frame.size[1], int(self.thresh), 40)
+ feat = FAST(frame.rawdata, frame.size[0], frame.size[1], self.thresh)
+ return [ (x,y) for (x,y,r) in feat if (16 <= x and x <= (640-16) and (16 <= y) and y < (480-16)) ]
- return [ (x,y) for (x,y) in feat if (16 <= x and x <= (640-16) and (16 <= y) and y < (480-16)) ]
- return feat
-
class FeatureDetector4x4:
def __init__(self, fd):
@@ -419,6 +448,16 @@
self.timer['disparity'].start()
disparities = [frame.lookup_disparity(x,y) for (x,y) in frame.kp2d]
frame.kp = [ (x,y,z) for ((x,y),z) in zip(frame.kp2d, disparities) if z]
+ #print "disparities", len(frame.kp2d), len(frame.kp)
+
+ if 0:
+ import pylab
+ pylab.imshow(numpy.fromstring(frame.lf.tostring(), numpy.uint8).reshape(480,640), cmap=pylab.cm.gray)
+ pylab.scatter([x for (x,y) in frame.kp2d], [y for (x,y) in frame.kp2d], label = '2d', c = 'red')
+ pylab.scatter([x for (x,y,d) in frame.kp], [y for (x,y,d) in frame.kp], label = 'disparities', c = 'green')
+ pylab.legend()
+ pylab.show()
+
self.timer['disparity'].stop()
lgrad = " " * (640 * 480)
@@ -455,12 +494,26 @@
#ps = Pose(numpy.mat([[1,0,0],[0,1,0],[0,0,1]]), numpy.array(shift))
#return pr * ps
+ def show_pairs(self, pairs, f0, f1):
+ print "*** SHOWING PAIRS FOR FRAMES ", f0.id, f1.id, "***"
+ print f0.id, "has", len(f0.kp), "keypoints"
+ print f1.id, "has", len(f1.kp), "keypoints"
+ print "There are", len(pairs), "pairs"
+ import pylab
+ for (a,b) in pairs:
+ pylab.plot([ f0.kp[a][0], f1.kp[b][0] ], [ f0.kp[a][1], f1.kp[b][1] ])
+ pylab.imshow(numpy.fromstring(f0.lf.tostring(), numpy.uint8).reshape(480,640), cmap=pylab.cm.gray)
+ pylab.scatter([x for (x,y,d) in f0.kp], [y for (x,y,d) in f0.kp], label = '%d kp' % f0.id, c = 'red')
+ pylab.scatter([x for (x,y,d) in f1.kp], [y for (x,y,d) in f1.kp], label = '%d kp' % f1.id, c = 'green')
+ pylab.legend()
+ pylab.show()
+
def proximity(self, f0, f1, scavenger = False):
"""Given frames f0, f1, returns (inliers, pose) where pose is the transform that maps f1's frame to f0's frame.)"""
self.num_frames += 1
pairs = self.temporal_match(f0, f1)
- #print "frames", (f0.id, f1.id), "got", len(pairs), "pairs",
+ #self.show_pairs(pairs, f0, f1)
if len(pairs) > 10:
solution = self.solve(f0.kp, f1.kp, pairs, True)
if scavenger and solution and solution[0] > 10:
@@ -646,19 +699,13 @@
self.ext_frames += 1
frame.externals.append((fext, VO.frame_pose(fext.id, fext.pose.tolist())))
- def handle_frame(self, frame):
- self.find_keypoints(frame)
- self.find_disparities(frame)
- self.collect_descriptors(frame)
- frame.id = self.num_frames
- return self.handle_frame_0(frame)
-
# just set up the frame with descriptors, no VO processing
def setup_frame(self, frame):
self.find_keypoints(frame)
self.find_disparities(frame)
self.collect_descriptors(frame)
frame.id = self.num_frames
+ frame.ref_frame_id = None
# return inliers from a match
def check_inliers(self, frame1, frame2):
@@ -677,6 +724,13 @@
self.maintain_tracks(oldkey, newkey)
self.sba_handle_frame(newkey)
+ def handle_frame(self, frame):
+ self.find_keypoints(frame)
+ self.find_disparities(frame)
+ self.collect_descriptors(frame)
+ frame.id = self.num_frames
+ return self.handle_frame_0(frame)
+
def handle_frame_0(self, frame):
if self.prev_frame:
# If the key->current is good, use it
@@ -684,6 +738,7 @@
ref = self.keyframe
self.pairs = self.temporal_match(ref, frame)
+ #if frame.id == 202 and ref.id == 199: self.show_pairs(self.pairs, ref, frame)
solution = self.solve(ref.kp, frame.kp, self.pairs)
if solution and solution[0] > 5:
(inl, rot, shift) = solution
@@ -749,3 +804,18 @@
Top = Tok * Tkp
print "** CORRECTED by", f.pose.d(Top), "**", "key", f.ref_frame_id, "of frame", f.id
f.pose = Top
+
+ def report_frame(self, frame):
+ import md5
+ print "*** FRAME", "id", frame.id, "key", frame.ref_frame_id, "***"
+ print "use_grad_img", frame.use_grad_img
+ print "limage md5:", " ".join([ "%02x" % ord(x) for x in md5.new(frame.lf.tostring()).digest()])
+ print "rimage md5:", " ".join([ "%02x" % ord(x) for x in md5.new(frame.rf.tostring()).digest()])
+ print "lgrad md5:", " ".join([ "%02x" % ord(x) for x in md5.new(frame.lgrad).digest()])
+ print "rimage md5:", " ".join([ "%02x" % ord(x) for x in md5.new(frame.rgrad).digest()])
+ print "kp2d length", len(frame.kp2d)
+ print " ", frame.kp2d[:7]
+ print "kp length", len(frame.kp)
+ for k in frame.kp:
+ print " ", k
+ print
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-28 03:48:04
|
Revision: 10349
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10349&view=rev
Author: sfkwc
Date: 2009-01-28 03:48:02 +0000 (Wed, 28 Jan 2009)
Log Message:
-----------
load_manifest updates
Modified Paths:
--------------
pkg/trunk/simulators/simulator_integration_tests/world_3d_map_gazebo_tests/testcloud.py
pkg/trunk/world_models/static_map_server/test/consumer.py
Modified: pkg/trunk/simulators/simulator_integration_tests/world_3d_map_gazebo_tests/testcloud.py
===================================================================
--- pkg/trunk/simulators/simulator_integration_tests/world_3d_map_gazebo_tests/testcloud.py 2009-01-28 03:47:32 UTC (rev 10348)
+++ pkg/trunk/simulators/simulator_integration_tests/world_3d_map_gazebo_tests/testcloud.py 2009-01-28 03:48:02 UTC (rev 10349)
@@ -38,7 +38,7 @@
NAME = 'testcloud'
import rostools
-rostools.update_path(PKG)
+rostools.load_manifest(PKG)
import sys, unittest
import os, os.path, threading, time
Modified: pkg/trunk/world_models/static_map_server/test/consumer.py
===================================================================
--- pkg/trunk/world_models/static_map_server/test/consumer.py 2009-01-28 03:47:32 UTC (rev 10348)
+++ pkg/trunk/world_models/static_map_server/test/consumer.py 2009-01-28 03:48:02 UTC (rev 10349)
@@ -36,7 +36,7 @@
NAME = 'consumer'
import rostools
-rostools.update_path(PKG)
+rostools.load_manifest(PKG)
import sys, unittest, time
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-28 08:14:05
|
Revision: 10359
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10359&view=rev
Author: sfkwc
Date: 2009-01-28 08:14:01 +0000 (Wed, 28 Jan 2009)
Log Message:
-----------
load_manifest updates
Modified Paths:
--------------
pkg/trunk/drivers/network/wifi_ddwrt/nodes/ddwrt
pkg/trunk/drivers/robot/pr2/ocean_battery_driver/scripts/monitor
pkg/trunk/drivers/robot/pr2/ocean_battery_driver/scripts/view_batteries
pkg/trunk/drivers/robot/pr2/pr2_power_board/scripts/send_command
pkg/trunk/drivers/robot/pr2/pr2_power_board/scripts/view_power
pkg/trunk/hardware_test/qualification/bin/gui
pkg/trunk/hardware_test/qualification/bin/gui_test
pkg/trunk/hardware_test/runtime_monitor/scripts/log_runtime_messages
pkg/trunk/hardware_test/runtime_monitor/scripts/monitor
pkg/trunk/hardware_test/runtime_monitor/scripts/runtime_test
pkg/trunk/hardware_test/runtime_monitor/scripts/test_high_level
pkg/trunk/vision/vision_utils/src/bag2tiff
pkg/trunk/visualization/pr2_gui/scripts/pr2_gui
pkg/trunk/world_models/static_map_server/nodes/map_server
Modified: pkg/trunk/drivers/network/wifi_ddwrt/nodes/ddwrt
===================================================================
--- pkg/trunk/drivers/network/wifi_ddwrt/nodes/ddwrt 2009-01-28 08:10:19 UTC (rev 10358)
+++ pkg/trunk/drivers/network/wifi_ddwrt/nodes/ddwrt 2009-01-28 08:14:01 UTC (rev 10359)
@@ -37,7 +37,7 @@
## subscribing to the same topic.
PKG = 'wifi_ddwrt'
-import rostools; rostools.update_path(PKG)
+import rostools; rostools.load_manifest(PKG)
import sys, time, StringIO, string
Modified: pkg/trunk/drivers/robot/pr2/ocean_battery_driver/scripts/monitor
===================================================================
--- pkg/trunk/drivers/robot/pr2/ocean_battery_driver/scripts/monitor 2009-01-28 08:10:19 UTC (rev 10358)
+++ pkg/trunk/drivers/robot/pr2/ocean_battery_driver/scripts/monitor 2009-01-28 08:14:01 UTC (rev 10359)
@@ -37,7 +37,7 @@
To run, invoke rosrun ocean_battery_driver monitor
"""
-import rostools; rostools.update_path('ocean_battery_driver')
+import rostools; rostools.load_manifest('ocean_battery_driver')
import os
import sys
Modified: pkg/trunk/drivers/robot/pr2/ocean_battery_driver/scripts/view_batteries
===================================================================
--- pkg/trunk/drivers/robot/pr2/ocean_battery_driver/scripts/view_batteries 2009-01-28 08:10:19 UTC (rev 10358)
+++ pkg/trunk/drivers/robot/pr2/ocean_battery_driver/scripts/view_batteries 2009-01-28 08:14:01 UTC (rev 10359)
@@ -35,7 +35,7 @@
## A basic node to listen to and display incoming diagnostic messages using wx
import rostools
-rostools.update_path('ocean_battery_driver')
+rostools.load_manifest('ocean_battery_driver')
import sys
import rospy
Modified: pkg/trunk/drivers/robot/pr2/pr2_power_board/scripts/send_command
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_power_board/scripts/send_command 2009-01-28 08:10:19 UTC (rev 10358)
+++ pkg/trunk/drivers/robot/pr2/pr2_power_board/scripts/send_command 2009-01-28 08:14:01 UTC (rev 10359)
@@ -37,7 +37,7 @@
PKG = 'pr2_power_board' # this package name
-import rostools; rostools.update_path(PKG)
+import rostools; rostools.load_manifest(PKG)
import sys
import os
Modified: pkg/trunk/drivers/robot/pr2/pr2_power_board/scripts/view_power
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_power_board/scripts/view_power 2009-01-28 08:10:19 UTC (rev 10358)
+++ pkg/trunk/drivers/robot/pr2/pr2_power_board/scripts/view_power 2009-01-28 08:14:01 UTC (rev 10359)
@@ -35,7 +35,7 @@
## A basic node to listen to and display incoming diagnostic messages using wx
import rostools
-rostools.update_path('pr2_power_board')
+rostools.load_manifest('pr2_power_board')
import sys
import rospy
Modified: pkg/trunk/hardware_test/qualification/bin/gui
===================================================================
--- pkg/trunk/hardware_test/qualification/bin/gui 2009-01-28 08:10:19 UTC (rev 10358)
+++ pkg/trunk/hardware_test/qualification/bin/gui 2009-01-28 08:14:01 UTC (rev 10359)
@@ -33,7 +33,7 @@
# POSSIBILITY OF SUCH DAMAGE.
import rostools
-rostools.update_path('qualification')
+rostools.load_manifest('qualification')
import wx
Modified: pkg/trunk/hardware_test/qualification/bin/gui_test
===================================================================
--- pkg/trunk/hardware_test/qualification/bin/gui_test 2009-01-28 08:10:19 UTC (rev 10358)
+++ pkg/trunk/hardware_test/qualification/bin/gui_test 2009-01-28 08:14:01 UTC (rev 10359)
@@ -33,7 +33,7 @@
# POSSIBILITY OF SUCH DAMAGE.
import rostools
-rostools.update_path('qualification')
+rostools.load_manifest('qualification')
import wx
Modified: pkg/trunk/hardware_test/runtime_monitor/scripts/log_runtime_messages
===================================================================
--- pkg/trunk/hardware_test/runtime_monitor/scripts/log_runtime_messages 2009-01-28 08:10:19 UTC (rev 10358)
+++ pkg/trunk/hardware_test/runtime_monitor/scripts/log_runtime_messages 2009-01-28 08:14:01 UTC (rev 10359)
@@ -36,7 +36,7 @@
import getopt
import rostools
-rostools.update_path('runtime_monitor')
+rostools.load_manifest('runtime_monitor')
import sys
import rospy
Modified: pkg/trunk/hardware_test/runtime_monitor/scripts/monitor
===================================================================
--- pkg/trunk/hardware_test/runtime_monitor/scripts/monitor 2009-01-28 08:10:19 UTC (rev 10358)
+++ pkg/trunk/hardware_test/runtime_monitor/scripts/monitor 2009-01-28 08:14:01 UTC (rev 10359)
@@ -35,7 +35,7 @@
## A basic node to listen to and display incoming diagnostic messages
import rostools
-rostools.update_path('runtime_monitor')
+rostools.load_manifest('runtime_monitor')
import sys
import rospy
Modified: pkg/trunk/hardware_test/runtime_monitor/scripts/runtime_test
===================================================================
--- pkg/trunk/hardware_test/runtime_monitor/scripts/runtime_test 2009-01-28 08:10:19 UTC (rev 10358)
+++ pkg/trunk/hardware_test/runtime_monitor/scripts/runtime_test 2009-01-28 08:14:01 UTC (rev 10359)
@@ -35,7 +35,7 @@
## A basic node to monitor diagnostics for expected status
import rostools
-rostools.update_path('runtime_monitor')
+rostools.load_manifest('runtime_monitor')
import sys
import rospy
@@ -63,7 +63,7 @@
def analyze(package, test_name):
# import the test script
- exec("rostools.update_path('%s')"%package)
+ exec("rostools.load_manifest('%s')"%package)
exec('from %s import %s'%(package, test_name))
# get it's parameters
Modified: pkg/trunk/hardware_test/runtime_monitor/scripts/test_high_level
===================================================================
--- pkg/trunk/hardware_test/runtime_monitor/scripts/test_high_level 2009-01-28 08:10:19 UTC (rev 10358)
+++ pkg/trunk/hardware_test/runtime_monitor/scripts/test_high_level 2009-01-28 08:14:01 UTC (rev 10359)
@@ -35,7 +35,7 @@
## A basic node to listen to and display incoming diagnostic messages
import rostools
-rostools.update_path('runtime_monitor')
+rostools.load_manifest('runtime_monitor')
import sys
import rospy
Modified: pkg/trunk/vision/vision_utils/src/bag2tiff
===================================================================
--- pkg/trunk/vision/vision_utils/src/bag2tiff 2009-01-28 08:10:19 UTC (rev 10358)
+++ pkg/trunk/vision/vision_utils/src/bag2tiff 2009-01-28 08:14:01 UTC (rev 10359)
@@ -1,6 +1,6 @@
#!/usr/bin/python
PKG='vision_utils'
-import rostools; rostools.update_path(PKG)
+import rostools; rostools.load_manifest(PKG)
import sys
Modified: pkg/trunk/visualization/pr2_gui/scripts/pr2_gui
===================================================================
--- pkg/trunk/visualization/pr2_gui/scripts/pr2_gui 2009-01-28 08:10:19 UTC (rev 10358)
+++ pkg/trunk/visualization/pr2_gui/scripts/pr2_gui 2009-01-28 08:14:01 UTC (rev 10359)
@@ -36,7 +36,7 @@
GUI for the PR2."""
import rostools
-rostools.update_path('pr2_gui')
+rostools.load_manifest('pr2_gui')
WXVER = '2.8'
import wxversion
@@ -57,4 +57,4 @@
frame.Show()
app.MainLoop()
-ogre_tools.cleanupOgre()
\ No newline at end of file
+ogre_tools.cleanupOgre()
Modified: pkg/trunk/world_models/static_map_server/nodes/map_server
===================================================================
--- pkg/trunk/world_models/static_map_server/nodes/map_server 2009-01-28 08:10:19 UTC (rev 10358)
+++ pkg/trunk/world_models/static_map_server/nodes/map_server 2009-01-28 08:14:01 UTC (rev 10359)
@@ -33,7 +33,7 @@
#
import rostools
-rostools.update_path('static_map_server')
+rostools.load_manifest('static_map_server')
import sys, time
import rospy
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-28 08:28:24
|
Revision: 10360
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10360&view=rev
Author: sfkwc
Date: 2009-01-28 08:28:15 +0000 (Wed, 28 Jan 2009)
Log Message:
-----------
replace rospy.ready calls with rospy.init_node
Modified Paths:
--------------
pkg/trunk/demos/2dnav_pr2/pose_listen.py
pkg/trunk/drivers/robot/pr2/ocean_battery_driver/scripts/view_batteries
pkg/trunk/drivers/robot/pr2/pr2_power_board/scripts/view_power
pkg/trunk/hardware_test/runtime_monitor/scripts/log_runtime_messages
pkg/trunk/hardware_test/runtime_monitor/scripts/monitor
pkg/trunk/hardware_test/runtime_monitor/scripts/runtime_test
pkg/trunk/hardware_test/runtime_monitor/scripts/test_high_level
pkg/trunk/hardware_test/runtime_monitor/scripts/wxmonitor.py
pkg/trunk/hardware_test/runtime_monitor/testmonitor.py
pkg/trunk/mechanism/mechanism_control/scripts/detect.py
pkg/trunk/mechanism/mechanism_control/scripts/listen.py
pkg/trunk/mechanism/mechanism_control/scripts/writer.py
pkg/trunk/mechanism/mechanism_control/test/listenerpublisher_ms.py
pkg/trunk/mechanism/mechanism_control/test/test_ms.py
pkg/trunk/mechanism/mechanism_control/test/test_ms_cpp.py
pkg/trunk/nav/trajectory_rollout/scripts/listen.py
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_prototype1/motor_monitor.py
pkg/trunk/simulators/simulator_integration_tests/world_3d_map_gazebo_tests/testcloud.py
pkg/trunk/util/ntp_monitor/ntp_monitor.py
pkg/trunk/vision/visual_odometry/nodes/camview_py.py
pkg/trunk/vision/visual_odometry/nodes/corrector.py
pkg/trunk/vision/visual_odometry/nodes/lineperf.py
pkg/trunk/vision/visual_odometry/nodes/vo.py
pkg/trunk/visualization/pr2_gui/src/pr2_gui/pr2_frame.py
pkg/trunk/visualization/wxpy_ros/tests/test_try.py
pkg/trunk/world_models/static_map_server/nodes/map_server
Modified: pkg/trunk/demos/2dnav_pr2/pose_listen.py
===================================================================
--- pkg/trunk/demos/2dnav_pr2/pose_listen.py 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/demos/2dnav_pr2/pose_listen.py 2009-01-28 08:28:15 UTC (rev 10360)
@@ -16,7 +16,7 @@
def pose_listen():
rospy.TopicSub("/odom", RobotBase2DOdom, odom_callback)
rospy.TopicSub("/localizedpose", RobotBase2DOdom, localized_callback)
- rospy.ready(NAME, anonymous=True)
+ rospy.init_node(NAME, anonymous=True)
rospy.spin()
if __name__ == '__main__':
Modified: pkg/trunk/drivers/robot/pr2/ocean_battery_driver/scripts/view_batteries
===================================================================
--- pkg/trunk/drivers/robot/pr2/ocean_battery_driver/scripts/view_batteries 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/drivers/robot/pr2/ocean_battery_driver/scripts/view_batteries 2009-01-28 08:28:15 UTC (rev 10360)
@@ -72,7 +72,7 @@
def listener():
app = wx.PySimpleApp()
- rospy.ready(NAME, anonymous=True)
+ rospy.init_node(NAME, anonymous=True)
frame = MainWindow(None, -1, "PR2 Power Board")
frame.Show()
Modified: pkg/trunk/drivers/robot/pr2/pr2_power_board/scripts/view_power
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_power_board/scripts/view_power 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/drivers/robot/pr2/pr2_power_board/scripts/view_power 2009-01-28 08:28:15 UTC (rev 10360)
@@ -73,7 +73,7 @@
def listener():
app = wx.PySimpleApp()
- rospy.ready(NAME, anonymous=True)
+ rospy.init_node(NAME, anonymous=True)
frame = MainWindow(None, -1, "PR2 Power Board")
frame.Show()
Modified: pkg/trunk/hardware_test/runtime_monitor/scripts/log_runtime_messages
===================================================================
--- pkg/trunk/hardware_test/runtime_monitor/scripts/log_runtime_messages 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/hardware_test/runtime_monitor/scripts/log_runtime_messages 2009-01-28 08:28:15 UTC (rev 10360)
@@ -72,7 +72,7 @@
def listener():
rospy.TopicSub("/diagnostics", DiagnosticMessage, callback)
- rospy.ready(NAME, anonymous=True)
+ rospy.init_node(NAME, anonymous=True)
rospy.spin()
if __name__ == '__main__':
Modified: pkg/trunk/hardware_test/runtime_monitor/scripts/monitor
===================================================================
--- pkg/trunk/hardware_test/runtime_monitor/scripts/monitor 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/hardware_test/runtime_monitor/scripts/monitor 2009-01-28 08:28:15 UTC (rev 10360)
@@ -55,7 +55,7 @@
def listener():
rospy.TopicSub("/diagnostics", DiagnosticMessage, callback)
- rospy.ready(NAME, anonymous=True)
+ rospy.init_node(NAME, anonymous=True)
rospy.spin()
if __name__ == '__main__':
Modified: pkg/trunk/hardware_test/runtime_monitor/scripts/runtime_test
===================================================================
--- pkg/trunk/hardware_test/runtime_monitor/scripts/runtime_test 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/hardware_test/runtime_monitor/scripts/runtime_test 2009-01-28 08:28:15 UTC (rev 10360)
@@ -104,7 +104,7 @@
def listener():
rospy.TopicSub("/diagnostics", DiagnosticMessage, callback)
- rospy.ready(NAME, anonymous=True)
+ rospy.init_node(NAME, anonymous=True)
rospy.spin()
if __name__ == '__main__':
Modified: pkg/trunk/hardware_test/runtime_monitor/scripts/test_high_level
===================================================================
--- pkg/trunk/hardware_test/runtime_monitor/scripts/test_high_level 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/hardware_test/runtime_monitor/scripts/test_high_level 2009-01-28 08:28:15 UTC (rev 10360)
@@ -79,7 +79,7 @@
def listener():
rospy.TopicSub("/diagnostics", DiagnosticMessage, callback)
- rospy.ready(NAME, anonymous=True)
+ rospy.init_node(NAME, anonymous=True)
rospy.spin()
if __name__ == '__main__':
Modified: pkg/trunk/hardware_test/runtime_monitor/scripts/wxmonitor.py
===================================================================
--- pkg/trunk/hardware_test/runtime_monitor/scripts/wxmonitor.py 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/hardware_test/runtime_monitor/scripts/wxmonitor.py 2009-01-28 08:28:15 UTC (rev 10360)
@@ -73,7 +73,7 @@
def wxmonitor():
app = wx.PySimpleApp()
- rospy.ready(NAME, anonymous=True)
+ rospy.init_node(NAME, anonymous=True)
frame = MainWindow(None, -1, "Runtime Monitor")
frame.Show()
Modified: pkg/trunk/hardware_test/runtime_monitor/testmonitor.py
===================================================================
--- pkg/trunk/hardware_test/runtime_monitor/testmonitor.py 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/hardware_test/runtime_monitor/testmonitor.py 2009-01-28 08:28:15 UTC (rev 10360)
@@ -82,7 +82,7 @@
def listener():
pub = rospy.TopicPub("/diagnostics", DiagnosticMessage)
- rospy.ready(NAME, anonymous=True)
+ rospy.init_node(NAME, anonymous=True)
while not rospy.is_shutdown():
loop(pub)
time.sleep(1)
Modified: pkg/trunk/mechanism/mechanism_control/scripts/detect.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/scripts/detect.py 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/mechanism/mechanism_control/scripts/detect.py 2009-01-28 08:28:15 UTC (rev 10360)
@@ -39,7 +39,7 @@
def listener_with_user_data():
rospy.TopicSub("/mechanism_state", MechanismState, callback)
- rospy.ready(NAME, anonymous=True)
+ rospy.init_node(NAME, anonymous=True)
rospy.spin()
if __name__ == '__main__':
Modified: pkg/trunk/mechanism/mechanism_control/scripts/listen.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/scripts/listen.py 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/mechanism/mechanism_control/scripts/listen.py 2009-01-28 08:28:15 UTC (rev 10360)
@@ -52,7 +52,7 @@
def listener_with_user_data():
rospy.TopicSub("/mechanism_state", MechanismState, callback)
- rospy.ready(NAME, anonymous=True)
+ rospy.init_node(NAME, anonymous=True)
rospy.spin()
if __name__ == '__main__':
Modified: pkg/trunk/mechanism/mechanism_control/scripts/writer.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/scripts/writer.py 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/mechanism/mechanism_control/scripts/writer.py 2009-01-28 08:28:15 UTC (rev 10360)
@@ -24,7 +24,7 @@
def listener_with_user_data():
rospy.TopicSub("/mechanism_state", MechanismState, callback)
- rospy.ready(NAME, anonymous=True)
+ rospy.init_node(NAME, anonymous=True)
rospy.spin()
if __name__ == '__main__':
Modified: pkg/trunk/mechanism/mechanism_control/test/listenerpublisher_ms.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/test/listenerpublisher_ms.py 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/mechanism/mechanism_control/test/listenerpublisher_ms.py 2009-01-28 08:28:15 UTC (rev 10360)
@@ -63,7 +63,7 @@
_pub.publish(data)
def listener_publisher():
- rospy.ready(NAME)
+ rospy.init_node(NAME)
rospy.TopicSub(IN, MSG, callback)
rospy.spin()
Modified: pkg/trunk/mechanism/mechanism_control/test/test_ms.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/test/test_ms.py 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/mechanism/mechanism_control/test/test_ms.py 2009-01-28 08:28:15 UTC (rev 10360)
@@ -141,5 +141,5 @@
self.assertEquals(1, self.callback_data.actuator_states[1].num_encoder_errors)
if __name__ == '__main__':
- rospy.ready(NAME)
+ rospy.init_node(NAME)
rostest.run(PKG, NAME, TestMechanismState, sys.argv)
Modified: pkg/trunk/mechanism/mechanism_control/test/test_ms_cpp.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/test/test_ms_cpp.py 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/mechanism/mechanism_control/test/test_ms_cpp.py 2009-01-28 08:28:15 UTC (rev 10360)
@@ -113,5 +113,5 @@
self.assertEquals(0, f.num_encoder_errors)
if __name__ == '__main__':
- rospy.ready(NAME)
+ rospy.init_node(NAME)
rostest.run(PKG, NAME, TestMechanismState, sys.argv)
Modified: pkg/trunk/nav/trajectory_rollout/scripts/listen.py
===================================================================
--- pkg/trunk/nav/trajectory_rollout/scripts/listen.py 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/nav/trajectory_rollout/scripts/listen.py 2009-01-28 08:28:15 UTC (rev 10360)
@@ -13,7 +13,7 @@
def listener_with_user_data():
rospy.TopicSub("/goal", Planner2DGoal, callback)
- rospy.ready(NAME, anonymous=True)
+ rospy.init_node(NAME, anonymous=True)
rospy.spin()
if __name__ == '__main__':
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_prototype1/motor_monitor.py
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_prototype1/motor_monitor.py 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_prototype1/motor_monitor.py 2009-01-28 08:28:15 UTC (rev 10360)
@@ -21,7 +21,7 @@
def listener_with_user_data():
rospy.TopicSub("/diagnostics", DiagnosticMessage, callback)
- rospy.ready(NAME, anonymous=True)
+ rospy.init_node(NAME, anonymous=True)
rospy.spin()
if __name__ == '__main__':
Modified: pkg/trunk/simulators/simulator_integration_tests/world_3d_map_gazebo_tests/testcloud.py
===================================================================
--- pkg/trunk/simulators/simulator_integration_tests/world_3d_map_gazebo_tests/testcloud.py 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/simulators/simulator_integration_tests/world_3d_map_gazebo_tests/testcloud.py 2009-01-28 08:28:15 UTC (rev 10360)
@@ -73,7 +73,7 @@
def test_cloud(self):
print "LNK\n"
rospy.TopicSub("world_3d_map", PointCloud, self.pointInput)
- rospy.ready(NAME, anonymous=True)
+ rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 30.0 #30 seconds
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
time.sleep(0.1)
Modified: pkg/trunk/util/ntp_monitor/ntp_monitor.py
===================================================================
--- pkg/trunk/util/ntp_monitor/ntp_monitor.py 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/util/ntp_monitor/ntp_monitor.py 2009-01-28 08:28:15 UTC (rev 10360)
@@ -52,7 +52,7 @@
return
else:
pub = rospy.Publisher("/diagnostics", DiagnosticMessage)
- rospy.ready(NAME, anonymous=True)
+ rospy.init_node(NAME, anonymous=True)
hostname = socket.gethostbyaddr(socket.gethostname())[0]
Modified: pkg/trunk/vision/visual_odometry/nodes/camview_py.py
===================================================================
--- pkg/trunk/vision/visual_odometry/nodes/camview_py.py 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/vision/visual_odometry/nodes/camview_py.py 2009-01-28 08:28:15 UTC (rev 10360)
@@ -247,7 +247,7 @@
vod = VODemo(mode, library)
- rospy.ready('camview_py')
+ rospy.init_node('camview_py')
rospy.spin()
vod.dump()
Modified: pkg/trunk/vision/visual_odometry/nodes/corrector.py
===================================================================
--- pkg/trunk/vision/visual_odometry/nodes/corrector.py 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/vision/visual_odometry/nodes/corrector.py 2009-01-28 08:28:15 UTC (rev 10360)
@@ -177,7 +177,7 @@
corr = Corrector(vo, library)
- rospy.ready('corrector')
+ rospy.init_node('corrector')
try:
corr.workloop()
except KeyboardInterrupt:
Modified: pkg/trunk/vision/visual_odometry/nodes/lineperf.py
===================================================================
--- pkg/trunk/vision/visual_odometry/nodes/lineperf.py 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/vision/visual_odometry/nodes/lineperf.py 2009-01-28 08:28:15 UTC (rev 10360)
@@ -118,7 +118,7 @@
vod = lineperftest(args[1])
- rospy.ready('lineperf_%s' % args[1])
+ rospy.init_node('lineperf_%s' % args[1])
rospy.spin()
if args[1] == 'send':
vod.report()
Modified: pkg/trunk/vision/visual_odometry/nodes/vo.py
===================================================================
--- pkg/trunk/vision/visual_odometry/nodes/vo.py 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/vision/visual_odometry/nodes/vo.py 2009-01-28 08:28:15 UTC (rev 10360)
@@ -177,7 +177,7 @@
def main(args):
vod = VO()
- rospy.ready('vo')
+ rospy.init_node('vo')
rospy.spin()
vod.vo.summarize_timers()
vod.dump()
Modified: pkg/trunk/visualization/pr2_gui/src/pr2_gui/pr2_frame.py
===================================================================
--- pkg/trunk/visualization/pr2_gui/src/pr2_gui/pr2_frame.py 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/visualization/pr2_gui/src/pr2_gui/pr2_frame.py 2009-01-28 08:28:15 UTC (rev 10360)
@@ -59,7 +59,7 @@
def __init__(self, parent, id=wx.ID_ANY, title='PR2 GUI', pos=wx.DefaultPosition, size=(1280, 1024), style=wx.DEFAULT_FRAME_STYLE):
wx.Frame.__init__(self, parent, id, title, pos, size, style)
- rospy.ready('pr2_gui', anonymous=True)
+ rospy.init_node('pr2_gui', anonymous=True)
self._config = wx.Config("pr2_gui")
self._aui_manager = wx.aui.AuiManager(self)
Modified: pkg/trunk/visualization/wxpy_ros/tests/test_try.py
===================================================================
--- pkg/trunk/visualization/wxpy_ros/tests/test_try.py 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/visualization/wxpy_ros/tests/test_try.py 2009-01-28 08:28:15 UTC (rev 10360)
@@ -52,7 +52,7 @@
Thread.__init__(self)
def run(self):
- rospy.ready(NAME, anonymous=True)
+ rospy.init_node(NAME, anonymous=True)
rospy.spin()
channel = MyChannel()
Modified: pkg/trunk/world_models/static_map_server/nodes/map_server
===================================================================
--- pkg/trunk/world_models/static_map_server/nodes/map_server 2009-01-28 08:14:01 UTC (rev 10359)
+++ pkg/trunk/world_models/static_map_server/nodes/map_server 2009-01-28 08:28:15 UTC (rev 10360)
@@ -95,7 +95,7 @@
self.scale = yaml_defs['resolution']
self.origin = yaml_defs['origin']
- rospy.ready(NAME)
+ rospy.init_node(NAME)
s1 = rospy.Service("static_map", std_srvs.srv.StaticMap, self.map_handler)
rospy.spin()
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-01-29 01:03:03
|
Revision: 10385
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10385&view=rev
Author: isucan
Date: 2009-01-29 01:02:59 +0000 (Thu, 29 Jan 2009)
Log Message:
-----------
changes for Wim, so he can implement his controller that makes sure the robot does not hit itself any more
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp
pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp
Added Paths:
-----------
pkg/trunk/util/self_watch/
pkg/trunk/util/self_watch/CMakeLists.txt
pkg/trunk/util/self_watch/Makefile
pkg/trunk/util/self_watch/manifest.xml
pkg/trunk/util/self_watch/self_watch.cpp
pkg/trunk/util/self_watch/self_watch.launch
Modified: pkg/trunk/motion_planning/kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-01-29 00:15:48 UTC (rev 10384)
+++ pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-01-29 01:02:59 UTC (rev 10385)
@@ -12,12 +12,10 @@
<depend package="robot_msgs" />
<depend package="robot_srvs" />
- <depend package="pr2_defs" />
- <depend package="pr2_default_controllers" />
<depend package="wg_robot_description_parser"/>
<depend package="planning_models"/>
<depend package="collision_space"/>
-
+
<depend package="ompl" />
<export>
Added: pkg/trunk/util/self_watch/CMakeLists.txt
===================================================================
--- pkg/trunk/util/self_watch/CMakeLists.txt (rev 0)
+++ pkg/trunk/util/self_watch/CMakeLists.txt 2009-01-29 01:02:59 UTC (rev 10385)
@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+rospack(self_watch)
+rospack_add_executable(self_watch self_watch.cpp)
Added: pkg/trunk/util/self_watch/Makefile
===================================================================
--- pkg/trunk/util/self_watch/Makefile (rev 0)
+++ pkg/trunk/util/self_watch/Makefile 2009-01-29 01:02:59 UTC (rev 10385)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/util/self_watch/manifest.xml
===================================================================
--- pkg/trunk/util/self_watch/manifest.xml (rev 0)
+++ pkg/trunk/util/self_watch/manifest.xml 2009-01-29 01:02:59 UTC (rev 10385)
@@ -0,0 +1,14 @@
+<package>
+ <description>A node that warns when the robot is about to hit itself</description>
+ <author>Ioan A. Sucan</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <depend package="roscpp" />
+ <depend package="rosconsole" />
+
+ <depend package="robot_msgs" />
+ <depend package="wg_robot_description_parser" />
+ <depend package="planning_models" />
+ <depend package="collision_space" />
+
+</package>
Added: pkg/trunk/util/self_watch/self_watch.cpp
===================================================================
--- pkg/trunk/util/self_watch/self_watch.cpp (rev 0)
+++ pkg/trunk/util/self_watch/self_watch.cpp 2009-01-29 01:02:59 UTC (rev 10385)
@@ -0,0 +1,231 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* 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 Willow Garage 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 Ioan Sucan */
+
+
+#include <ros/node.h>
+#include <collision_space/environmentODE.h>
+#include <robot_msgs/MechanismState.h>
+
+class SelfWatch : public ros::Node
+{
+public:
+
+ SelfWatch(void) : ros::Node("self_watch")
+ {
+ setupCollisionSpace();
+ subscribe("mechanism_state", m_mechanismState, &SelfWatch::mechanismStateCallback, this, 1);
+ }
+
+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
+ param("self_collision_scale_factor", m_scaling, 1.2);
+
+ // load the string description of the robot
+ std::string content;
+ if (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->setVerbose(false);
+ m_kmodel->build(*file);
+
+ // make sure the kinematic model is in its own frame
+ // (remove all transforms caused by planar or floating
+ // joints)
+ m_kmodel->reduceToRobotFrame();
+
+ // create a state that can be used to monitor the
+ // changes in the joints of the kinematic model
+ m_robotState = m_kmodel->newStateParams();
+
+ // make sure the transforms caused by the planar and
+ // floating joints are identity, to be compatible with
+ // the fact we are considering the robot in its own
+ // frame
+ m_robotState->setInRobotFrame();
+
+ // create a new collision space
+ m_collisionSpace = 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");
+ else
+ {
+ ROS_INFO("Collision checking enabled for links: ");
+ for (unsigned int i = 0 ; i < links.size() ; ++i)
+ ROS_INFO(" %s", links[i].c_str());
+ }
+
+ // add the robot model to the collision space
+ m_collisionSpace->addRobotModel(m_kmodel, links, m_scaling);
+
+
+ // get the self collision groups and add them to the collision space
+ int nscgroups = 0;
+ std::vector<robot_desc::URDF::Group*> groups;
+ file->getGroups(groups);
+ for (unsigned int i = 0 ; i < groups.size() ; ++i)
+ if (groups[i]->hasFlag("self_collision"))
+ {
+ m_collisionSpace->addSelfCollisionGroup(0, groups[i]->linkNames);
+ ROS_INFO("Self-collision check group %d", nscgroups);
+ for (unsigned int j = 0 ; j < groups[i]->linkNames.size() ; ++j)
+ ROS_INFO(" %s", groups[i]->linkNames[j].c_str());
+ nscgroups++;
+ }
+
+ if (nscgroups == 0)
+ ROS_WARN("No self-collision checking enabled");
+
+ ROS_INFO("Self-collision monitor is active, with scaling %g", m_scaling);
+ }
+ else
+ ROS_ERROR("Unable to parse robot description");
+ }
+ else
+ ROS_ERROR("Could not load robot description");
+ }
+
+ void mechanismStateCallback(void)
+ {
+ bool change = false;
+ if (m_robotState)
+ {
+ unsigned int n = m_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);
+ if (joint)
+ {
+ if (joint->usedParams == 1)
+ {
+ double pos = m_mechanismState.joint_states[i].position;
+ bool this_changed = m_robotState->setParams(&pos, m_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
+ ROS_ERROR("Unknown joint: %s", m_mechanismState.joint_states[i].name.c_str());
+ }
+ }
+ if (change)
+ stateUpdate();
+ }
+
+ void stateUpdate(void)
+ {
+ // when this function is called, we have a collision space set
+ // up and we know the position of the robot has changed
+
+ ros::Time start_time = ros::Time::now();
+
+ // do forward kinematics to compute the new positions of all robot parts of interest
+ m_kmodel->computeTransforms(m_robotState->getParams());
+
+ // ask the collision space to look at the updates
+ m_collisionSpace->updateRobotModel(0);
+
+ // get the first contact point
+ std::vector<collision_space::EnvironmentModel::Contact> contacts;
+ m_collisionSpace->getCollisionContacts(0, contacts, 1);
+
+ if (contacts.size() > 0)
+ {
+ ROS_WARN("Collision found in %g seconds", (ros::Time::now() - start_time).toSec());
+ for (unsigned int i = 0 ; i < contacts.size() ; ++i)
+ {
+ ROS_INFO("Collision between link '%s' and '%s'", contacts[i].link1->name.c_str(), contacts[i].link2 ? contacts[i].link2->name.c_str() : "ENVIRONMENT");
+ ROS_INFO("Contact point (in robot frame): (%g, %g, %g)", contacts[i].pos.x(), contacts[i].pos.y(), contacts[i].pos.z());
+ ROS_INFO("Contact normal: (%g, %g, %g)", contacts[i].normal.x(), contacts[i].normal.y(), contacts[i].normal.z());
+ ROS_INFO("Contact depth: %g", contacts[i].depth);
+ }
+ }
+ }
+
+ // 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;
+
+ // the complete robot state
+ planning_models::KinematicModel::StateParams *m_robotState;
+
+ // the kinematic model
+ planning_models::KinematicModel *m_kmodel;
+
+ // the collision space
+ collision_space::EnvironmentModel *m_collisionSpace;
+
+
+ robot_msgs::MechanismState m_mechanismState;
+
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+
+ SelfWatch sw;
+
+ sw.spin();
+
+ ros::fini();
+ return 0;
+}
Added: pkg/trunk/util/self_watch/self_watch.launch
===================================================================
--- pkg/trunk/util/self_watch/self_watch.launch (rev 0)
+++ pkg/trunk/util/self_watch/self_watch.launch 2009-01-29 01:02:59 UTC (rev 10385)
@@ -0,0 +1,4 @@
+<launch>
+ <param name="self_collision_scale_factor" type="double" value="1.15" />
+ <node pkg="self_watch" type="self_watch" args="robot_description:=robotdesc/pr2" output="screen" />
+</launch>
\ No newline at end of file
Modified: pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h 2009-01-29 00:15:48 UTC (rev 10384)
+++ pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h 2009-01-29 01:02:59 UTC (rev 10385)
@@ -38,6 +38,7 @@
#define COLLISION_SPACE_ENVIRONMENT_MODEL_
#include <planning_models/kinematic.h>
+#include <LinearMath/btVector3.h>
#include <boost/thread/mutex.hpp>
#include <vector>
#include <string>
@@ -65,6 +66,16 @@
{
public:
+ /** Definition of a contact point */
+ struct Contact
+ {
+ btVector3 pos; // contact position
+ btVector3 normal; // normal unit vector at contact
+ double depth; // depth (penetration between bodies)
+ planning_models::KinematicModel::Link *link1; // first link involved in contact
+ planning_models::KinematicModel::Link *link2; // if the contact is between two links, this is not NULL
+ };
+
EnvironmentModel(void)
{
m_selfCollision = true;
@@ -82,6 +93,9 @@
/** Check if a model is in collision */
virtual bool isCollision(unsigned int model_id) = 0;
+
+ /** Get the list of contacts (collisions) */
+ virtual bool getCollisionContacts(unsigned int model_id, std::vector<Contact> &contacts, unsigned int max_count = 1) = 0;
/** Remove all obstacles from collision model */
virtual void clearObstacles(void) = 0;
@@ -92,8 +106,11 @@
/** Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
virtual void addStaticPlane(double a, double b, double c, double d) = 0;
- /** Add a robot model. Ignore robot links if their name is not specified in the string vector */
- virtual unsigned int addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links);
+ /** Add a robot model. Ignore robot links if their name is not
+ specified in the string vector. The scale argument can be
+ used to increase or decrease the size of the robot's
+ bodies */
+ virtual unsigned int addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links, double scale = 1.0);
/** Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(unsigned int model_id) = 0;
Modified: pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h 2009-01-29 00:15:48 UTC (rev 10384)
+++ pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h 2009-01-29 01:02:59 UTC (rev 10385)
@@ -82,6 +82,9 @@
/** Return the space ID for the space in which the particular model is instanciated */
dSpaceID getModelODESpace(unsigned int model_id) const;
+ /** Get the list of contacts (collisions) */
+ virtual bool getCollisionContacts(unsigned int model_id, std::vector<Contact> &contacts, unsigned int max_count = 1);
+
/** Check if a model is in collision */
virtual bool isCollision(unsigned int model_id);
@@ -95,7 +98,7 @@
virtual void addStaticPlane(double a, double b, double c, double d);
/** Add a robot model. Ignore robot links if their name is not specified in the string vector */
- virtual unsigned int addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links);
+ virtual unsigned int addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links, double scale = 1.0);
/** Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(unsigned int model_id);
@@ -110,7 +113,10 @@
virtual int setCollisionCheck(unsigned int model_id, const std::string &link, bool state);
protected:
-
+
+ /** Internal function for collision detection */
+ void testCollision(unsigned int model_id, void *data);
+
class ODECollide2
{
public:
@@ -220,11 +226,12 @@
struct ModelInfo
{
std::vector< kGeom* > linkGeom;
+ double scale;
dSpaceID space;
std::vector< std::vector<unsigned int> > selfCollision;
};
- dGeomID createODEGeom(dSpaceID space, planning_models::KinematicModel::Shape *shape) const;
+ dGeomID createODEGeom(dSpaceID space, planning_models::KinematicModel::Shape *shape, double scale) const;
void updateGeom(dGeomID geom, btTransform &pose) const;
void freeMemory(void);
Modified: pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp 2009-01-29 00:15:48 UTC (rev 10384)
+++ pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp 2009-01-29 01:02:59 UTC (rev 10385)
@@ -41,7 +41,7 @@
m_verbose = verbose;
}
-unsigned int collision_space::EnvironmentModel::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links)
+unsigned int collision_space::EnvironmentModel::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links, double scale)
{
unsigned int pos = m_models.size();
m_models.push_back(model);
Modified: pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp 2009-01-29 00:15:48 UTC (rev 10384)
+++ pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp 2009-01-29 01:02:59 UTC (rev 10385)
@@ -54,14 +54,15 @@
dSpaceDestroy(m_spaceBasicGeoms);
}
-unsigned int collision_space::EnvironmentModelODE::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links)
+unsigned int collision_space::EnvironmentModelODE::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links, double scale)
{
- unsigned int id = collision_space::EnvironmentModel::addRobotModel(model, links);
+ unsigned int id = collision_space::EnvironmentModel::addRobotModel(model, links, scale);
if (m_modelsGeom.size() <= id)
{
m_modelsGeom.resize(id + 1);
m_modelsGeom[id].space = dHashSpaceCreate(0);
+ m_modelsGeom[id].scale = scale;
}
std::map<std::string, bool> exists;
@@ -84,12 +85,12 @@
kGeom *kg = new kGeom();
kg->link = robot->links[i];
kg->enabled = true;
- dGeomID g = createODEGeom(m_modelsGeom[id].space, robot->links[i]->shape);
+ dGeomID g = createODEGeom(m_modelsGeom[id].space, robot->links[i]->shape, scale);
assert(g);
kg->geom.push_back(g);
for (unsigned int k = 0 ; k < kg->link->attachedBodies.size() ; ++k)
{
- dGeomID ga = createODEGeom(m_modelsGeom[id].space, kg->link->attachedBodies[k]->shape);
+ dGeomID ga = createODEGeom(m_modelsGeom[id].space, kg->link->attachedBodies[k]->shape, scale);
assert(ga);
kg->geom.push_back(ga);
}
@@ -99,26 +100,26 @@
return id;
}
-dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, planning_models::KinematicModel::Shape *shape) const
+dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, planning_models::KinematicModel::Shape *shape, double scale) const
{
dGeomID g = NULL;
switch (shape->type)
{
case planning_models::KinematicModel::Shape::SPHERE:
{
- g = dCreateSphere(space, static_cast<planning_models::KinematicModel::Sphere*>(shape)->radius);
+ g = dCreateSphere(space, static_cast<planning_models::KinematicModel::Sphere*>(shape)->radius * scale);
}
break;
case planning_models::KinematicModel::Shape::BOX:
{
const double *size = static_cast<planning_models::KinematicModel::Box*>(shape)->size;
- g = dCreateBox(space, size[0], size[1], size[2]);
+ g = dCreateBox(space, size[0] * scale, size[1] * scale, size[2] * scale);
}
break;
case planning_models::KinematicModel::Shape::CYLINDER:
{
- g = dCreateCylinder(space, static_cast<planning_models::KinematicModel::Cylinder*>(shape)->radius,
- static_cast<planning_models::KinematicModel::Cylinder*>(shape)->length);
+ g = dCreateCylinder(space, static_cast<planning_models::KinematicModel::Cylinder*>(shape)->radius * scale,
+ static_cast<planning_models::KinematicModel::Cylinder*>(shape)->length * scale);
}
break;
default:
@@ -153,7 +154,7 @@
const unsigned int nab = kg->link->attachedBodies.size();
for (unsigned int k = 0 ; k < nab ; ++k)
{
- dGeomID ga = createODEGeom(m_modelsGeom[model_id].space, kg->link->attachedBodies[k]->shape);
+ dGeomID ga = createODEGeom(m_modelsGeom[model_id].space, kg->link->attachedBodies[k]->shape, m_modelsGeom[model_id].scale);
assert(ga);
kg->geom.push_back(ga);
}
@@ -306,50 +307,130 @@
struct CollisionData
{
- bool collides;
+ CollisionData(void)
+ {
+ done = false;
+ collides = false;
+ max_contacts = 0;
+ contacts = NULL;
+ link1 = link2 = NULL;
+ }
+
+ bool done;
+
+ bool collides;
+ unsigned int max_contacts;
+ std::vector<collision_space::EnvironmentModelODE::Contact> *contacts;
+
+ planning_models::KinematicModel::Link *link1;
+ planning_models::KinematicModel::Link *link2;
};
static void nearCallbackFn(void *data, dGeomID o1, dGeomID o2)
{
- bool &coll = reinterpret_cast<CollisionData*>(data)->collides;
- if (!coll)
+ CollisionData *cdata = reinterpret_cast<CollisionData*>(data);
+
+ if (cdata->done)
+ return;
+
+ if (cdata->contacts)
{
+ static const int MAX_CONTACTS = 3;
+ dContact contact[MAX_CONTACTS];
+ int numc = dCollide (o1, o2, MAX_CONTACTS,
+ &contact[0].geom, sizeof(dContact));
+ if (numc)
+ {
+ cdata->collides = true;
+ for (int i = 0 ; i < numc ; ++i)
+ {
+ if (cdata->contacts->size() < cdata->max_contacts)
+ {
+ collision_space::EnvironmentModelODE::Contact add;
+
+ add.pos.setX(contact[i].geom.pos[0]);
+ add.pos.setY(contact[i].geom.pos[1]);
+ add.pos.setZ(contact[i].geom.pos[2]);
+
+ add.normal.setX(contact[i].geom.normal[0]);
+ add.normal.setY(contact[i].geom.normal[1]);
+ add.normal.setZ(contact[i].geom.normal[2]);
+
+ add.depth = contact[i].geom.depth;
+
+ add.link1 = cdata->link1;
+ add.link2 = cdata->link2;
+
+ cdata->contacts->push_back(add);
+ }
+ else
+ break;
+ }
+ }
+ if (cdata->contacts->size() >= cdata->max_contacts)
+ cdata->done = true;
+ }
+ else
+ {
static const int MAX_CONTACTS = 1;
dContact contact[MAX_CONTACTS];
int numc = dCollide (o1, o2, MAX_CONTACTS,
&contact[0].geom, sizeof(dContact));
if (numc)
- coll = true;
+ {
+ cdata->collides = true;
+ cdata->done = true;
+ }
}
}
+bool collision_space::EnvironmentModelODE::getCollisionContacts(unsigned int model_id, std::vector<Contact> &contacts, unsigned int max_count)
+{
+ CollisionData cdata;
+ cdata.contacts = &contacts;
+ cdata.max_contacts = max_count;
+ contacts.clear();
+ testCollision(model_id, reinterpret_cast<void*>(&cdata));
+ return cdata.collides;
+}
+
bool collision_space::EnvironmentModelODE::isCollision(unsigned int model_id)
{
CollisionData cdata;
- cdata.collides = false;
+ testCollision(model_id, reinterpret_cast<void*>(&cdata));
+ return cdata.collides;
+}
+
+void collision_space::EnvironmentModelODE::testCollision(unsigned int model_id, void *data)
+{
+ CollisionData *cdata = reinterpret_cast<CollisionData*>(data);
/* check self collision */
if (m_selfCollision)
{
- for (int i = m_modelsGeom[model_id].selfCollision.size() - 1 ; i >= 0 ; --i)
+ for (int i = m_modelsGeom[model_id].selfCollision.size() - 1 ; i >= 0 && !cdata->done ; --i)
{
const std::vector<unsigned int> &vec = m_modelsGeom[model_id].selfCollision[i];
unsigned int n = vec.size();
- for (unsigned int j = 0 ; j < n ; ++j)
- for (unsigned int k = j + 1 ; k < n ; ++k)
+ for (unsigned int j = 0 ; j < n && !cdata->done ; ++j)
+ {
+ cdata->link1 = m_modelsGeom[model_id].linkGeom[vec[j]]->link;
+ const unsigned int njg = m_modelsGeom[model_id].linkGeom[vec[j]]->geom.size();
+
+ for (unsigned int k = j + 1 ; k < n && !cdata->done; ++k)
{
// dSpaceCollide2 expects AABBs to be computed, so
// we force that by calling dGeomGetAABB. Since we
// get the data anyway, we attempt to speed things
// up using it.
- const unsigned int njg = m_modelsGeom[model_id].linkGeom[vec[j]]->geom.size();
const unsigned int nkg = m_modelsGeom[model_id].linkGeom[vec[k]]->geom.size();
-
+ cdata->link2 = m_modelsGeom[model_id].linkGeom[vec[k]]->link;
+
/* this will account for attached bodies as well */
- for (unsigned int jg = 0 ; jg < njg ; ++jg)
- for (unsigned int kg = 0 ; kg < nkg ; ++kg)
+ for (unsigned int jg = 0 ; jg < njg && !cdata->done; ++jg)
+ for (unsigned int kg = 0 ; kg < nkg && !cdata->done; ++kg)
{
dGeomID g1 = m_modelsGeom[model_id].linkGeom[vec[j]]->geom[jg];
dGeomID g2 = m_modelsGeom[model_id].linkGeom[vec[k]]->geom[kg];
@@ -362,32 +443,31 @@
aabb1[3] < aabb2[2] ||
aabb1[4] > aabb2[5] ||
aabb1[5] < aabb2[4]))
- dSpaceCollide2(g1, g2, reinterpret_cast<void*>(&cdata), nearCallbackFn);
+ dSpaceCollide2(g1, g2, data, nearCallbackFn);
- if (cdata.collides)
- {
- if (m_verbose)
- printf("Self-collision between '%s' and '%s'\n",
- m_modelsGeom[model_id].linkGeom[vec[j]]->link->name.c_str(), m_modelsGeom[model_id].linkGeom[vec[k]]->link->name.c_str());
- goto OUT1;
- }
+ if (cdata->collides && m_verbose)
+ printf("Self-collision between '%s' and '%s'\n",
+ m_modelsGeom[model_id].linkGeom[vec[j]]->link->name.c_str(), m_modelsGeom[model_id].linkGeom[vec[k]]->link->name.c_str());
}
}
+ }
}
}
/* check collision with standalone ode bodies */
- OUT1:
- if (!cdata.collides)
+ if (!cdata->done)
{
- for (int i = m_modelsGeom[model_id].linkGeom.size() - 1 ; i >= 0 ; --i)
+ cdata->link2 = NULL;
+ for (int i = m_modelsGeom[model_id].linkGeom.size() - 1 ; i >= 0 && !cdata->done ; --i)
{
/* skip disabled bodies */
if (!m_modelsGeom[model_id].linkGeom[i]->enabled)
continue;
const unsigned int ng = m_modelsGeom[model_id].linkGeom[i]->geom.size();
- for (unsigned int ig = 0 ; ig < ng ; ++ig)
+ cdata->link1 = m_modelsGeom[model_id].linkGeom[i]->link;
+
+ for (unsigned int ig = 0 ; ig < ng && !cdata->done ; ++ig)
{
dGeomID g1 = m_modelsGeom[model_id].linkGeom[i]->geom[ig];
dReal aabb1[6];
@@ -402,46 +482,38 @@
aabb1[3] < aabb2[2] ||
aabb1[4] > aabb2[5] ||
aabb1[5] < aabb2[4]))
- dSpaceCollide2(g1, g2, reinterpret_cast<void*>(&cdata), nearCallbackFn);
+ dSpaceCollide2(g1, g2, data, nearCallbackFn);
- if (cdata.collides)
- {
- if (m_verbose)
- printf("Collision between static body and link '%s'\n",
- m_modelsGeom[model_id].linkGeom[i]->link->name.c_str());
- goto OUT2;
- }
-
+ if (cdata->collides && m_verbose)
+ printf("Collision between static body and link '%s'\n",
+ m_modelsGeom[model_id].linkGeom[i]->link->name.c_str());
}
}
}
}
/* check collision with pointclouds */
- OUT2:
-
- if (!cdata.collides)
- {
+
+ if (!cdata->done)
+ {
+ cdata->link2 = NULL;
m_collide2.setup();
- for (int i = m_modelsGeom[model_id].linkGeom.size() - 1 ; i >= 0 && !cdata.collides ; --i)
+ for (int i = m_modelsGeom[model_id].linkGeom.size() - 1 ; i >= 0 && !cdata->done ; --i)
if (m_modelsGeom[model_id].linkGeom[i]->enabled)
{
const unsigned int ng = m_modelsGeom[model_id].linkGeom[i]->geom.size();
- for (unsigned int ig = 0 ; ig < ng ; ++ig)
+ cdata->link1 = m_modelsGeom[model_id].linkGeom[i]->link;
+ for (unsigned int ig = 0 ; ig < ng && !cdata->done ; ++ig)
{
- m_collide2.collide(m_modelsGeom[model_id].linkGeom[i]->geom[ig], reinterpret_cast<void*>(&cdata), nearCallbackFn);
- if (cdata.collides)
- {
- if (m_verbose)
- printf("Collision between dynamic body and link '%s'\n",
- m_modelsGeom[model_id].linkGeom[i]->link->name.c_str());
- break;
- }
+ m_collide2.collide(m_modelsGeom[model_id].linkGeom[i]->geom[ig], data, nearCallbackFn);
+ if (cdata->collides && m_verbose)
+ printf("Collision between dynamic body and link '%s'\n",
+ m_modelsGeom[model_id].linkGeom[i]->link->name.c_str());
}
}
}
- return cdata.collides;
+ cdata->done = true;
}
void collision_space::EnvironmentModelODE::addPointCloud(unsigned int n, const double *points)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-01-30 05:25:06
|
Revision: 10398
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10398&view=rev
Author: isucan
Date: 2009-01-30 05:25:03 +0000 (Fri, 30 Jan 2009)
Log Message:
-----------
more complex pose constraints
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPConstraintEvaluator.h
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
pkg/trunk/motion_planning/kinematic_planning/test/avoid_monster.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_position_minimal.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_state.cpp
pkg/trunk/prcore/robot_msgs/msg/PoseConstraint.msg
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-01-30 03:04:05 UTC (rev 10397)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-01-30 05:25:03 UTC (rev 10398)
@@ -58,21 +58,14 @@
PoseConstraintEvaluator *pce = new PoseConstraintEvaluator();
pce->use(m_model->kmodel, pc[i]);
m_pce.push_back(pce);
-
- switch (pc[i].type)
- {
- case robot_msgs::PoseConstraint::ONLY_POSITION:
+
+ // if we have position constraints
+ if (pc[i].type & 0xFF)
threshold += pc[i].position_distance;
- break;
- case robot_msgs::PoseConstraint::ONLY_ORIENTATION:
- threshold += pc[i].orientation_distance;
- break;
- case robot_msgs::PoseConstraint::COMPLETE_POSE:
- threshold += pc[i].position_distance + pc[i].orientation_distance * pc[i].orientation_importance;
- break;
- default:
- break;
- }
+
+ // if we have orientation constraints
+ if (pc[i].type & (~0xFF))
+ threshold += ((pc[i].type & 0xFF) ? pc[i].orientation_importance : 1.0) * pc[i].orientation_distance;
}
}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPConstraintEvaluator.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPConstraintEvaluator.h 2009-01-30 03:04:05 UTC (rev 10397)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPConstraintEvaluator.h 2009-01-30 05:25:03 UTC (rev 10398)
@@ -39,6 +39,7 @@
#include <planning_models/kinematic.h>
#include <robot_msgs/KinematicConstraints.h>
+#include <angles/angles.h>
#include <iostream>
#include <vector>
@@ -110,13 +111,48 @@
{
if (distPos)
{
- if ((m_pc.type == robot_msgs::PoseConstraint::ONLY_POSITION || m_pc.type == robot_msgs::PoseConstraint::COMPLETE_POSE))
+ if (m_pc.type & 0xFF)
{
- btVector3 bodyPos = m_link->globalTrans.getOrigin();
- double dx = bodyPos.getX() - m_pc.pose.position.x;
- double dy = bodyPos.getY() - m_pc.pose.position.y;
- double dz = bodyPos.getZ() - m_pc.pose.position.z;
- *distPos = dx * dx + dy * dy + dz * dz;
+ const btVector3 &bodyPos = m_link->globalTrans.getOrigin();
+ double dx, dy, dz;
+ switch (m_pc.type & 0xFF)
+ {
+ case robot_msgs::PoseConstraint::POSITION_XYZ:
+ dx = bodyPos.getX() - m_pc.x;
+ dy = bodyPos.getY() - m_pc.y;
+ dz = bodyPos.getZ() - m_pc.z;
+ *distPos = dx * dx + dy * dy + dz * dz;
+ break;
+ case robot_msgs::PoseConstraint::POSITION_XY:
+ dx = bodyPos.getX() - m_pc.x;
+ dy = bodyPos.getY() - m_pc.y;
+ *distPos = dx * dx + dy * dy;
+ break;
+ case robot_msgs::PoseConstraint::POSITION_XZ:
+ dx = bodyPos.getX() - m_pc.x;
+ dz = bodyPos.getZ() - m_pc.z;
+ *distPos = dx * dx + dz * dz;
+ break;
+ case robot_msgs::PoseConstraint::POSITION_YZ:
+ dy = bodyPos.getY() - m_pc.y;
+ dz = bodyPos.getZ() - m_pc.z;
+ *distPos = dy * dy + dz * dz;
+ break;
+ case robot_msgs::PoseConstraint::POSITION_X:
+ dx = bodyPos.getX() - m_pc.x;
+ *distPos = dx * dx;
+ break;
+ case robot_msgs::PoseConstraint::POSITION_Y:
+ dy = bodyPos.getY() - m_pc.y;
+ *distPos = dy * dy;
+ break;
+ case robot_msgs::PoseConstraint::POSITION_Z:
+ dz = bodyPos.getZ() - m_pc.z;
+ *distPos = dz * dz;
+ break;
+ default:
+ *distPos = 0.0;
+ }
}
else
*distPos = 0.0;
@@ -124,10 +160,42 @@
if (distAng)
{
- if ((m_pc.type == robot_msgs::PoseConstraint::ONLY_ORIENTATION || m_pc.type == robot_msgs::PoseConstraint::COMPLETE_POSE))
+ if (m_pc.type & (~0xFF))
{
- btQuaternion quat(m_pc.pose.orientation.x, m_pc.pose.orientation.y, m_pc.pose.orientation.z, m_pc.pose.orientation.w);
- *distAng = quat.angle(m_link->globalTrans.getRotation());
+ btScalar yaw, pitch, roll;
+ m_link->globalTrans.getBasis().getEulerYPR(yaw, pitch, roll);
+ switch (m_pc.type & (~0xFF))
+ {
+ case robot_msgs::PoseConstraint::ORIENTATION_RPY:
+ *distAng = angles::shortest_angular_distance(roll, m_pc.roll) +
+ angles::shortest_angular_distance(pitch, m_pc.pitch) +
+ angles::shortest_angular_distance(yaw, m_pc.yaw);
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_RP:
+ *distAng = angles::shortest_angular_distance(roll, m_pc.roll) +
+ angles::shortest_angular_distance(pitch, m_pc.pitch);
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_RY:
+ *distAng = angles::shortest_angular_distance(roll, m_pc.roll) +
+ angles::shortest_angular_distance(yaw, m_pc.yaw);
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_PY:
+ *distAng = angles::shortest_angular_distance(pitch, m_pc.pitch) +
+ angles::shortest_angular_distance(yaw, m_pc.yaw);
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_R:
+ *distAng = angles::shortest_angular_distance(roll, m_pc.roll);
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_P:
+ *distAng = angles::shortest_angular_distance(pitch, m_pc.pitch);
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_Y:
+ *distAng = angles::shortest_angular_distance(yaw, m_pc.yaw);
+ break;
+ default:
+ *distAng = 0.0;
+ break;
+ }
}
else
*distAng = 0.0;
@@ -144,25 +212,9 @@
bool decide(double dPos, double dAng)
{
- bool result = true;
- switch (m_pc.type)
- {
- case robot_msgs::PoseConstraint::ONLY_POSITION:
- result = dPos < m_pc.position_distance;
- break;
-
- case robot_msgs::PoseConstraint::ONLY_ORIENTATION:
- result = dAng < m_pc.orientation_distance;
- break;
-
- case robot_msgs::PoseConstraint::COMPLETE_POSE:
- result = (dPos < m_pc.position_distance) && (dAng < m_pc.orientation_distance);
- break;
-
- default:
- break;
- }
- return result;
+ bool v1 = (m_pc.type & 0xFF) ? dPos < m_pc.position_distance : true;
+ bool v2 = (m_pc.type & (~0xFF)) ? dAng < m_pc.orientation_distance : true;
+ return v1 && v2;
}
const robot_msgs::PoseConstraint& getConstraintMessage(void) const
@@ -175,12 +227,86 @@
if (m_link)
{
out << "Constraint on link '" << m_pc.robot_link << "'" << std::endl;
- if (m_pc.type != robot_msgs::PoseConstraint::ONLY_ORIENTATION)
- out << " Desired position: " << m_pc.pose.position.x << ", " << m_pc.pose.position.y << ", " << m_pc.pose.position.z
- << " (within distance: " << m_pc.position_distance << ")" << std::endl;
- if (m_pc.type != robot_msgs::PoseConstraint::ONLY_POSITION)
- out << " Desired orientation: " << m_pc.pose.orientation.x << ", " << m_pc.pose.orientation.y << ", " << m_pc.pose.orientation.z << ", " << m_pc.pose.orientation.w
- << " (within distance: " << m_pc.orientation_distance << ")" << std::endl;
+
+ if (m_pc.type & 0xFF)
+ {
+ out << " Desired position: ";
+ switch (m_pc.type & 0xFF)
+ {
+ case robot_msgs::PoseConstraint::POSITION_XYZ:
+ out << "x = " << m_pc.x << " ";
+ out << "y = " << m_pc.y << " ";
+ out << "z = " << m_pc.z << " ";
+ break;
+ case robot_msgs::PoseConstraint::POSITION_XY:
+ out << "x = " << m_pc.x << " ";
+ out << "y = " << m_pc.y << " ";
+ break;
+ case robot_msgs::PoseConstraint::POSITION_XZ:
+ out << "x = " << m_pc.x << " ";
+ out << "z = " << m_pc.z << " ";
+ break;
+ case robot_msgs::PoseConstraint::POSITION_YZ:
+ out << "y = " << m_pc.y << " ";
+ out << "z = " << m_pc.z << " ";
+ break;
+ case robot_msgs::PoseConstraint::POSITION_X:
+ out << "x = " << m_pc.x << " ";
+ break;
+ case robot_msgs::PoseConstraint::POSITION_Y:
+ out << "y = " << m_pc.y << " ";
+ break;
+ case robot_msgs::PoseConstraint::POSITION_Z:
+ out << "z = " << m_pc.z << " ";
+ break;
+ default:
+ break;
+ }
+
+ out << " (within distance: " << m_pc.position_distance << ")" << std::endl;
+ }
+
+
+ if (m_pc.type & (~0xFF))
+ {
+ out << " Desired orientation: ";
+ switch (m_pc.type & (~0xFF))
+ {
+ case robot_msgs::PoseConstraint::ORIENTATION_RPY:
+ out << "roll = " << m_pc.roll << " ";
+ out << "pitch = " << m_pc.pitch << " ";
+ out << "yaw = " << m_pc.yaw << " ";
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_RP:
+ out << "roll = " << m_pc.roll << " ";
+ out << "pitch = " << m_pc.pitch << " ";
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_RY:
+ out << "roll = " << m_pc.roll << " ";
+ out << "yaw = " << m_pc.yaw << " ";
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_PY:
+ out << "pitch = " << m_pc.pitch << " ";
+ out << "yaw = " << m_pc.yaw << " ";
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_R:
+ out << "roll = " << m_pc.roll << " ";
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_P:
+ out << "pitch = " << m_pc.pitch << " ";
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_Y:
+ out << "yaw = " << m_pc.yaw << " ";
+ break;
+ default:
+ break;
+ }
+
+ out << " (within distance: " << m_pc.orientation_distance;
+ if (m_pc.type & 0xFF)
+ out << " and with importance " << m_pc.orientation_importance;
+ out << ")" << std::endl;
+ }
}
else
out << "No constraint" << std::endl;
Modified: pkg/trunk/motion_planning/kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-01-30 03:04:05 UTC (rev 10397)
+++ pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-01-30 05:25:03 UTC (rev 10398)
@@ -6,6 +6,7 @@
<depend package="roscpp" />
<depend package="rosconsole" />
<depend package="tf" />
+ <depend package="angles" />
<depend package="std_msgs" />
<depend package="std_srvs" />
Modified: pkg/trunk/motion_planning/kinematic_planning/test/avoid_monster.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/test/avoid_monster.cpp 2009-01-30 03:04:05 UTC (rev 10397)
+++ pkg/trunk/motion_planning/kinematic_planning/test/avoid_monster.cpp 2009-01-30 05:25:03 UTC (rev 10398)
@@ -123,11 +123,11 @@
// the goal region is basically the position of a set of bodies
req.set_goal_constraints_size(1);
- req.goal_constraints[0].type = robot_msgs::PoseConstraint::ONLY_POSITION;
+ req.goal_constraints[0].type = robot_msgs::PoseConstraint::POSITION_XYZ;
req.goal_constraints[0].robot_link = "wrist_flex_right";
- req.goal_constraints[0].pose.position.x = 0.0;
- req.goal_constraints[0].pose.position.y = 0.0;
- req.goal_constraints[0].pose.position.z = -100.0;
+ req.goal_constraints[0].x = 0.0;
+ req.goal_constraints[0].y = 0.0;
+ req.goal_constraints[0].z = -100.0;
req.goal_constraints[0].position_distance = 0.01;
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-01-30 03:04:05 UTC (rev 10397)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-01-30 05:25:03 UTC (rev 10398)
@@ -127,17 +127,15 @@
// set the goal constraints
req.set_goal_constraints_size(1);
- req.goal_constraints[0].type = robot_msgs::PoseConstraint::COMPLETE_POSE;
+ req.goal_constraints[0].type = robot_msgs::PoseConstraint::POSITION_XYZ + robot_msgs::PoseConstraint::ORIENTATION_RY;
req.goal_constraints[0].robot_link = "r_gripper_palm_link";
- req.goal_constraints[0].pose.position.x = 0.75025;
- req.goal_constraints[0].pose.position.y = -0.188;
- req.goal_constraints[0].pose.position.z = 0.829675;
+ req.goal_constraints[0].x = 0.75025;
+ req.goal_constraints[0].y = -0.188;
+ req.goal_constraints[0].z = 0.829675;
- req.goal_constraints[0].pose.orientation.x = 0;
- req.goal_constraints[0].pose.orientation.y = 0;
- req.goal_constraints[0].pose.orientation.z = 0;
- req.goal_constraints[0].pose.orientation.w = 1;
-
+ req.goal_constraints[0].roll = 0.0;
+ req.goal_constraints[0].yaw = 0.0;
+
req.goal_constraints[0].position_distance = 0.001;
req.goal_constraints[0].orientation_distance = 0.03;
req.goal_constraints[0].orientation_importance = 0.01;
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_position_minimal.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_position_minimal.cpp 2009-01-30 03:04:05 UTC (rev 10397)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_position_minimal.cpp 2009-01-30 05:25:03 UTC (rev 10398)
@@ -80,18 +80,16 @@
// place some constraints on the goal
req.set_goal_constraints_size(1);
- // other options are ONLY_POSITION, ONLY_ORIENTATION
// see the constraints message definition
- req.goal_constraints[0].type = robot_msgs::PoseConstraint::COMPLETE_POSE;
- req.goal_constraints[0].robot_link = "r_gripper_palm_link";
- req.goal_constraints[0].pose.position.x = 0.75025;
- req.goal_constraints[0].pose.position.y = -0.188;
- req.goal_constraints[0].pose.position.z = 0.829675;
- req.goal_constraints[0].pose.orientation.x = 0;
- req.goal_constraints[0].pose.orientation.y = 0;
- req.goal_constraints[0].pose.orientation.z = 0;
- req.goal_constraints[0].pose.orientation.w = 1;
+ req.goal_constraints[0].type = robot_msgs::PoseConstraint::POSITION_XYZ + robot_msgs::PoseConstraint::ORIENTATION_RY;
+ req.goal_constraints[0].robot_link = "r_gripper_palm_link";
+ req.goal_constraints[0].x = 0.75025;
+ req.goal_constraints[0].y = -0.188;
+ req.goal_constraints[0].z = 0.829675;
+ req.goal_constraints[0].roll = 0.0;
+ req.goal_constraints[0].yaw = 0.0;
+
// the threshold for solution is position_distance + orientation_distance * orientation_importance
// but the distance requirements must be satisfied by
req.goal_constraints[0].position_distance = 0.005; // in L2square norm
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_state.cpp 2009-01-30 03:04:05 UTC (rev 10397)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_state.cpp 2009-01-30 05:25:03 UTC (rev 10398)
@@ -119,11 +119,11 @@
// are set large so that we always find a solution
// all these values have to be in the base_link frame
req.constraints.set_pose_size(1);
- req.constraints.pose[0].type = robot_msgs::PoseConstraint::ONLY_POSITION;
+ req.constraints.pose[0].type = robot_msgs::PoseConstraint::POSITION_XYZ;
req.constraints.pose[0].robot_link = "r_gripper_palm_link";
- req.constraints.pose[0].pose.position.x = 1;
- req.constraints.pose[0].pose.position.y = 1;
- req.constraints.pose[0].pose.position.z = 1;
+ req.constraints.pose[0].x = 1;
+ req.constraints.pose[0].y = 1;
+ req.constraints.pose[0].z = 1;
req.constraints.pose[0].position_distance = 10.0; // L2Square
// allow 1 second computation time
Modified: pkg/trunk/prcore/robot_msgs/msg/PoseConstraint.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/PoseConstraint.msg 2009-01-30 03:04:05 UTC (rev 10397)
+++ pkg/trunk/prcore/robot_msgs/msg/PoseConstraint.msg 2009-01-30 05:25:03 UTC (rev 10398)
@@ -2,22 +2,43 @@
# Since there are multiple types of constraints, the 'type' member is used
# to identify the different constraints
-byte COMPLETE_POSE=0 # 0 = complete pose is considered
-byte ONLY_POSITION=1 # 1 = only pose position is considered
-byte ONLY_ORIENTATION=2 # 2 = only pose orientation is considered
-byte type
+int32 POSITION_XYZ=1 # only x,y,z of position is considered
+int32 POSITION_XY=2 # only x,y of position is considered
+int32 POSITION_XZ=3 # only x,z of position is considered
+int32 POSITION_YZ=4 # only y,z of position is considered
+int32 POSITION_X=5 # only x of position is considered
+int32 POSITION_Y=6 # only y of position is considered
+int32 POSITION_Z=7 # only z of position is considered
+# the next values can be combined with one of the above, so they are offset by
+# 256, so we can use bit operations on them
+
+int32 ORIENTATION_RPY=256 # only roll, pitch, yaw of orientation is considered
+int32 ORIENTATION_RY=512 # only roll, yaw of orientation is considered
+int32 ORIENTATION_RP=768 # only roll, yaw of orientation is considered
+int32 ORIENTATION_PY=1024 # only roll, yaw of orientation is considered
+int32 ORIENTATION_R=1280 # only roll, yaw of orientation is considered
+int32 ORIENTATION_P=1536 # only roll, yaw of orientation is considered
+int32 ORIENTATION_Y=1792 # only roll, yaw of orientation is considered
+
+int32 type
+
# The robot link this constraint refers to
string robot_link
-# The desired pose of the robot link
-std_msgs/Pose pose
+# The desired pose of the robot link (in the robot frame)
+float64 x
+float64 y
+float64 z
+float64 roll # around Z axis
+float64 pitch # around X axis
+float64 yaw # around Y axis
# the allowed difference (square of euclidian distance) for position
float64 position_distance
-# the allowed difference (shortest angular distance) for orientation
+# the allowed difference (shortest angular distance, in radians) for orientation
float64 orientation_distance
# a factor that gets multiplied to the orientation when computing the
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-01-30 06:58:43
|
Revision: 10400
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10400&view=rev
Author: isucan
Date: 2009-01-30 06:58:36 +0000 (Fri, 30 Jan 2009)
Log Message:
-----------
added padding option for collision space
Modified Paths:
--------------
pkg/trunk/util/self_watch/self_watch.cpp
pkg/trunk/util/self_watch/self_watch.launch
pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp
pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp
Modified: pkg/trunk/util/self_watch/self_watch.cpp
===================================================================
--- pkg/trunk/util/self_watch/self_watch.cpp 2009-01-30 05:47:21 UTC (rev 10399)
+++ pkg/trunk/util/self_watch/self_watch.cpp 2009-01-30 06:58:36 UTC (rev 10400)
@@ -59,6 +59,7 @@
// increase the robot parts by x%, to detect collisions before they happen
param("self_collision_scale_factor", m_scaling, 1.2);
+ param("self_collision_padding", m_padding, 0.05);
// load the string description of the robot
std::string content;
@@ -111,7 +112,7 @@
}
// add the robot model to the collision space
- m_collisionSpace->addRobotModel(m_kmodel, links, m_scaling);
+ m_collisionSpace->addRobotModel(m_kmodel, links, m_scaling, m_padding);
// get the self collision groups and add them to the collision space
@@ -131,7 +132,7 @@
if (nscgroups == 0)
ROS_WARN("No self-collision checking enabled");
- ROS_INFO("Self-collision monitor is active, with scaling %g", m_scaling);
+ ROS_INFO("Self-collision monitor is active, with scaling %g, padding %g", m_scaling, m_padding);
}
else
ROS_ERROR("Unable to parse robot description");
@@ -203,6 +204,7 @@
// 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;
// the complete robot state
planning_models::KinematicModel::StateParams *m_robotState;
Modified: pkg/trunk/util/self_watch/self_watch.launch
===================================================================
--- pkg/trunk/util/self_watch/self_watch.launch 2009-01-30 05:47:21 UTC (rev 10399)
+++ pkg/trunk/util/self_watch/self_watch.launch 2009-01-30 06:58:36 UTC (rev 10400)
@@ -1,4 +1,5 @@
<launch>
- <param name="self_collision_scale_factor" type="double" value="1.15" />
+ <param name="self_collision_scale_factor" type="double" value="1.05" />
+ <param name="self_collision_padding" type="double" value="0.05" />
<node pkg="self_watch" type="self_watch" args="robot_description:=robotdesc/pr2" output="screen" />
</launch>
\ No newline at end of file
Modified: pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h 2009-01-30 05:47:21 UTC (rev 10399)
+++ pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h 2009-01-30 06:58:36 UTC (rev 10400)
@@ -109,8 +109,10 @@
/** Add a robot model. Ignore robot links if their name is not
specified in the string vector. The scale argument can be
used to increase or decrease the size of the robot's
- bodies */
- virtual unsigned int addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links, double scale = 1.0);
+ bodies (multiplicative factor). The padding can be used to
+ increase or decrease the robot's bodies with by an
+ additive term */
+ virtual unsigned int addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
/** Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(unsigned int model_id) = 0;
Modified: pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h 2009-01-30 05:47:21 UTC (rev 10399)
+++ pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h 2009-01-30 06:58:36 UTC (rev 10400)
@@ -97,8 +97,13 @@
/** Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
virtual void addStaticPlane(double a, double b, double c, double d);
- /** Add a robot model. Ignore robot links if their name is not specified in the string vector */
- virtual unsigned int addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links, double scale = 1.0);
+ /** Add a robot model. Ignore robot links if their name is not
+ specified in the string vector. The scale argument can be
+ used to increase or decrease the size of the robot's
+ bodies (multiplicative factor). The padding can be used to
+ increase or decrease the robot's bodies with by an
+ additive term */
+ virtual unsigned int addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
/** Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(unsigned int model_id);
@@ -227,11 +232,12 @@
{
std::vector< kGeom* > linkGeom;
double scale;
+ double padding;
dSpaceID space;
std::vector< std::vector<unsigned int> > selfCollision;
};
- dGeomID createODEGeom(dSpaceID space, planning_models::KinematicModel::Shape *shape, double scale) const;
+ dGeomID createODEGeom(dSpaceID space, planning_models::KinematicModel::Shape *shape, double scale, double padding) const;
void updateGeom(dGeomID geom, btTransform &pose) const;
void freeMemory(void);
Modified: pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp 2009-01-30 05:47:21 UTC (rev 10399)
+++ pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp 2009-01-30 06:58:36 UTC (rev 10400)
@@ -41,7 +41,7 @@
m_verbose = verbose;
}
-unsigned int collision_space::EnvironmentModel::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links, double scale)
+unsigned int collision_space::EnvironmentModel::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links, double scale, double padding)
{
unsigned int pos = m_models.size();
m_models.push_back(model);
Modified: pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp 2009-01-30 05:47:21 UTC (rev 10399)
+++ pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp 2009-01-30 06:58:36 UTC (rev 10400)
@@ -54,15 +54,16 @@
dSpaceDestroy(m_spaceBasicGeoms);
}
-unsigned int collision_space::EnvironmentModelODE::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links, double scale)
+unsigned int collision_space::EnvironmentModelODE::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links, double scale, double padding)
{
- unsigned int id = collision_space::EnvironmentModel::addRobotModel(model, links, scale);
+ unsigned int id = collision_space::EnvironmentModel::addRobotModel(model, links, scale, padding);
if (m_modelsGeom.size() <= id)
{
m_modelsGeom.resize(id + 1);
m_modelsGeom[id].space = dHashSpaceCreate(0);
m_modelsGeom[id].scale = scale;
+ m_modelsGeom[id].padding = padding;
}
std::map<std::string, bool> exists;
@@ -85,12 +86,12 @@
kGeom *kg = new kGeom();
kg->link = robot->links[i];
kg->enabled = true;
- dGeomID g = createODEGeom(m_modelsGeom[id].space, robot->links[i]->shape, scale);
+ dGeomID g = createODEGeom(m_modelsGeom[id].space, robot->links[i]->shape, scale, padding);
assert(g);
kg->geom.push_back(g);
for (unsigned int k = 0 ; k < kg->link->attachedBodies.size() ; ++k)
{
- dGeomID ga = createODEGeom(m_modelsGeom[id].space, kg->link->attachedBodies[k]->shape, scale);
+ dGeomID ga = createODEGeom(m_modelsGeom[id].space, kg->link->attachedBodies[k]->shape, scale, padding);
assert(ga);
kg->geom.push_back(ga);
}
@@ -100,26 +101,26 @@
return id;
}
-dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, planning_models::KinematicModel::Shape *shape, double scale) const
+dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, planning_models::KinematicModel::Shape *shape, double scale, double padding) const
{
dGeomID g = NULL;
switch (shape->type)
{
case planning_models::KinematicModel::Shape::SPHERE:
{
- g = dCreateSphere(space, static_cast<planning_models::KinematicModel::Sphere*>(shape)->radius * scale);
+ g = dCreateSphere(space, static_cast<planning_models::KinematicModel::Sphere*>(shape)->radius * scale + padding);
}
break;
case planning_models::KinematicModel::Shape::BOX:
{
const double *size = static_cast<planning_models::KinematicModel::Box*>(shape)->size;
- g = dCreateBox(space, size[0] * scale, size[1] * scale, size[2] * scale);
+ g = dCreateBox(space, size[0] * scale + padding, size[1] * scale + padding, size[2] * scale + padding);
}
break;
case planning_models::KinematicModel::Shape::CYLINDER:
{
- g = dCreateCylinder(space, static_cast<planning_models::KinematicModel::Cylinder*>(shape)->radius * scale,
- static_cast<planning_models::KinematicModel::Cylinder*>(shape)->length * scale);
+ g = dCreateCylinder(space, static_cast<planning_models::KinematicModel::Cylinder*>(shape)->radius * scale + padding,
+ static_cast<planning_models::KinematicModel::Cylinder*>(shape)->length * scale + padding);
}
break;
default:
@@ -154,7 +155,7 @@
const unsigned int nab = kg->link->attachedBodies.size();
for (unsigned int k = 0 ; k < nab ; ++k)
{
- dGeomID ga = createODEGeom(m_modelsGeom[model_id].space, kg->link->attachedBodies[k]->shape, m_modelsGeom[model_id].scale);
+ dGeomID ga = createODEGeom(m_modelsGeom[model_id].space, kg->link->attachedBodies[k]->shape, m_modelsGeom[model_id].scale, m_modelsGeom[model_id].padding);
assert(ga);
kg->geom.push_back(ga);
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-01-30 22:55:26
|
Revision: 10406
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10406&view=rev
Author: isucan
Date: 2009-01-30 22:33:17 +0000 (Fri, 30 Jan 2009)
Log Message:
-----------
updated doc, some messages, added code to test the attached object functionality
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/CMakeLists.txt
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
pkg/trunk/motion_planning/kinematic_planning/src/state_validity_monitor.cpp
pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStatus.msg
Added Paths:
-----------
pkg/trunk/motion_planning/kinematic_planning/src/display_planner_collision_model.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/CMakeLists.txt 2009-01-30 20:58:22 UTC (rev 10405)
+++ pkg/trunk/motion_planning/kinematic_planning/CMakeLists.txt 2009-01-30 22:33:17 UTC (rev 10406)
@@ -7,9 +7,11 @@
rospack_add_executable(kinematic_planning src/kinematic_planning.cpp)
rospack_add_executable(motion_validator src/motion_validator.cpp)
rospack_add_executable(state_validity_monitor src/state_validity_monitor.cpp)
+rospack_add_executable(display_planner_collision_model src/display_planner_collision_model.cpp)
rospack_link_boost(kinematic_planning thread)
rospack_link_boost(motion_validator thread)
rospack_link_boost(state_validity_monitor thread)
+rospack_link_boost(display_planner_collision_model thread)
add_subdirectory(test)
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-01-30 20:58:22 UTC (rev 10405)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-01-30 22:33:17 UTC (rev 10406)
@@ -56,12 +56,9 @@
@section topic ROS topics
Subscribes to (name/type):
- - @b collision_map/CollisionMap : data describing the 3D environment
- - @b attach_object/AttachedObject : data describing an object to be attached to a link
+ - @b "collision_map"/CollisionMap : data describing the 3D environment
+ - @b "attach_object"/AttachedObject : data describing an object to be attached to a link
- Additional subscriptions due to inheritance from KinematicStateMonitor:
- - @b robot_srvs/MechanismModel : position for each of the robot's joints
-
Publishes to (name/type):
- None
@@ -73,7 +70,7 @@
- None
Provides (name/type):
- - @b set_collision_state/CollisionCheckState : service to allow enabling and disabling collision checking for links
+ - @b "set_collision_state"/CollisionCheckState : service to allow enabling and disabling collision checking for links
<hr>
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-01-30 20:58:22 UTC (rev 10405)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-01-30 22:33:17 UTC (rev 10406)
@@ -65,8 +65,9 @@
@section topic ROS topics
Subscribes to (name/type):
- - @b robot_srvs/MechanismModel : position for each of the robot's joints
-
+ - @b "mechanism_model"/MechanismModel : position for each of the robot's joints
+ - @b "odom_combined"/PoseWithCovariance : localized robot pose
+
Publishes to (name/type):
- None
@@ -82,6 +83,9 @@
<hr>
+ @section parameters ROS parameters
+ - @b "robot_description"/string : the URDF description of the robot we are monitoring
+
**/
class KinematicStateMonitor
Added: pkg/trunk/motion_planning/kinematic_planning/src/display_planner_collision_model.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/display_planner_collision_model.cpp (rev 0)
+++ pkg/trunk/motion_planning/kinematic_planning/src/display_planner_collision_model.cpp 2009-01-30 22:33:17 UTC (rev 10406)
@@ -0,0 +1,205 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* 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 Willow Garage 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 Ioan Sucan */
+
+/**
+
+@htmlinclude ../manifest.html
+
+@b DisplayPlannerCollisionModel is a node that displays the state of
+the robot's collision space, as seen by the planner
+
+<hr>
+
+@section usage Usage
+@verbatim
+$ display_planner_collision_model [standard ROS args]
+@endverbatim
+
+@par Example
+
+@verbatim
+$ display_planner_collision_model robot_description:=robotdesc/pr2
+@endverbatim
+
+<hr>
+
+@section topic ROS topics
+
+Subscribes to (name/type):
+- None
+
+Publishes to (name/type):
+- @b "visualizationMarker"/VisualizationMarker
+
+<hr>
+
+@section services ROS services
+
+Uses (name/type):
+- None
+
+Provides (name/type):
+- None
+
+
+<hr>
+
+@section parameters ROS parameters
+- None
+
+**/
+
+#include "kinematic_planning/CollisionSpaceMonitor.h"
+
+#include <std_msgs/Byte.h>
+#include <robot_msgs/VisualizationMarker.h>
+
+#include <iostream>
+#include <sstream>
+#include <string>
+#include <map>
+using namespace kinematic_planning;
+
+class DisplayPlannerCollisionModel : public ros::Node,
+ public CollisionSpaceMonitor
+{
+public:
+
+ DisplayPlannerCollisionModel(void) : ros::Node("display_planner_collision_model"),
+ CollisionSpaceMonitor(dynamic_cast<ros::Node*>(this)),
+ id_(0)
+ {
+ advertise<robot_msgs::VisualizationMarker>("visualizationMarker", 10240);
+
+ advertise<robot_msgs::AttachedObject>("attach_object", 1);
+ }
+
+ virtual ~DisplayPlannerCollisionModel(void)
+ {
+ }
+
+protected:
+
+ void afterWorldUpdate(void)
+ {
+ CollisionSpaceMonitor::afterWorldUpdate();
+
+ unsigned int n = m_collisionMap.get_boxes_size();
+ for (unsigned int i = 0 ; i < n ; ++i)
+ {
+ sendPoint(m_collisionMap.boxes[i].center.x,
+ m_collisionMap.boxes[i].center.y,
+ m_collisionMap.boxes[i].center.z,
+ radiusOfBox(m_collisionMap.boxes[i].extents),
+ m_collisionMap.header, 1);
+ }
+ }
+
+ void afterAttachBody(planning_models::KinematicModel::Link *link)
+ {
+ rostools::Header header = m_attachedObject.header;
+ header.frame_id = link->name;
+ for (unsigned int i = 0 ; i < link->attachedBodies.size() ; ++i)
+ {
+ planning_models::KinematicModel::Box *box = dynamic_cast<planning_models::KinematicModel::Box*>(link->attachedBodies[i]->shape);
+ if (box)
+ {
+ btVector3 &v = link->attachedBodies[i]->attachTrans.getOrigin();
+ sendPoint(v.x(), v.y(), v.z(), std::max(std::max(box->size[0], box->size[1]), box->size[2] / 2.0), header, 0);
+ }
+ }
+ }
+
+
+private:
+
+ void sendPoint(double x, double y, double z, double radius, const rostools::Header &header, int color)
+ {
+ robot_msgs::VisualizationMarker mk;
+ mk.header = header;
+
+ mk.id = id_++;
+ mk.type = robot_msgs::VisualizationMarker::SPHERE;
+ mk.action = robot_msgs::VisualizationMarker::ADD;
+ mk.x = x;
+ mk.y = y;
+ mk.z = z;
+
+ mk.roll = 0;
+ mk.pitch = 0;
+ mk.yaw = 0;
+
+ mk.xScale = radius * 2.0;
+ mk.yScale = radius * 2.0;
+ mk.zScale = radius * 2.0;
+
+ mk.alpha = 255;
+
+ if (color == 0)
+ {
+ mk.r = 255;
+ mk.g = 10;
+ mk.b = 10;
+ }
+ else
+ {
+ mk.r = 10;
+ mk.g = 255;
+ mk.b = 10;
+ }
+
+ publish("visualizationMarker", mk);
+ }
+
+ int id_;
+
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+
+ DisplayPlannerCollisionModel *disp = new DisplayPlannerCollisionModel();
+ disp->loadRobotDescription();
+
+ disp->spin();
+ disp->shutdown();
+
+
+ delete disp;
+
+ return 0;
+}
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-01-30 20:58:22 UTC (rev 10405)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-01-30 22:33:17 UTC (rev 10406)
@@ -68,8 +68,8 @@
setup/configuration code, there exists a base class that defines the
functionality and an inherited class for each type of planner that can
be instantiated. The planners are associated to string names: RRT,
-LazyRRT, EST, SBL. These string names can be used for the planner_id
-component of the planning request.
+LazyRRT, EST, SBL, IKSBL. These string names can be used for the
+planner_id component of the planning request.
When checking states for validity, a resolution at which paths are
check needs to be defined. To make things easier for the user, this
@@ -80,20 +80,33 @@
\todo
- Find a better way to specify resolution for state validity
checking.
-- Move code from header files to .cpp files (maybe define a library?)
+When using replanning, this node monitors the current state of the
+robot and that of the environment. When a change is detected, the
+currently executed path is checked for validity. If the current path
+is no longer valid, the validity status is set to false and a new path
+is computed. When the new path is computed, it is sent in the status
+messsage and validity is set to true (unless no solution is found, in
+which case the status remains invalid; a change in the map or a call
+to force_replanning will make the planner try again). When the planner
+detects that the robot reached the desired position, it stops the
+replanning thread and sets the done flag to true.
+If the monitored state of the robot and of the environment is not
+updated for a while (see the @ref parameters section), the unsafe flag
+of the planning status is set to true.
+
<hr>
@section usage Usage
@verbatim
-$ kinematic_planning robot_model [standard ROS args]
+$ kinematic_planning [standard ROS args]
@endverbatim
@par Example
@verbatim
-$ kinematic_planning robotdesc/pr2
+$ kinematic_planning robot_description:=robotdesc/pr2
@endverbatim
<hr>
@@ -101,21 +114,11 @@
@section topic ROS topics
Subscribes to (name/type):
-- @b "replan_kinematic_path_state"/KinematicPlanStateRequest : given a robot model, starting and goal states,
- this service computes and recomputes a collision free path until the monitored state is actually at the goal or
- stopping is requested. Changes in the collision model trigger replanning.
+- None
-
-- @b "replan_kinematic_path_position"/KinematicPlanStateRequest : given a robot model, starting state and goal
- poses of certain links, this service computes a collision free path until the monitored state is actually at the
-goal or stopping is requested. Changes in the collision model trigger replanning.
-
-- @b "replan_stop"/Empty : signal the planner to stop replanning
-
-Additional subscriptions due to inheritance from CollisionSpaceMonitor:
-
Publishes to (name/type):
-- @b "path_to_goal"/KinematicPath : the current path to goal (published when replanning)
+- @b "kinematic_planning_status"/KinematicPlanStatus : the current path to goal (published when replanning) and the
+ status of the motion planner (whether the path is valid, complete, etc)
<hr>
@@ -125,17 +128,40 @@
- None
Provides (name/type):
+
- @b "plan_kinematic_path_state"/KinematicPlanState : given a robot model, starting and goal
-states, this service computes a collision free path
+ states, this service computes a collision free path
+
- @b "plan_kinematic_path_position"/KinematicPlanLinkPosition : given a robot model, starting
- state and goal poses of certain links, this service computes a collision free path
+ state and goal poses of certain links, this service computes a collision free path
+- @b "replan_kinematic_path_state"/KinematicReplanState : given a robot model, starting and goal states,
+ this service computes and recomputes a collision free path until the monitored state is actually at the goal or
+ stopping is requested. Changes in the collision model trigger replanning. The computed path is published
+ as part of the status message.
+- @b "replan_kinematic_path_position"/KinematicReplanLinkPosition : given a robot model, starting state and goal
+ poses of certain links, this service computes a collision free path until the monitored state is actually at the
+ goal or stopping is requested. Changes in the collision model trigger replanning.
+
+- @b "replan_force"/Empty : signal the planner to replan one more step
+
+- @b "replan_stop"/Empty : signal the planner to stop replanning
+
+
<hr>
@section parameters ROS parameters
-- None
+- @b "refresh_interval_collision_map"/double : if more than this interval passes when receiving a request for motion planning,
+ the unsafe flag is set to true
+- @b "refresh_interval_kinematic_state"/double : if more than this interval passes when receiving a request for motion planning,
+ the unsafe flag is set to true
+
+- @b "refresh_interval_base_pose"/double : if more than this interval passes when receiving a request for motion planning,
+ the unsafe flag is set to true, unless we are planning in the robot frame, in which case, this the pase pose does not matter
+ (the collision map would be in the same frame)
+
**/
#include "kinematic_planning/CollisionSpaceMonitor.h"
@@ -172,6 +198,7 @@
m_currentPlanStatus.distance = -1.0;
m_currentPlanStatus.done = 1;
m_currentPlanStatus.valid = 1;
+ m_currentPlanStatus.unsafe = 0;
m_currentlyExecutedPath.set_states_size(0);
advertiseService("replan_kinematic_path_state", &KinematicPlanning::replanToState);
@@ -439,21 +466,11 @@
bool result = false;
- if (isSafeToPlan())
- {
- result = m_requestState.execute(m_models, req.value, res.value.path, res.value.distance, trivial);
-
- res.value.id = -1;
- res.value.done = trivial ? 1 : 0;
- res.value.valid = res.value.path.get_states_size() > 0;
- }
- else
- {
- res.value.id = -1;
- res.value.done = 0;
- res.value.valid = 0;
- res.value.path.set_states_size(0);
- }
+ res.value.unsafe = isSafeToPlan() ? 0 : 1;
+ result = m_requestState.execute(m_models, req.value, res.value.path, res.value.distance, trivial);
+ res.value.id = -1;
+ res.value.done = trivial ? 1 : 0;
+ res.value.valid = res.value.path.get_states_size() > 0;
return result;
}
@@ -471,27 +488,20 @@
boost::mutex::scoped_lock lock(m_continueReplanningLock);
m_collisionMonitorChange = false;
double distance = 0.0;
+ bool safe = isSafeToPlan();
+
+ currentState(m_currentPlanToStateRequest.start_state);
+ m_currentlyExecutedPathStart = m_currentPlanToStateRequest.start_state;
+ m_requestState.execute(m_models, m_currentPlanToStateRequest, solution, distance, trivial);
- if (isSafeToPlan())
- {
- currentState(m_currentPlanToStateRequest.start_state);
- m_currentlyExecutedPathStart = m_currentPlanToStateRequest.start_state;
- m_requestState.execute(m_models, m_currentPlanToStateRequest, solution, distance, trivial);
+ m_statusLock.lock();
+ m_currentPlanStatus.path = solution;
+ m_currentPlanStatus.distance = distance;
+ m_currentPlanStatus.done = trivial ? 1 : 0;
+ m_currentPlanStatus.valid = solution.get_states_size() > 0 ? 1 : 0;
+ m_currentPlanStatus.unsafe = safe ? 0 : 1;
+ m_statusLock.unlock();
- m_statusLock.lock();
- m_currentPlanStatus.path = solution;
- m_currentPlanStatus.distance = distance;
- m_currentPlanStatus.done = trivial ? 1 : 0;
- m_currentPlanStatus.valid = solution.get_states_size() > 0 ? 1 : 0;
- m_statusLock.unlock();
- }
- else
- {
- m_statusLock.lock();
- m_currentPlanStatus.valid = 0;
- m_statusLock.unlock();
- }
-
if (trivial)
break;
@@ -512,22 +522,13 @@
bool result = false;
- if (isSafeToPlan())
- {
- result = m_requestLinkPosition.execute(m_models, req.value, res.value.path, res.value.distance, trivial);
-
- res.value.id = -1;
- res.value.done = trivial ? 1 : 0;
- res.value.valid = res.value.path.get_states_size() > 0;
- }
- else
- {
- res.value.id = -1;
- res.value.done = 0;
- res.value.valid = 0;
- res.value.path.set_states_size(0);
- }
+ res.value.unsafe = isSafeToPlan() ? 0 : 1;
+ result = m_requestLinkPosition.execute(m_models, req.value, res.value.path, res.value.distance, trivial);
+ res.value.id = -1;
+ res.value.done = trivial ? 1 : 0;
+ res.value.valid = res.value.path.get_states_size() > 0;
+
return result;
}
@@ -545,27 +546,20 @@
boost::mutex::scoped_lock lock(m_continueReplanningLock);
m_collisionMonitorChange = false;
double distance = 0.0;
+ bool safe = isSafeToPlan();
- if (isSafeToPlan())
- {
- currentState(m_currentPlanToPositionRequest.start_state);
- m_currentlyExecutedPathStart = m_currentPlanToPositionRequest.start_state;
- m_requestLinkPosition.execute(m_models, m_currentPlanToPositionRequest, solution, distance, trivial);
-
- m_statusLock.lock();
- m_currentPlanStatus.path = solution;
- m_currentPlanStatus.distance = distance;
- m_currentPlanStatus.done = trivial ? 1 : 0;
- m_currentPlanStatus.valid = solution.get_states_size() > 0 ? 1 : 0;
- m_statusLock.unlock();
- }
- else
- {
- m_statusLock.lock();
- m_currentPlanStatus.valid = 0;
- m_statusLock.unlock();
- }
+ currentState(m_currentPlanToPositionRequest.start_state);
+ m_currentlyExecutedPathStart = m_currentPlanToPositionRequest.start_state;
+ m_requestLinkPosition.execute(m_models, m_currentPlanToPositionRequest, solution, distance, trivial);
+ m_statusLock.lock();
+ m_currentPlanStatus.path = solution;
+ m_currentPlanStatus.distance = distance;
+ m_currentPlanStatus.done = trivial ? 1 : 0;
+ m_currentPlanStatus.valid = solution.get_states_size() > 0 ? 1 : 0;
+ m_currentPlanStatus.unsafe = safe ? 0 : 1;
+ m_statusLock.unlock();
+
if (trivial)
break;
while (m_currentRequestType == R_POSITION && !m_collisionMonitorChange)
Modified: pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-01-30 20:58:22 UTC (rev 10405)
+++ pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-01-30 22:33:17 UTC (rev 10406)
@@ -36,7 +36,6 @@
/**
-@mainpage
@htmlinclude ../manifest.html
@@ -48,13 +47,13 @@
@section usage Usage
@verbatim
-$ motion_validator robot_model [standard ROS args]
+$ motion_validator [standard ROS args]
@endverbatim
@par Example
@verbatim
-$ motion_validator robotdesc/pr2
+$ motion_validator robot_description:=robotdesc/pr2
@endverbatim
<hr>
@@ -64,8 +63,6 @@
Subscribes to (name/type):
- None
-Additional subscriptions due to inheritance from NodeCollisionModel:
-
Publishes to (name/type):
- None
@@ -77,7 +74,7 @@
- None
Provides (name/type):
-- @b "validate_path"/KinematicPlanState : given a robot model, starting and goal states, this service computes a collision free path
+- @b "validate_path"/ValidateKinematicPath : given a robot model, starting and goal states, this service computes whether the straight path is valid
<hr>
@@ -157,7 +154,7 @@
return false;
}
- ROS_INFO("Validating path for '%s'...", req.model_id.c_str());
+ ROS_INFO("Validating direct path for '%s'...", req.model_id.c_str());
const unsigned int dim = model->si->getStateDimension();
ompl::SpaceInformationKinematic::StateKinematic_t start = new ompl::SpaceInformationKinematic::StateKinematic(dim);
Modified: pkg/trunk/motion_planning/kinematic_planning/src/state_validity_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/state_validity_monitor.cpp 2009-01-30 20:58:22 UTC (rev 10405)
+++ pkg/trunk/motion_planning/kinematic_planning/src/state_validity_monitor.cpp 2009-01-30 22:33:17 UTC (rev 10406)
@@ -36,7 +36,6 @@
/**
-@mainpage
@htmlinclude ../manifest.html
@@ -63,8 +62,6 @@
Subscribes to (name/type):
- None
-Additional subscriptions due to inheritance from CollisionSpaceMonitor:
-
Publishes to (name/type):
- @b "state_validity"/Byte : 1 if state is valid, 0 if it is invalid
@@ -104,11 +101,9 @@
StateValidityMonitor(void) : ros::Node("state_validity_monitor"),
CollisionSpaceMonitor(dynamic_cast<ros::Node*>(this)),
- last_(-1),
- id_(0)
+ last_(-1)
{
advertise<std_msgs::Byte>("state_validity", 1);
- advertise<robot_msgs::VisualizationMarker>("visualizationMarker", 10240);
}
virtual ~StateValidityMonitor(void)
@@ -120,21 +115,6 @@
void afterWorldUpdate(void)
{
CollisionSpaceMonitor::afterWorldUpdate();
-
- for (int i = 0 ; i < id_ ; ++i)
- delPoint(i, m_collisionMap.header);
- id_ = 0;
-
- unsigned int n = m_collisionMap.get_boxes_size();
- for (unsigned int i = 0 ; i < n ; ++i)
- {
- sendPoint(m_collisionMap.boxes[i].center.x,
- m_collisionMap.boxes[i].center.y,
- m_collisionMap.boxes[i].center.z,
- radiusOfBox(m_collisionMap.boxes[i].extents),
- m_collisionMap.header);
- }
-
last_ = -1;
}
@@ -165,48 +145,7 @@
private:
- void sendPoint(double x, double y, double z, double radius, const rostools::Header &header)
- {
- robot_msgs::VisualizationMarker mk;
- mk.header = header;
-
- mk.id = id_++;
- mk.type = robot_msgs::VisualizationMarker::SPHERE;
- mk.action = robot_msgs::VisualizationMarker::ADD;
- mk.x = x;
- mk.y = y;
- mk.z = z;
-
- mk.roll = 0;
- mk.pitch = 0;
- mk.yaw = 0;
-
- mk.xScale = radius * 2.0;
- mk.yScale = radius * 2.0;
- mk.zScale = radius * 2.0;
-
- mk.alpha = 255;
- mk.r = 255;
- mk.g = 10;
- mk.b = 10;
-
- publish("visualizationMarker", mk);
- }
-
- void delPoint(int id, const rostools::Header &header)
- {
- robot_msgs::VisualizationMarker mk;
- mk.header = header;
-
- mk.id = id;
- mk.type = robot_msgs::VisualizationMarker::SPHERE;
- mk.action = robot_msgs::VisualizationMarker::DELETE;
-
- publish("visualizationMarker", mk);
- }
-
int last_;
- int id_;
};
Modified: pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStatus.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStatus.msg 2009-01-30 20:58:22 UTC (rev 10405)
+++ pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStatus.msg 2009-01-30 22:33:17 UTC (rev 10406)
@@ -28,3 +28,6 @@
# If computing the path was not possible, this will be set to 0
# otherwise, it will be 1
byte valid
+
+# If the data the planner has when planning is stale, set this flag to 1
+byte unsafe
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tpr...@us...> - 2009-01-31 00:18:03
|
Revision: 10409
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10409&view=rev
Author: tpratkanis
Date: 2009-01-31 00:17:56 +0000 (Sat, 31 Jan 2009)
Log Message:
-----------
Modifiy move_arm2 for old message format.
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/src/move_arm2.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/call_plan_to_state.cpp
pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg
pkg/trunk/pr2_msgs/msg/MoveArmState.msg
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_arm2.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_arm2.cpp 2009-01-30 23:44:58 UTC (rev 10408)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_arm2.cpp 2009-01-31 00:17:56 UTC (rev 10409)
@@ -1,5 +1,4 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
+/********************************************************************* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
@@ -92,6 +91,8 @@
#include <pr2_mechanism_controllers/TrajectoryQuery.h>
#include <pr2_mechanism_controllers/TrajectoryCancel.h>
+#include <planning_models/kinematic.h>
+
#include <sstream>
#include <cassert>
@@ -112,11 +113,12 @@
const std::string& kinematic_model,
const std::string& controller_name);
- virtual ~MoveArm() {};
+ virtual ~MoveArm();
private:
const std::string kinematic_model_;
const std::string controller_name_;
+ planning_models::KinematicModel* robot_model_;
bool have_new_traj_;
robot_msgs::KinematicPath new_traj_;
@@ -146,6 +148,12 @@
void kpsCallback();
};
+MoveArm::~MoveArm() {
+ if (robot_model_) {
+ delete robot_model_;
+ }
+}
+
MoveArm::MoveArm(const std::string& node_name,
const std::string& state_topic,
const std::string& goal_topic,
@@ -154,6 +162,7 @@
: HighlevelController<pr2_msgs::MoveArmState, pr2_msgs::MoveArmGoal>(node_name, state_topic, goal_topic),
kinematic_model_(kinematic_model),
controller_name_(controller_name),
+ robot_model_(NULL),
have_new_traj_(false),
replanning_(false),
traj_id_(-1),
@@ -165,6 +174,22 @@
&MoveArm::kpsCallback,
1);
+ //Create robot model.
+ robot_model_ = new planning_models::KinematicModel();
+ robot_desc::URDF file;
+ std::string model = "/robotdesc/pr2";
+ param("move_arm/model", model, model);
+ std::string data = "";
+ param(model, data, data);
+ if (data == "") {
+ ROS_ERROR("Could not open robot description: %s. Please set move_arm/model.", model.c_str());
+ ROS_ASSERT(false);
+ exit(1);
+ }
+ file.loadString(data.c_str());
+ robot_model_->build(file);
+
+
// Say that we're up and ready
initialize();
}
@@ -172,9 +197,7 @@
void MoveArm::updateGoalMsg()
{
lock();
- stateMsg.implicit_goal = goalMsg.implicit_goal;
- stateMsg.goal_state = goalMsg.goal_state;
- stateMsg.goal_constraints = goalMsg.goal_constraints;
+ stateMsg.configuration = goalMsg.configuration;
unlock();
}
@@ -182,49 +205,86 @@
{
// Not clear whether we need to call lock() here...
- if(!goalMsg.implicit_goal)
+ /*if(!goalMsg.implicit_goal)
+ {*/
+ robot_srvs::KinematicReplanState::request req;
+ robot_srvs::KinematicReplanState::response res;
+
+ req.value.params.model_id = kinematic_model_;
+ req.value.params.distance_metric = "L2Square";
+ req.value.params.planner_id = "SBL";
+ req.value.threshold = 0.01;
+ req.value.interpolate = 1;
+ req.value.times = 1;
+
+ // req.start_state is left empty, because we only support replanning, in
+ // which case the planner monitors the robot's current state.
+
+
+ //Copies in the state.
+ //First create stateparams for the group of intrest.
+ planning_models::KinematicModel::StateParams state(robot_model_);
+
+ //Set the stateparam's values from the goal (need to be locked).
+ goalMsg.lock();
+ for (unsigned int i = 0; i < goalMsg.configuration.size(); i++)
{
- robot_srvs::KinematicReplanState::request req;
- robot_srvs::KinematicReplanState::response res;
+ unsigned int axes = robot_model_->getJoint(goalMsg.configuration[i].name)->usedParams;
+ ROS_ASSERT(axes == 1);
+ double* param = new double[axes];
+ param[0] = goalMsg.configuration[i].position;
+ state.setParams(param, goalMsg.configuration[i].name);
+ delete[] param;
+ }
+ goalMsg.unlock();
+
+ //Debug
+ //state.print();
- req.value.params.model_id = kinematic_model_;
- req.value.params.distance_metric = "L2Square";
- req.value.params.planner_id = "SBL";
- req.value.threshold = 0.01;
- req.value.interpolate = 1;
- req.value.times = 1;
+ //Copy the stateparams in to the req.
+ unsigned int len = robot_model_->getGroupDimension(robot_model_->getGroupID(kinematic_model_));
+ double* param = new double[len];
+ state.copyParams(param, robot_model_->getGroupID(kinematic_model_));
- // req.start_state is left empty, because we only support replanning, in
- // which case the planner monitors the robot's current state.
+ req.value.goal_state.vals.clear();
+ for (unsigned int i = 0; i < len; i++) {
+ //ROS_INFO("%f", param[i]);
+ req.value.goal_state.vals.push_back(param[i]);
+ }
- req.value.goal_state = goalMsg.goal_state;
+ delete[] param;
+
- req.value.allowed_time = 0.5;
-
- //req.params.volume* are left empty, because we're not planning for the
- //base
-
- if(replanning_)
- requestStopReplanning();
- replanning_ = true;
- active_request_state_ = req;
- if(!ros::service::call("replan_kinematic_path_state", req, res))
- {
- ROS_WARN("Service call on replan_kinematic_path_state failed");
- return false;
- }
- else
- {
- plan_id_ = res.id;
- ROS_INFO("Issued a replanning request. Waiting for plan ID %d", plan_id_);
- return true;
- }
+
+ req.value.allowed_time = 0.5;
+
+ //req.params.volume* are left empty, because we're not planning for the
+ //base
+ //Lock here to prevent issues where plan_id_ is not set and a plan is gotten.
+ kps_msg_.lock();
+ bool ret = false;
+ if(replanning_)
+ requestStopReplanning();
+ replanning_ = true;
+ active_request_state_ = req;
+ if(!ros::service::call("replan_kinematic_path_state", req, res))
+ {
+ ROS_WARN("Service call on replan_kinematic_path_state failed");
+ ret = false;
}
else
{
+ plan_id_ = res.id;
+ ROS_INFO("Issued a replanning request. Waiting for plan ID %d", plan_id_);
+ ret = true;
+ }
+ kps_msg_.unlock();
+ return ret;
+ /*}
+ else
+ {
robot_srvs::KinematicReplanLinkPosition::request req;
robot_srvs::KinematicReplanLinkPosition::response res;
-
req.value.params.model_id = kinematic_model_;
req.value.params.distance_metric = "L2Square";
req.value.params.planner_id = "IKSBL";
@@ -256,7 +316,7 @@
ROS_INFO("Issued a replanning request");
return true;
}
- }
+ }*/
}
bool MoveArm::goalReached()
@@ -371,6 +431,8 @@
// by the time have_new_traj_ is looked at, it could be the case a
// new message is received and the trajectory is lost
+ } else {
+ ROS_INFO("KPS message has wrong id: %d, should be: %d", kps_msg_.id, plan_id_);
}
}
}
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/call_plan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/call_plan_to_state.cpp 2009-01-30 23:44:58 UTC (rev 10408)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/call_plan_to_state.cpp 2009-01-31 00:17:56 UTC (rev 10409)
@@ -62,15 +62,12 @@
{
// construct the request for the highlevel controller
pr2_msgs::MoveArmGoal ag;
- ag.implicit_goal = 0;
ag.enable = 1;
ag.timeout = 10.0;
- ag.goal_state.set_vals_size(7);
- for (unsigned int i = 0 ; i <ag.goal_state.get_vals_size(); ++i)
- ag.goal_state.vals[i] = 0.0;
- ag.goal_state.vals[0] = -0.5;
- ag.goal_state.vals[1] = -0.2;
+ ag.set_configuration_size(1);
+ ag.configuration[0].name = "r_shoulder_pan_joint";
+ ag.configuration[0].position = -0.5;
publish("right_arm_goal", ag);
}
Modified: pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg 2009-01-30 23:44:58 UTC (rev 10408)
+++ pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg 2009-01-31 00:17:56 UTC (rev 10409)
@@ -1,12 +1,7 @@
Header header
-# Only one of goal_state or goal_constraints is used. implicit_goal ==
-# 1 means use goal_constraints, 0 means use goal_state
-byte implicit_goal
-# The desired joint state
-robot_msgs/KinematicState goal_state
-# Goal constraints (implicit definition of goal)
-robot_msgs/PoseConstraint[] goal_constraints
+# The desired joint configuration
+robot_msgs/JointState[] configuration
# Indicate if the goal is being enabled or disabled
byte enable
Modified: pkg/trunk/pr2_msgs/msg/MoveArmState.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveArmState.msg 2009-01-30 23:44:58 UTC (rev 10408)
+++ pkg/trunk/pr2_msgs/msg/MoveArmState.msg 2009-01-31 00:17:56 UTC (rev 10409)
@@ -10,11 +10,6 @@
#Was the planner told to stop?
byte preempted
#Current arm configuration
-robot_msgs/KinematicState configuration
-# Only one of goal_state or goal_constraints is used. implicit_goal ==
-# 1 means use goal_constraints, 0 means use goal_state
-byte implicit_goal
-# The desired joint state
-robot_msgs/KinematicState goal_state
-# Goal constraints (implicit definition of goal)
-robot_msgs/PoseConstraint[] goal_constraints
+robot_msgs/JointState[] configuration
+#Goal arm configuration
+robot_msgs/JointState[] goal
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|