/***********************************************************************
 *  Method: RobotController::Execute
 *  Params:
 * Returns: void
 * Effects: Run the controller, specifically running anything that should
 * 	happen when in a state
 ***********************************************************************/
void RobotController::Execute()
{
    ros::spinOnce();
    // ROS_INFO_STREAM_THROTTLE(3, m_status.ToString());

    // 1) Check to see if robot has reached goal & transition if needed
    // 2) Perform any state related actions
    StateExecute();

    UpdatePose();
    m_statusPub.publish(m_status.GetMessage());

    //Don't need anymore since we're using services
    // if (ros::Time::now() - m_lastStatusUpdate > ros::Duration(2))
    // {
    //     ROS_WARN_STREAM_THROTTLE(10, "Robot has not been in communication for "<<(ros::Time::now() - m_lastStatusUpdate) << " seconds");
    //     m_lastStatusUpdate = ros::Time::now();
    // }

    // Constant update every 200ms ~ 5 Hz
    if (ros::Time::now() - m_lastConstantStatusUpdate > ros::Duration(0.2))
    {
        UpdatePose();
        m_statusPub.publish(m_status.GetMessage());
        m_lastConstantStatusUpdate = ros::Time::now();
    }
}
/***********************************************************************
 *  Method: RobotController::SendRobotStatus
 *  Params:
 * Returns: void
 * Effects: Sends the current status over ROS to any listening
 *  nodes
 ***********************************************************************/
bool RobotController::SendRobotStatus(global_planner::RobotStatusSrv::Request  &req,
                                      global_planner::RobotStatusSrv::Response &res)
{
    UpdatePose();
    res.status = m_status.GetMessage();
    m_lastStatusUpdate = ros::Time::now();
    return true;
}
예제 #3
0
bool SScanLine::add(SSensorData const& data) {
    if(m_vecscan.empty()) {
        m_vecscan.emplace_back(UpdatePose(rbt::pose<double>::zero(), data), data.m_nAngle, data.m_nDistance);
        return true;
    } else {
        auto CompareAngle = [](int lhs, int rhs) {
            if(lhs<rhs) return -1;
            if(rhs<lhs) return 1;
            return 0;
        };
        auto const nCompare = CompareAngle(m_vecscan.front().m_nAngle, m_vecscan.back().m_nAngle);
        auto const nCompareOther = CompareAngle(m_vecscan.back().m_nAngle, data.m_nAngle);
        if(nCompare==0 || nCompareOther==0 || nCompare==nCompareOther) { 
            // Lidar didn't change direction, add sensor to scanline
            m_vecscan.emplace_back(UpdatePose(m_vecscan.back().m_pose, data), data.m_nAngle, data.m_nDistance);
            return true;
        } else {
            return false; // start new scanline
        }
    }
}
/***********************************************************************
 *  Method: RobotController::SetupCallbacks
 *  Params:
 * Returns: void
 * Effects:	Sets up the subscribers, publishers, and services for the bot
 ***********************************************************************/
