[Personalrobots-commit] SF.net SVN: personalrobots:[20559] pkg/trunk

[<is...@us...>]

Revision: 20559
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20559&view=rev
Author:   isucan
Date:     2009-08-03 23:14:44 +0000 (Mon, 03 Aug 2009)

Log Message:
-----------
switching from mechanism_state to joint_states

Modified Paths:
--------------
    pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
    pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
    pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp
    pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h
    pkg/trunk/motion_planning/planning_environment/mainpage.dox
    pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
    pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp
    pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp

Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp	2009-08-03 23:08:40 UTC (rev 20558)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp	2009-08-03 23:14:44 UTC (rev 20559)
@@ -219,7 +219,7 @@
     motion_planning_msgs::KinematicPath kp;	
     
     kp.header.frame_id = km.getFrameId();
-    kp.header.stamp = km.lastMechanismStateUpdate();
+    kp.header.stamp = km.lastJointStateUpdate();
     
     // fill in start state with current one
     std::vector<planning_models::KinematicModel::Joint*> joints;
@@ -229,7 +229,7 @@
     for (unsigned int i = 0 ; i < joints.size() ; ++i)
     {
 	kp.start_state[i].header.frame_id = km.getFrameId();
-	kp.start_state[i].header.stamp = km.lastMechanismStateUpdate();
+	kp.start_state[i].header.stamp = km.lastJointStateUpdate();
 	kp.start_state[i].joint_name = joints[i]->name;
 	st.copyParamsJoint(kp.start_state[i].value, joints[i]->name);
     }

Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp	2009-08-03 23:08:40 UTC (rev 20558)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp	2009-08-03 23:14:44 UTC (rev 20559)
@@ -542,7 +542,7 @@
 	for (unsigned int i = 0 ; i < joints.size() ; ++i)
 	{
 	    start_state[i].header.frame_id = planningMonitor_->getFrameId();
-	    start_state[i].header.stamp = planningMonitor_->lastMechanismStateUpdate();
+	    start_state[i].header.stamp = planningMonitor_->lastJointStateUpdate();
 	    start_state[i].joint_name = joints[i]->name;
 	    st.copyParamsJoint(start_state[i].value, joints[i]->name);
 	}
@@ -591,7 +591,7 @@
 			    motion_planning_msgs::JointConstraint jc;
 			    jc.joint_name = arm_joint_names_[i];
 			    jc.header.frame_id = req.goal_constraints.pose_constraint[0].pose.header.frame_id;
-			    jc.header.stamp = planningMonitor_->lastMechanismStateUpdate();
+			    jc.header.stamp = planningMonitor_->lastJointStateUpdate();
 			    unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
 			    for (unsigned int j = 0 ; j < u ; ++j)
 			    {

Modified: pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp	2009-08-03 23:08:40 UTC (rev 20558)
+++ pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp	2009-08-03 23:14:44 UTC (rev 20559)
@@ -643,7 +643,7 @@
 	return;
     
     visualization_msgs::Marker mk;        
-    mk.header.stamp = psetup->model->planningMonitor->lastMechanismStateUpdate();
+    mk.header.stamp = psetup->model->planningMonitor->lastJointStateUpdate();
     mk.header.frame_id = psetup->model->planningMonitor->getFrameId();
     mk.ns = ros::this_node::getName();
     mk.id = 1;    

Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h	2009-08-03 23:08:40 UTC (rev 20558)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h	2009-08-03 23:14:44 UTC (rev 20559)
@@ -41,7 +41,7 @@
 #include <tf/transform_datatypes.h>
 #include <tf/transform_listener.h>
 #include <tf/message_notifier.h>
-#include <mechanism_msgs/MechanismState.h>
+#include <mechanism_msgs/JointStates.h>
 #include <mapping_msgs/AttachedObject.h>
 #include <boost/bind.hpp>
 #include <vector>
@@ -143,16 +143,16 @@
 	    return robot_frame_;
 	}
 
-	/** \brief Return true if a full mechanism state has been received (including pose, if pose inclusion is enabled) */
+	/** \brief Return true if a full joint state has been received (including pose, if pose inclusion is enabled) */
 	bool haveState(void) const
 	{
-	    return haveMechanismState_ && (!includePose_ || (includePose_ && havePose_));
+	    return haveJointState_ && (!includePose_ || (includePose_ && havePose_));
 	}
 	
 	/** \brief Return the time of the last state update */
-	const ros::Time& lastMechanismStateUpdate(void) const
+	const ros::Time& lastJointStateUpdate(void) const
 	{
-	    return lastMechanismStateUpdate_;
+	    return lastJointStateUpdate_;
 	}
 
 	/** \brief Return the time of the last pose update */
@@ -161,11 +161,11 @@
 	    return lastPoseUpdate_;
 	}
 	
