void simulateOneCycle()
{
tf::StampedTransform transform;
try{
listener.waitForTransform("map","EEframe", ros::Time(0), ros::Duration(1.0));
listener.lookupTransform( "map","EEframe", ros::Time(0), transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
// This is correct, but does not work.
// state_ = transform;
KDL::Frame state_copy;
tf::transformTFToKDL(state_, state_copy);
nh_.getParamCached("/cds_controller_pr2/run_controller", run_controller_flag);
if (run_controller_flag == 1.0){ // Controller running -> Update next desired position
tf::transformKDLToTF(cds_.update(state_copy), state_);
} else {
state_ = transform;
}
broadcastTF();
}
/** Update reconfigurable parameter (for strings). */
bool inline updateParam(const std::string &name, std::string &val)
{
bool changed = false;
std::string prev = val;
if (nh_.getParamCached(name, val) && (val != prev))
changed = true;
return changed;
}
inline std::string getStringParam(std::string name)
{
std::string value;
if (!root_nh_.getParamCached(name, value))
{
ROS_ERROR("Hand description: could not find parameter %s", name.c_str());
}
//ROS_INFO_STREAM("Hand description param " << name << " resolved to " << value);
return value;
}
inline std::vector<std::string> getVectorParam(std::string name)
{
std::vector<std::string> values;
XmlRpc::XmlRpcValue list;
if (!root_nh_.getParamCached(name, list))
{
ROS_ERROR("Hand description: could not find parameter %s", name.c_str());
}
if (list.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR("Hand description: bad parameter %s", name.c_str());
}
//ROS_INFO_STREAM("Hand description vector param " << name << " resolved to:");
for (int32_t i=0; i<list.size(); i++)
{
if (list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
{
ROS_ERROR("Hand description: bad parameter %s", name.c_str());
}
values.push_back( static_cast<std::string>(list[i]) );
//ROS_INFO_STREAM(" " << values.back());
}
return values;
}
void executeActionCB(const OrientToBaseGoalConstPtr& goal) {
BaseScanLinearRegression srv;
double min_angle = -M_PI/8.0, max_angle=M_PI/8.0, min_dist=0.02, max_dist=0.8;
nh_.getParamCached("/laser_linear_regression/min_angle", min_angle);
nh_.getParamCached("/laser_linear_regression/max_angle", max_angle);
nh_.getParamCached("/laser_linear_regression/min_dist", min_dist);
nh_.getParamCached("/laser_linear_regression/max_dist", max_dist);
srv.request.filter_minAngle = angles::from_degrees(min_angle);
srv.request.filter_maxAngle = angles::from_degrees(max_angle);
srv.request.filter_minDistance = min_dist;
srv.request.filter_maxDistance = max_dist;
std::cout << "min_dist " << min_dist << ", max_dist " << max_dist << std::endl;
//srv.request.filter_minAngle = -M_PI / 8.0;
//srv.request.filter_maxAngle = M_PI / 8.0;
//srv.request.filter_minDistance = 0.02;
//srv.request.filter_maxDistance = 0.80;
target_distance = goal->distance;
ros::Duration max_time(50.0);
ros::Time stamp = ros::Time::now();
OrientToBaseResult result;
bool oriented = false;
int iterator = 0;
while (ros::ok()) {
ROS_INFO("Call service Client");
if(client.call(srv)) {
ROS_INFO("Called service LaserScanLinearRegressionService");
//std::cout << "result: " << srv.response.center << ", " << srv.response.a << ", " << srv.response.b << std::endl;
geometry_msgs::Twist cmd = calculateVelocityCommand(srv.response.center, srv.response.a, srv.response.b,oriented,iterator);
cmd_pub.publish(cmd);
std::cout << "cmd x:" << cmd.linear.x << ", y: " << cmd.linear.y << ", z: " << cmd.angular.z << std::endl;
if ((fabs(cmd.angular.z) + fabs(cmd.linear.x) ) < 0.001) {
ROS_INFO("Point reached");
result.succeed = true;
as_.setSucceeded(result);
geometry_msgs::Twist zero_vel;
cmd_pub.publish(zero_vel);
break;
}
if (stamp + max_time < ros::Time::now()) {
result.succeed = false;
as_.setAborted(result);
geometry_msgs::Twist zero_vel;
cmd_pub.publish(zero_vel);
break;
}
} else {
ROS_ERROR("Failed to call service LaserScanLinearRegressionService");
if (stamp + max_time < ros::Time::now()) {
result.succeed = false;
as_.setAborted(result);
break;
}
}
}
}