void PickPlace::evaluate_move_accuracy()
{
robot_state::RobotState target_state = group_->getJointValueTarget();
target_state.update();
//group_->getcurrentstate does not give the correct values
robot_state::RobotState current_state(target_state);
//geting state from joint state message
std::string topic;
if (robot_connected_)
{
topic = "/" + robot_type_ + "_driver/out/joint_state";
}
else
{
topic = "/" + robot_type_ + "/joint_states"; //for gazebo
}
sensor_msgs::JointStateConstPtr msg = ros::topic::waitForMessage<sensor_msgs::JointState>(topic, ros::Duration(1.0));
for (int i =0;i<joint_names_.size();i++)
{
current_state.setJointPositions(joint_names_[i].c_str(), &(msg->position[i]) );
}
current_state.update();
Eigen::Affine3d transform = current_state.getGlobalLinkTransform (robot_type_ + "_end_effector");
geometry_msgs::Pose feeback_pose;
tf::poseEigenToMsg(transform,feeback_pose);
transform = target_state.getGlobalLinkTransform (robot_type_ + "_end_effector");
geometry_msgs::Pose target_state_pose;
tf::poseEigenToMsg(transform,target_state_pose);
std::string error_out,joints_out,torques_out;
error_out.clear();
joints_out.clear();
torques_out.clear();
o_file_<<std::endl<<std::endl<<"**********************New trajectory***************************"<<std::endl;
//geometry_msgs::PoseStampedConstPtr pose_robot = ros::topic::waitForMessage<geometry_msgs::PoseStamped>("/tf", ros::Duration(1.0));
if (cartesian_move_)
{
//Cartesian error
//geometry_msgs::PoseStamped target_pose = group_->getPoseTarget(robot_type_ + "_end_effector");
//geometry_msgs::PoseStamped current_pose = group_->getCurrentPose (robot_type_ + "_end_effector");
o_file_<<"Target - "<<std::endl<<target_state_pose;//target_pose.pose;
o_file_<<"Feedback Moveit - "<<std::endl<<feeback_pose;
if (robot_connected_)
{
topic = "/" + robot_type_ + "_driver/out/tool_pose";
geometry_msgs::PoseStampedConstPtr pose_robot = ros::topic::waitForMessage<geometry_msgs::PoseStamped>(topic, ros::Duration(1.0));
o_file_<<"Feedback Robot - "<<std::endl<<pose_robot->pose;
}
o_file_<<"Error --- x = "<<fabs(target_state_pose.position.x - feeback_pose.position.x)<<
", y = "<<fabs(target_state_pose.position.y - feeback_pose.position.y)<<
", z = "<<fabs(target_state_pose.position.z - feeback_pose.position.z)<<std::endl;
}
{
for (int i =0;i<joint_names_.size();i++)
{
double target = *target_state.getJointPositions (joint_names_[i])*180/3.1415;
double current = msg->position[i]*180/3.1415;
double torque = msg->effort[i];
double error = fabs(target - current);
error = fmod(error, 360);
if (error>180)
error = fabs(error-360);
joints_out = joints_out + joint_names_[i]+ " Target=" + boost::lexical_cast<std::string>(target) + " Feedback=" + boost::lexical_cast<std::string>(current) + " ";
error_out = error_out + boost::lexical_cast<std::string>(error) + " ";
torques_out = torques_out + boost::lexical_cast<std::string>(torque) + " ";
}
o_file_<<joints_out<<std::endl<<std::endl<<"Joints Error ----"<<error_out<<std::endl<<"Joints Torques -----"<<torques_out<<std::endl;
}
}
1、得到當前狀態
robot_state::RobotState target_state = group_->getJointValueTarget();
target_state.update();
//group_->getcurrentstate does not give the correct values
robot_state::RobotState current_state(target_state);
2、得到當前和目標狀態的末端執行器的位姿信息
sensor_msgs::JointStateConstPtr msg = ros::topic::waitForMessage<sensor_msgs::JointState>(topic, ros::Duration(1.0));
for (int i =0;i<joint_names_.size();i++)
{
current_state.setJointPositions(joint_names_[i].c_str(), &(msg->position[i]) );
}
current_state.update();
Eigen::Affine3d transform = current_state.getGlobalLinkTransform (robot_type_ + "_end_effector");
geometry_msgs::Pose feeback_pose;
tf::poseEigenToMsg(transform,feeback_pose);
transform = target_state.getGlobalLinkTransform (robot_type_ + "_end_effector");
geometry_msgs::Pose target_state_pose;
tf::poseEigenToMsg(transform,target_state_pose);
通過ros::topic::waitForMessage<sensor_msgs::JointState>(topic, ros::Duration(1.0));等待sensor_msgs::JointState類型的message到達Topic，等待時間爲ros::Duration(1.0),返回值爲當前的msg。
通過current_state.setJointPositions(joint_name,msg->position)，設置各個關節的position,同樣還能設置關節的速度，加速度
之後就得到當前與目標狀態的位姿的msg形式
3、輸出目標位姿和當前位姿的信息
if (cartesian_move_)
{
//Cartesian error
//geometry_msgs::PoseStamped target_pose = group_->getPoseTarget(robot_type_ + "_end_effector");
//geometry_msgs::PoseStamped current_pose = group_->getCurrentPose (robot_type_ + "_end_effector");
o_file_<<"Target - "<<std::endl<<target_state_pose;//target_pose.pose;
o_file_<<"Feedback Moveit - "<<std::endl<<feeback_pose;
if (robot_connected_)
{
topic = "/" + robot_type_ + "_driver/out/tool_pose";
geometry_msgs::PoseStampedConstPtr pose_robot = ros::topic::waitForMessage<geometry_msgs::PoseStamped>(topic, ros::Duration(1.0));
o_file_<<"Feedback Robot - "<<std::endl<<pose_robot->pose;
}
o_file_<<"Error --- x = "<<fabs(target_state_pose.position.x - feeback_pose.position.x)<<
", y = "<<fabs(target_state_pose.position.y - feeback_pose.position.y)<<
", z = "<<fabs(target_state_pose.position.z - feeback_pose.position.z)<<std::endl;
}
通過 //geometry_msgs::PoseStamped target_pose = group_->getPoseTarget(robot_type_ + "_end_effector");
//geometry_msgs::PoseStamped current_pose = group_->getCurrentPose (robot_type_ + "_end_effector");
可以分別得到當前的位姿以及設置的目標的位姿信息。
通過訂閱/j2n6s300_driver/out/tool_pose,消息類型爲geometry_msgs::PoseStamped，輸入當文件中，注意，輸出文件只能輸出msg類型。
最後打印各個方向的差的絕對值。
4、打印最終信息
{
for (int i =0;i<joint_names_.size();i++)
{
double target = *target_state.getJointPositions (joint_names_[i])*180/3.1415;
double current = msg->position[i]*180/3.1415;
double torque = msg->effort[i];
double error = fabs(target - current);
error = fmod(error, 360);
if (error>180)
error = fabs(error-360);
joints_out = joints_out + joint_names_[i]+ " Target=" + boost::lexical_cast<std::string>(target) + " Feedback=" + boost::lexical_cast<std::string>(current) + " ";
error_out = error_out + boost::lexical_cast<std::string>(error) + " ";
torques_out = torques_out + boost::lexical_cast<std::string>(torque) + " ";
}
o_file_<<joints_out<<std::endl<<std::endl<<"Joints Error ----"<<error_out<<std::endl<<"Joints Torques -----"<<torques_out<<std::endl;
}
關節是弧度信息，弧度×180/pi換算成角度。