void RobotController::SetupCallbacks()
{
    m_eStopSub = m_nh->subscribe("/e_stop", 10, &RobotController::cb_eStopSub, this);

    m_odomSub = m_nh->subscribe("odom", 10, &RobotController::cb_odomSub, this);

    m_statusPub = m_nh->advertise<global_planner::RobotStatus>("/robot_status", 100);

    ROS_INFO_STREAM("Connecting to service: "<<Conversion::RobotIDToWaypointFinishedTopic(m_status.GetID()));
    // ROS_INFO_STREAM("Waiting for wp service to come up...");
    // ros::service::waitForService( Conversion::RobotIDToWaypointFinishedTopic(m_status.GetID()) );
    //Task Finished services
    m_waypointFinishedPub = m_nh->serviceClient<global_planner::WaypointFinished>(Conversion::RobotIDToWaypointFinishedTopic(m_status.GetID()), false);
    if (m_waypointFinishedPub.isValid())
    {
        ROS_INFO_STREAM("Successfully connected to wp finished service");
    }
    else
    {
        ROS_ERROR_STREAM("Could not connect to wp finished service.");
    }

    ROS_INFO_STREAM("Connecting to service: "<<Conversion::RobotIDToDumpFinishedTopic(m_status.GetID()));
    // ROS_INFO_STREAM("Waiting for dump service to come up...");
    // ros::service::waitForService( Conversion::RobotIDToDumpFinishedTopic(m_status.GetID()) );
    m_dumpFinishedPub = m_nh->serviceClient<global_planner::DumpFinished>(Conversion::RobotIDToDumpFinishedTopic(m_status.GetID()), true);
    if (m_dumpFinishedPub.isValid())
    {
        ROS_INFO_STREAM("Successfully connected to dump finished service");
    }
    else
    {
        ROS_ERROR_STREAM("Could not connect to dump finished service.");
    }

    m_statusService = m_nh->advertiseService(Conversion::RobotIDToServiceName(m_status.GetID()), &RobotController::SendRobotStatus, this);
    m_waypointService = m_nh->advertiseService(Conversion::RobotIDToWaypointTopic(m_status.GetID()), &RobotController::cb_waypointSub, this);
    m_dumpService = m_nh->advertiseService(Conversion::RobotIDToDumpTopic(m_status.GetID()), &RobotController::cb_dumpSub, this);
    m_setStatusService = m_nh->advertiseService(Conversion::RobotIDToSetStatusTopic(m_status.GetID()), &RobotController::cb_SetRobotStatus, this);
    m_setTrashService = m_nh->advertiseService(Conversion::RobotIDToSetTrash(m_status.GetID()), &RobotController::cb_SetTrash, this);

    //Let the global planner know that this robot is alive an active
    UpdatePose();
    m_statusPub.publish(m_status.GetMessage());

    // Publishers to send human interface output
    m_soundPub = m_nh->advertise<std_msgs::String>("/interface_sound", 100);
    m_textPub = m_nh->advertise<std_msgs::String>("/interface_text", 100);

    ROS_INFO_STREAM("Finished setting up topics");
}
예제 #5
0
//-------------------------------------------
// とりあえずIK
void BoneModel::VMDIkAnimation()
{

	//XMStoreFloat4()
	//XMLoadFloat4()
	if (mBone.empty())return;
	if (mMotion.empty())return;

	DWORD mBoneNum = mBone.size();
	DWORD mIkNum = mIk.size();
	// IK計算
	for (DWORD i = 0; i < mIkNum; i++){
		//{
		//	int i = 0;
		Ik& ik = mIk[i];
		UINT tg_idx = ik.target_bone_index;
		UINT ik_idx = ik.bone_index;

		for (UINT ite = 0; ite<ik.iterations; ++ite){
			for (UINT chn = 0; chn<ik.chain_length; ++chn){
				UINT link_idx = ik.child_bone_index[chn];//
				if (link_idx >= mBoneNum)continue;
				Bone& link_bone = mBone[link_idx];

				//UINT link_pidx = link_bone.mIkBoneIdx;
				UINT link_pidx = link_bone.mHierarchy.mIdxParent;

				//if (link_bone.mIkBoneIdx != 0){
				//	continue;
				//}

				if (link_pidx >= mBoneNum)continue;
				Bone& link_parent = mBone[link_pidx];

				Bone& tg_bone = mBone[tg_idx];
				(void)tg_bone;
				Bone& ik_bone = mBone[ik_idx];
				(void)ik_bone;

				XMVECTOR target_wpos = mBone[tg_idx].mMtxPose.r[3];
				XMVECTOR ik_wpos = mBone[ik_idx].mMtxPose.r[3];
				XMVECTOR lp_wpos = link_parent.mMtxPose.r[3];

				//Linkボーンのローカル空間に変換
				XMVECTOR Determinant;
				XMMATRIX inv_mtx = XMMatrixInverse(&Determinant, link_bone.mMtxPose);
				XMVECTOR tg_pos = XMVector4Transform(target_wpos, inv_mtx);
				XMVECTOR ik_pos = XMVector4Transform(ik_wpos, inv_mtx);
				XMVECTOR lp_pos = XMVector4Transform(lp_wpos, inv_mtx);


				// 回転軸と角度 
				XMVECTOR rot_axis = XMVectorSet(1, 0, 0, 0);
				float ang = 0.0f;
				bool same_dir = false;
				if (!RotDir(tg_pos, ik_pos, ik.control_weight, &rot_axis, &ang)){
					same_dir = true;
				}

				if (!same_dir){

					//tg_dirをik_dirに一致させるための回転
					XMVECTOR rot = XMQuaternionRotationAxis(rot_axis, ang);

					XMVECTOR lrot = FloatToVector(link_bone.mRot);
					XMVECTOR bone_rot_before = lrot;
					link_bone.mRot = VectorToFloat(XMQuaternionMultiply(rot, lrot));

					float dist_tg = XMVectorGetX(XMVector3Length(tg_pos));
					float dist_ik = XMVectorGetX(XMVector3Length(ik_pos));
					(void)dist_ik;
					float dist_lp = XMVectorGetX(XMVector3Length(lp_pos));
					(void)dist_lp;
					float dist_pltg = XMVectorGetX(XMVector3Length(lp_pos - tg_pos));
					float dist_plik = XMVectorGetX(XMVector3Length(lp_pos - ik_pos));
					float dot_tgik = XMVectorGetX(XMVector3Dot(XMVector3Normalize(tg_pos), XMVector3Normalize(ik_pos)));
					(void)dot_tgik;

					// 回転制限
					if (/*link.bLimit*/ 1){
						XMVECTOR rotmax, rotmin;
						//114.5916 = 2
						float a = 2;// XM_PI / 180.0f * 57.25f;
						rotmax = XMVectorSet(a, a, a, 0);//link.vMax;
						rotmin = XMVectorSet(-a, -a, -a, 0);//link.vMin;

						//名前に"ひざ"があったら回転制限
						if (std::string::npos != link_bone.mStrName.find("ひざ")){
							rotmax = XMVectorSet(-XM_PI / 180.0f*0.5f, 0, 0, 0);
							rotmin = XMVectorSet(-XM_PI, 0, 0, 0);
						}
						struct IkLink{
							XMFLOAT4 mMax;
							XMFLOAT4 mMin;
						};
						IkLink link = { VectorToFloat(rotmax), VectorToFloat(rotmin) };
						//Bone& link = link_bone;
						link_bone.mRot = VectorToFloat(LimitAngle(FloatToVector(link_bone.mRot), rotmin, rotmax));

						XMVECTOR angxyz = GetAngle(rot);
						//膝を曲げるための仮処理 かなりてきとう
						if (XMVectorGetX(angxyz) >= 0 &&
							//0.9f < dot_tgik &&
							//dist_tg > dist_ik &&
							dist_pltg > dist_plik &&
							link.mMax.x < 0 && link.mMax.y == link.mMin.y && link.mMax.z == link.mMin.z){
							//親リンクの回転接平面(できるだけこの平面に近づけたほうがよりIK目標に近づける)
							XMVECTOR lp_nor = XMVector3Normalize(-lp_pos);//平面の法線
							//lp_norとの内積が0になる位置を目標にする
							//2つあるので回転制限後の|内積|が小さいほう
							XMVECTOR tng = XMVector3Cross(XMVectorSet(1, 0, 0, 0), lp_nor);
							//+tngと-tngの2つ
							XMVECTOR rot_axis0, rot_axis1;
							float ang0 = 0, ang1 = 0;

							// 回転軸をXに限定
							rot_axis1 = rot_axis0 = XMVectorSet(1, 0, 0, 0);
							XMVECTOR tdir = XMVector3Normalize(XMVectorSetX(tg_pos, 0));
							tng = XMVector3Normalize(XMVectorSetX(tng, 0));
							RotDir(tdir, tng, ik.control_weight, &rot_axis0, &ang0);
							RotDir(tdir, -tng, ik.control_weight, &rot_axis1, &ang1);
							if (XMVectorGetX(rot_axis0) < 0.0f)ang0 = -ang0;
							if (XMVectorGetX(rot_axis1) < 0.0f)ang1 = -ang1;

							//これは絶対違う ぴくぴく対策
							float coef = (dist_pltg - dist_plik) / dist_tg;
							if (coef > 1)coef = 1;
							ang0 *= coef;
							ang1 *= coef;


							//ang0,1は現在の位置からの相対角度 
							// 回転制限を考慮した相対角度に
							float angx_b = XMVectorGetX(GetAngle(bone_rot_before));
							float angx_a0 = angx_b + ang0;
							float angx_a1 = angx_b + ang1;
							if (angx_a0 < link.mMin.x) angx_a0 = link.mMin.x;
							if (angx_a0 > link.mMax.x) angx_a0 = link.mMax.x;
							if (angx_a1 < link.mMin.x) angx_a1 = link.mMin.x;
							if (angx_a1 > link.mMax.x) angx_a1 = link.mMax.x;
							ang0 = angx_a0 - angx_b;
							ang1 = angx_a1 - angx_b;


							XMVECTOR rot0 = XMQuaternionRotationRollPitchYaw(ang0, 0, 0);
							XMVECTOR rot1 = XMQuaternionRotationRollPitchYaw(ang1, 0, 0);

							XMVECTOR tdir0 = XMVector3TransformCoord(tdir, XMMatrixRotationQuaternion(rot0));
							XMVECTOR tdir1 = XMVector3TransformCoord(tdir, XMMatrixRotationQuaternion(rot1));
							float d0 = XMVectorGetX(XMVectorAbs(XMVector3Dot(tdir0, lp_nor)));
							float d1 = XMVectorGetX(XMVectorAbs(XMVector3Dot(tdir1, lp_nor)));
							if (d0 < d1){
								link_bone.mRot = VectorToFloat(XMQuaternionMultiply(rot0, bone_rot_before));
							}
							else{
								link_bone.mRot = VectorToFloat(XMQuaternionMultiply(rot1, bone_rot_before));
							}
						}
					}

				}




				//ワールド行列更新
				link_bone.mMtxPose = SQTMatrix(FloatToVector(link_bone.mScale), FloatToVector(link_bone.mRot), FloatToVector(link_bone.mPos));
				if (link_bone.mHierarchy.mIdxParent < mBoneNum){
					link_bone.mMtxPose = XMMatrixMultiply(link_bone.mMtxPose, mBone[link_bone.mHierarchy.mIdxParent].mMtxPose);
				}

				// 子階層のリンク再計算
				for (int lidown = chn - 1; lidown >= 0; --lidown){
					UINT idx = ik.child_bone_index[lidown];
					if (idx >= mBoneNum)continue;
					Bone& linkb = mBone[idx];
					linkb.mMtxPose = SQTMatrix(FloatToVector(linkb.mScale), FloatToVector(linkb.mRot), FloatToVector(linkb.mPos));
					if (linkb.mHierarchy.mIdxParent < mBoneNum){
						linkb.mMtxPose = XMMatrixMultiply(linkb.mMtxPose, mBone[linkb.mHierarchy.mIdxParent].mMtxPose);
					}
				}

				mBone[tg_idx].mMtxPose = SQTMatrix(FloatToVector(mBone[tg_idx].mScale), FloatToVector(mBone[tg_idx].mRot), FloatToVector(mBone[tg_idx].mPos));
				if (mBone[tg_idx].mHierarchy.mIdxParent < mBoneNum){
					mBone[tg_idx].mMtxPose = XMMatrixMultiply(mBone[tg_idx].mMtxPose, mBone[mBone[tg_idx].mHierarchy.mIdxParent].mMtxPose);
				}
			}
		}


		//Bone& b = mBone[tg_idx];
		//Bone& b2 = mBone[mBone[tg_idx].mHierarchy.mIdxParent];
		//Bone& b3 = mBone[b2.mHierarchy.mIdxParent];
		//int sa = 1;

		//IKの計算結果を子階層に反映
		//UpdatePose();
	}
	UpdatePose();
}
예제 #6
0
// とりあえずアニメーション
void BoneModel::VMDAnimation(float key_time)
{
	if (!mBoneAssetDataPtr)return;
	if (mMotion.empty())return;

	auto& bones = mBoneAssetDataPtr->GetFileData().GetBoneData().mBoneBuffer;
	if (bones.empty())return;

	for (DWORD mid = 0; mid < bones.size(); mid++){

		Motion& mot = mMotion[mid];

		if (mot.mKeyFrame.size() == 0)continue;
		UINT idx = 0;
		for (auto key : mot.mKeyFrame){
			if (key.mKeyFrame.FrameNo >= key_time){
				break;

			}
			++idx;
		}

		if (idx >= mot.mKeyFrame.size())idx = mot.mKeyFrame.size() - 1;

		UINT prev = idx;
		if (idx>0)prev = idx - 1;

		vmd::VMDKeyFrame& key0 = mot.mKeyFrame[prev].mKeyFrame;
		vmd::VMDKeyFrame& key1 = mot.mKeyFrame[idx].mKeyFrame;

		float t = 1.0f;
		if (idx != prev){
			t = float(key_time - key0.FrameNo) / float(key1.FrameNo - key0.FrameNo);
		}
		if (t < 0.0f)t = 0.0f;
		if (t > 1.0f)t = 1.0f;


		{
			XMVECTOR pos0 = XMVectorSet(key0.Location[0], key0.Location[1], key0.Location[2], 0.0f);
			XMVECTOR pos1 = XMVectorSet(key1.Location[0], key1.Location[1], key1.Location[2], 0.0f);
			XMVECTOR rot0 = XMVectorSet(key0.Rotatation[0], key0.Rotatation[1], key0.Rotatation[2], key0.Rotatation[3]);
			XMVECTOR rot1 = XMVectorSet(key1.Rotatation[0], key1.Rotatation[1], key1.Rotatation[2], key1.Rotatation[3]);

			float tx = Bezier(key0.Interpolation[0], key0.Interpolation[8], key0.Interpolation[4], key0.Interpolation[12], t);
			float ty = Bezier(key0.Interpolation[1], key0.Interpolation[9], key0.Interpolation[5], key0.Interpolation[13], t);
			float tz = Bezier(key0.Interpolation[2], key0.Interpolation[10], key0.Interpolation[6], key0.Interpolation[14], t);
			float tr = Bezier(key0.Interpolation[3], key0.Interpolation[11], key0.Interpolation[7], key0.Interpolation[15], t);


			XMVECTOR v = XMVectorLerpV(pos0, pos1, XMVectorSet(tx, ty, tz, 0.0f));
			mBone[mid].mPos = XMFLOAT3(XMVectorGetX(v), XMVectorGetY(v), XMVectorGetZ(v));
			XMVECTOR q = XMQuaternionSlerp(rot0, rot1, tr);
			mBone[mid].mRot = XMFLOAT4(XMVectorGetX(q), XMVectorGetY(q), XMVectorGetZ(q), XMVectorGetW(q));

			////ワールド行列計算
			//XMVECTOR scale = { 1, 1, 1, 1 };
			//mBone[mid].mMtxPose = SRTMatrix(scale, q, v);
			//if (mBone[mid].mHierarchy.mIdxParent < mBoneNum){
			//	mBone[mid].mMtxPose = XMMatrixMultiply(mBone[mid].mMtxPose, mBone[mBone[mid].mHierarchy.mIdxParent].mMtxPose);
			//}
		}
	}
	UpdatePose();
	VMDIkAnimation();
}