void PositionCommand::getGoal(const geometry_msgs::PoseStamped::ConstPtr & Goal)
{
ROS_INFO("GOT NEW GOAL");
goal_received_ = true;
convertPose(Goal,goal_pose_);
std::cout << "goal_pose: " << goal_pose_.transpose() << std::endl;
}
void PositionCommand::getPose(const geometry_msgs::PoseStamped::ConstPtr & Pose)
{
previous_pose_ = current_pose_;
convertPose ( Pose, current_pose_);
ros::Time current_time = ros::Time::now();
duration_ = current_time - previous_time_;
if (duration_.toSec() < 0.1)
pose_received_ = true;
return;
}
/**
* addCamera
*/
int ArticulatedObject::addCamera(const std::vector<Eigen::VectorXd> &_part_parameter)
{
if (_part_parameter.size()==0)
return -1;
Eigen::Matrix4f _pose;
convertPose(_part_parameter[0],_pose);
part_parameter.push_back(_part_parameter);
cameras.push_back(_pose);
return part_parameter.size()-1;
}