-	/** \brief Wait until a full mechanism state is received (including pose, if pose inclusion is enabled) */
+	/** \brief Wait until a full joint state is received (including pose, if pose inclusion is enabled) */
 	void waitForState(void) const;
 
-	/** \brief Return true if a mechanism state has been received in the last sec seconds. If sec < 10us, this function always returns true. */
-	bool isMechanismStateUpdated(double sec) const;
+	/** \brief Return true if a joint state has been received in the last sec seconds. If sec < 10us, this function always returns true. */
+	bool isJointStateUpdated(double sec) const;
 
 	/** \brief Return true if a pose has been received in the last
 	    sec seconds. If sec < 10us, this function always returns
@@ -184,7 +184,7 @@
     protected:
 
 	void setupRSM(void);
-	void mechanismStateCallback(const mechanism_msgs::MechanismStateConstPtr &mechanismState);
+	void jointStateCallback(const mechanism_msgs::JointStatesConstPtr &jointState);
 	void attachObjectCallback(const mapping_msgs::AttachedObjectConstPtr &attachedObject);
 	virtual bool attachObject(const mapping_msgs::AttachedObjectConstPtr &attachedObject);
 
@@ -197,7 +197,7 @@
 	bool                             stateMonitorStarted_;
 	
 	ros::NodeHandle                  nh_;
-	ros::Subscriber                  mechanismStateSubscriber_;
+	ros::Subscriber                  jointStateSubscriber_;
 	tf::TransformListener           *tf_;
 
 	tf::MessageNotifier<mapping_msgs::AttachedObject>
@@ -213,8 +213,8 @@
 	                                 onAfterAttachBody_;
 
 	bool                             havePose_;
-	bool                             haveMechanismState_;
-	ros::Time                        lastMechanismStateUpdate_;
+	bool                             haveJointState_;
+	ros::Time                        lastJointStateUpdate_;
 	ros::Time                        lastPoseUpdate_;
     };
 

Modified: pkg/trunk/motion_planning/planning_environment/mainpage.dox
===================================================================
--- pkg/trunk/motion_planning/planning_environment/mainpage.dox	2009-08-03 23:08:40 UTC (rev 20558)
+++ pkg/trunk/motion_planning/planning_environment/mainpage.dox	2009-08-03 23:14:44 UTC (rev 20559)
@@ -176,7 +176,7 @@
 <hr>
 
 The monitor classes are:
-- \b planning_environment::KinematicModelStateMonitor : monitors the kinematic state of the robot. Optionally, monitors the base location. It uses the 'mechanism_state' topic.
+- \b planning_environment::KinematicModelStateMonitor : monitors the kinematic state of the robot. Optionally, monitors the base location. It uses the 'joint_states' topic.
 - \b planning_environment::CollisionSpaceMonitor : same as \b planning_environment::KinematicModelStateMonitor except it also monitors the state of the collision environment. It uses the 'collision_map' topic to receive new full maps and the 'collision_map_update' to receive updates. Attaching objects to robot links is possible using the 'attach_object' topic. 
 - \b planning_environment::PlanningMonitor : same as \b planning_environment::CollisionSpaceMonitor except it also offers the ability to evaluate kinematic constraints and check paths and states for validity.
 
@@ -185,7 +185,7 @@
 \subsection topics ROS topics
 
 Subscribes to:
- - @b "mechanism_state"/MechanismState : the parameters describing the robot's current state
+ - @b "joint_states"/JointStates : the parameters describing the robot's current state
  - @b "collision_map"/CollisionMap : data describing the 3D environment
  - @b "collision_map_update"/CollisionMap : updates (additive) to data describing the 3D environment
  - @b "object_in_map"/ObjectInMap : definition of an object identified in the environment (to be used for collision checking)

Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp	2009-08-03 23:08:40 UTC (rev 20558)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp	2009-08-03 23:14:44 UTC (rev 20559)
@@ -48,7 +48,7 @@
     onStateUpdate_ = NULL;
     onAfterAttachBody_ = NULL;
     attachedBodyNotifier_ = NULL;
-    havePose_ = haveMechanismState_ = false;
+    havePose_ = haveJointState_ = false;
 
     if (rm_->loadedModels())
     {
@@ -88,8 +88,8 @@
     
     if (rm_->loadedModels())
     {
-	mechanismStateSubscriber_ = nh_.subscribe("mechanism_state", 1, &KinematicModelStateMonitor::mechanismStateCallback, this);
-	ROS_DEBUG("Listening to mechanism state");
+	jointStateSubscriber_ = nh_.subscribe("joint_states", 1, &KinematicModelStateMonitor::jointStateCallback, this);
+	ROS_DEBUG("Listening to joint states");
 	
 	attachedBodyNotifier_ = new tf::MessageNotifier<mapping_msgs::AttachedObject>(*tf_, boost::bind(&KinematicModelStateMonitor::attachObjectCallback, this, _1), "attach_object", "", 1);
 	attachedBodyNotifier_->setTargetFrame(rm_->getCollisionCheckLinks());
@@ -106,53 +106,53 @@
     delete attachedBodyNotifier_;
     attachedBodyNotifier_ = NULL;
     
-    mechanismStateSubscriber_.shutdown();
+    jointStateSubscriber_.shutdown();
     
     ROS_DEBUG("Kinematic state is no longer being monitored");
     
     stateMonitorStarted_ = false;
 }
 
-void planning_environment::KinematicModelStateMonitor::mechanismStateCallback(const mechanism_msgs::MechanismStateConstPtr &mechanismState)
+void planning_environment::KinematicModelStateMonitor::jointStateCallback(const mechanism_msgs::JointStatesConstPtr &jointState)
 {
-    bool change = !haveMechanismState_;
+    bool change = !haveJointState_;
 
     static bool first_time = true;
 
-    unsigned int n = mechanismState->get_joint_states_size();
+    unsigned int n = jointState->get_joints_size();
     for (unsigned int i = 0 ; i < n ; ++i)
     {
-	planning_models::KinematicModel::Joint* joint = kmodel_->getJoint(mechanismState->joint_states[i].name);
+	planning_models::KinematicModel::Joint* joint = kmodel_->getJoint(jointState->joints[i].name);
 	if (joint)
 	{
 	    if (joint->usedParams == 1)
 	    {
-		double pos = mechanismState->joint_states[i].position;
+		double pos = jointState->joints[i].position;
 		planning_models::KinematicModel::RevoluteJoint* rjoint = dynamic_cast<planning_models::KinematicModel::RevoluteJoint*>(joint);
 		if (rjoint)
 		    if (rjoint->continuous)
 			pos = angles::normalize_angle(pos);
-		bool this_changed = robotState_->setParamsJoint(&pos, mechanismState->joint_states[i].name);
+		bool this_changed = robotState_->setParamsJoint(&pos, jointState->joints[i].name);
 		change = change || this_changed;
 	    }
 	    else
 	    {
 		if (first_time)
-		    ROS_WARN("Incorrect number of parameters: %s (expected %d, had 1)", mechanismState->joint_states[i].name.c_str(), joint->usedParams);
+		    ROS_WARN("Incorrect number of parameters: %s (expected %d, had 1)", jointState->joints[i].name.c_str(), joint->usedParams);
 	    }
 	}
 	else
 	{
 	    if (first_time)
-		ROS_ERROR("Unknown joint: %s", mechanismState->joint_states[i].name.c_str());
+		ROS_ERROR("Unknown joint: %s", jointState->joints[i].name.c_str());
 	}
     }
 
     first_time = false;
 
-    lastMechanismStateUpdate_ = mechanismState->header.stamp;
-    if (!haveMechanismState_)
-	haveMechanismState_ = robotState_->seenAll();
+    lastJointStateUpdate_ = jointState->header.stamp;
+    if (!haveJointState_)
+	haveJointState_ = robotState_->seenAll();
 
     if (includePose_)
     {
@@ -301,13 +301,13 @@
 	ROS_INFO("Robot state received!");
 }
 
-bool planning_environment::KinematicModelStateMonitor::isMechanismStateUpdated(double sec) const
+bool planning_environment::KinematicModelStateMonitor::isJointStateUpdated(double sec) const
 {  
-    if (!haveMechanismState_)
+    if (!haveJointState_)
 	return false;
     
     // less than 10us is considered 0 
-    if (sec > 1e-5 && lastMechanismStateUpdate_ < ros::Time::now() - ros::Duration(sec))
+    if (sec > 1e-5 && lastJointStateUpdate_ < ros::Time::now() - ros::Duration(sec))
 	return false;
     else
 	return true;

Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp	2009-08-03 23:08:40 UTC (rev 20558)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp	2009-08-03 23:14:44 UTC (rev 20559)
@@ -53,7 +53,7 @@
         return false;
     }
   
-    if (!isMechanismStateUpdated(intervalState_))
+    if (!isJointStateUpdated(intervalState_))
     {
         ROS_WARN("Planning is not safe: robot state not updated in the last %f seconds", intervalState_);
         return false;

Modified: pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp	2009-08-03 23:08:40 UTC (rev 20558)
+++ pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp	2009-08-03 23:14:44 UTC (rev 20559)
@@ -131,7 +131,7 @@
     void afterAttachBody(planning_models::KinematicModel::Link *link, const mapping_msgs::AttachedObjectConstPtr &attachedObject)
     {
 	roslib::Header header;
-	header.stamp = collisionSpaceMonitor_->lastMechanismStateUpdate();
+	header.stamp = collisionSpaceMonitor_->lastJointStateUpdate();
 	header.frame_id = link->name;
 	for (unsigned int i = 0 ; i < link->attachedBodies.size() ; ++i)
         {


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.