|
From: <hsu...@us...> - 2008-08-20 17:29:43
|
Revision: 3323
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3323&view=rev
Author: hsujohnhsu
Date: 2008-08-20 17:29:51 +0000 (Wed, 20 Aug 2008)
Log Message:
-----------
base controller testing.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
Modified: pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp 2008-08-20 17:23:02 UTC (rev 3322)
+++ pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp 2008-08-20 17:29:51 UTC (rev 3323)
@@ -84,11 +84,16 @@
robot_desc::URDF::Link *link;
std::string joint_name;
+
+
+
+
// ros::g_node->advertise<std_msgs::RobotBase2DOdom>("odom");
std::string xml_content;
(ros::g_node)->get_param("robotdesc/pr2",xml_content);
- if(!urdf_model_.loadString(xml_content.c_str()))
- return;
+ assert(urdf_model_.loadString(xml_content.c_str()));
+ //if(!urdf_model_.loadString(xml_content.c_str()))
+ // return;
for(jcp_iter = jcp.begin(); jcp_iter != jcp.end(); jcp_iter++)
{
@@ -108,12 +113,17 @@
{
base_object.local_id_ = num_casters_;
base_casters_.push_back(base_object);
+ steer_angle_actual_.push_back(0);
+ steer_velocity_desired_.push_back(0);
num_casters_++;
}
if(joint_name.find("wheel") != string::npos)
{
base_object.local_id_ = num_wheels_;
base_wheels_.push_back(base_object);
+ wheel_speed_actual_.push_back(0);
+ libTF::Pose3D::Vector *v=new libTF::Pose3D::Vector();
+ base_wheels_position_.push_back(*v);
num_wheels_++;
}
}
@@ -131,13 +141,16 @@
}
}
}
+ std::cout << " assigning robot_ " << std::endl;
robot_ = robot;
}
void BaseController::initXml(mechanism::Robot *robot, TiXmlElement *config)
{
+ std::cout << " base controller initxml " << std::endl << *config << std::endl;
TiXmlElement *elt = config->FirstChildElement("controller");
+ std::cout << " child " << std::endl << *elt << std::endl;
std::vector<JointControlParam> jcp_vec;
JointControlParam jcp;
while (elt){
@@ -152,8 +165,15 @@
jcp.joint_name = jnt->Attribute("name");
jcp_vec.push_back(jcp);
- elt = config->NextSiblingElement("controller");
+ std::cout << "name:" << jcp.joint_name << std::endl;
+ std::cout << "controller type:" << jcp.control_type << std::endl;
+ std::cout << std::endl << *elt << std::endl;
+ std::cout << " sub controller : " << elt->Attribute("name") << std::endl;
+
+ elt = elt->NextSiblingElement("controller");
+
}
+
elt = config->FirstChildElement("map");
while(elt)
{
@@ -171,6 +191,7 @@
}
}
elt = config->NextSiblingElement("map");
+ std::cout << " sub map : " << elt->Attribute("name") << std::endl;
}
init(jcp_vec,robot);
}
@@ -182,6 +203,11 @@
for(int i=0; i < num_wheels_; i++)
wheel_speed_actual_[i] = base_wheels_[i].controller_.getMeasuredVelocity();
+
+ for(int i=0; i < num_casters_; i++)
+ std::cout << " caster angles " << i << " : " << steer_angle_actual_[i] << std::endl;
+ for(int i=0; i < num_wheels_; i++)
+ std::cout << " wheel rates " << i << " : " << wheel_speed_actual_[i] << std::endl;
}
void BaseController::computeWheelPositions()
@@ -222,7 +248,7 @@
if(odom_publish_counter_ > odom_publish_count_)
{
- (ros::g_node)->publish("odom", odom_msg_);
+ //(ros::g_node)->publish("odom", odom_msg_);
odom_publish_counter_ = 0;
}
@@ -246,12 +272,14 @@
{
libTF::Pose3D::Vector result;
double steer_angle_desired;
+ kp_speed_ = 1.0;
for(int i=0; i < num_casters_; i++)
{
result = computePointVelocity2D(base_casters_[i].pos_, cmd_vel_);
steer_angle_desired = atan2(result.y,result.x);
steer_angle_desired = ModNPiBy2(steer_angle_desired);//Clean steer Angle
steer_velocity_desired_[i] = kp_speed_*steer_angle_desired;
+ std::cout << "setting steering velocity??? " << i << " : " << steer_velocity_desired_[i] << " kp: " << kp_speed_ << std::endl;
base_casters_[i].controller_.setCommand(steer_velocity_desired_[i]);
}
}
@@ -263,6 +291,9 @@
libTF::Pose3D::Vector wheel_caster_steer_component;
libTF::Pose3D::Vector caster_2d_velocity;
+ //---------------------------------------------------------------------FIXME:
+ wheel_radius_ = 0.079;
+
caster_2d_velocity.x = 0;
caster_2d_velocity.y = 0;
caster_2d_velocity.z = 0;
@@ -278,6 +309,7 @@
// wheel_point_velocity_projected = rotate2D(wheel_point_velocity,-steer_angle_actual);
wheel_point_velocity_projected = wheel_point_velocity.rot2D(-steer_angle_actual);
wheel_speed_cmd = (wheel_point_velocity_projected.x + wheel_caster_steer_component.x)/wheel_radius_;
+ std::cout << "setting wheel speed " << i << " : " << wheel_speed_cmd << " r:" << wheel_radius_ << std::endl;
base_wheels_[i].controller_.setCommand(wheel_speed_cmd);
}
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-08-20 17:23:02 UTC (rev 3322)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-08-20 17:29:51 UTC (rev 3323)
@@ -9,7 +9,7 @@
rospack_add_library(Ros_Node src/Ros_Node.cc)
rospack_add_library(P3D src/P3D.cc)
rospack_add_library(gazebo_actuators src/gazebo_actuators.cpp)
-#rospack_add_library(test_actuators src/test_actuators.cpp)
+rospack_add_library(test_actuators src/test_actuators.cpp)
# This target requires adding player to the manifest.xml.
#rospack_add_executable(player_pr2 src/player/Pr2_Player.cc)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|