|
From: <tpr...@us...> - 2008-10-10 22:49:05
|
Revision: 5263
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5263&view=rev
Author: tpratkanis
Date: 2008-10-10 22:49:01 +0000 (Fri, 10 Oct 2008)
Log Message:
-----------
Start fixing arm.1 test.
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp
pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp 2008-10-10 22:47:28 UTC (rev 5262)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp 2008-10-10 22:49:01 UTC (rev 5263)
@@ -80,6 +80,7 @@
#include <robot_msgs/DisplayKinematicPath.h>
#include <robot_srvs/NamedKinematicPlanState.h>
#include <robot_srvs/PlanNames.h>
+#include <rosTF/rosTF.h>
static const unsigned int RIGHT_ARM_JOINTS_BASE_INDEX = 11;
static const unsigned int LEFT_ARM_JOINTS_BASE_INDEX = 12;
@@ -119,6 +120,7 @@
mechanism_control::MechanismState mechanismState;
robot_srvs::NamedKinematicPlanState::response plan;
unsigned int currentWaypoint; /*!< The waypoint in the plan that we are targetting */
+ rosTFClient tf_; /**< Used to do transforms */
protected:
std::vector<std::string> jointNames_; /*< The collection of joint names of interest. Instantiate in the derived class.*/
@@ -142,7 +144,7 @@
MoveArm::MoveArm(const std::string& nodeName, const std::string& stateTopic, const std::string& goalTopic,
const std::string& armPosTopic, const std::string& _armCmdTopic, const std::string& _kinematicModel)
: HighlevelController<pr2_msgs::MoveArmState, pr2_msgs::MoveArmGoal>(nodeName, stateTopic, goalTopic),
- armCmdTopic(_armCmdTopic), kinematicModel(_kinematicModel), currentWaypoint(0){
+ armCmdTopic(_armCmdTopic), kinematicModel(_kinematicModel), currentWaypoint(0), tf_(*this, true, 10000000000ULL) {
// Advertise for messages to command the arm
@@ -213,6 +215,30 @@
req.times = 1;
+
+ //Get the pose of the robot:
+ libTF::TFPose2D robotPose, globalPose;
+ robotPose.x = 0;
+ robotPose.y = 0;
+ robotPose.yaw = 0;
+ robotPose.frame = "base";
+ robotPose.time = 0;
+ try{
+ globalPose = this->tf_.transformPose2D("map", robotPose);
+ }
+ catch(libTF::TransformReference::LookupException& ex){
+ std::cout << "No Transform available Error\n";
+ }
+ catch(libTF::TransformReference::ConnectivityException& ex){
+ std::cout << "Connectivity Error\n";
+ }
+ catch(libTF::TransformReference::ExtrapolateException& ex){
+ std::cout << "Extrapolation Error\n";
+ }
+
+
+
+
//initializing full value state
req.start_state.set_names_size(names.get_names_size());
req.start_state.set_joints_size(names.get_names_size());
@@ -220,8 +246,15 @@
req.start_state.names[i] = names.names[i];
//std::cout << req.start_state.names[i] << ": " << names.num_values[i] << std::endl;
req.start_state.joints[i].set_vals_size(names.num_values[i]);
- for (int k = 0 ; k < names.num_values[i]; k++) {
- req.start_state.joints[i].vals[k] = 0; //FIXME: should be the value from mechanism state.
+ if (names.names[i] == "base_joint") {
+ std::cout << "Base: " << i << ", " << globalPose.x << ", " << globalPose.y << ", " << globalPose.yaw << std::endl;
+ req.start_state.joints[i].vals[0] = globalPose.x;
+ req.start_state.joints[i].vals[1] = globalPose.y;
+ req.start_state.joints[i].vals[2] = globalPose.yaw;
+ } else {
+ for (int k = 0 ; k < names.num_values[i]; k++) {
+ req.start_state.joints[i].vals[k] = 0; //FIXME: should be the value from mechanism state.
+ }
}
}
Modified: pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h 2008-10-10 22:47:28 UTC (rev 5262)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h 2008-10-10 22:49:01 UTC (rev 5263)
@@ -251,7 +251,14 @@
for (unsigned int i = 0 ; i < n ; ++i)
{
double pos = m_mechanismState.joint_states[i].position;
- m_robotState->setParams(&pos, m_mechanismState.joint_states[i].name);
+ //printf("%d: %s\n", i, m_mechanismState.joint_states[i].name.c_str());
+ if (m_mechanismState.joint_states[i].name == "base_joint" && m_haveBasePos) {
+ m_robotState->setParams(m_basePos, m_mechanismState.joint_states[i].name);
+ }
+ else
+ {
+ m_robotState->setParams(&pos, m_mechanismState.joint_states[i].name);
+ }
}
}
if (m_robotStateSimple)
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-10-10 22:47:28 UTC (rev 5262)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-10-10 22:49:01 UTC (rev 5263)
@@ -188,6 +188,7 @@
void stateUpdate(void)
{
+ //m_robotState->print();
planning_node_util::NodeRobotModel::stateUpdate();
if (m_kmodel)
m_kmodel->computeTransforms(m_robotState->getParams());
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|