From 97e026fcfbbe1fb4790849593539c83d9cbd099e Mon Sep 17 00:00:00 2001 From: "Oscar E. Ramos" <15218582+oscar-ramos@users.noreply.github.com> Date: Fri, 2 Feb 2018 23:50:46 -0500 Subject: [PATCH] Remove extra spaces --- src/markers.cpp | 8 +++--- src/nao_arms_teleop.cpp | 41 ++++++++++++++++-------------- src/tools.cpp | 4 +-- src/windows/kinect_acquisition.cpp | 30 +++++++++++----------- 4 files changed, 43 insertions(+), 40 deletions(-) diff --git a/src/markers.cpp b/src/markers.cpp index 6ecb83e..79b3b28 100644 --- a/src/markers.cpp +++ b/src/markers.cpp @@ -1,6 +1,6 @@ -/* +/* * Copyright 2016 - * J.Avalos, S.Cortez, O.Ramos. + * J.Avalos, S.Cortez, O.Ramos. * Universidad de Ingenieria y Tecnologia - UTEC * * This file is part of nao_kinect_teleop. @@ -23,14 +23,14 @@ Marker::Marker(ros::NodeHandle& nh) { nh.param("reference_frame", reference_frame_, "/map"); - marker_pub_ = + marker_pub_ = nh.advertise("visualization_marker", 10); } unsigned int BallMarker::id_=0; -BallMarker::BallMarker(ros::NodeHandle& nh, +BallMarker::BallMarker(ros::NodeHandle& nh, double color[3], const double& scale) : Marker(nh) diff --git a/src/nao_arms_teleop.cpp b/src/nao_arms_teleop.cpp index 02ab7c4..ccf8492 100644 --- a/src/nao_arms_teleop.cpp +++ b/src/nao_arms_teleop.cpp @@ -1,6 +1,6 @@ -/* +/* * Copyright 2016 - * J.Avalos, S.Cortez, O.Ramos. + * J.Avalos, S.Cortez, O.Ramos. * Universidad de Ingenieria y Tecnologia - UTEC * * This file is part of nao_kinect_teleop. @@ -22,19 +22,22 @@ ----------------------------------------- */ +#include #include #include #include #include + #include #include #include #include #include + #include #include #include -#include + using namespace osik; @@ -115,7 +118,7 @@ int main(int argc, char **argv) { if (niter++ == max_iter) { - std::cerr << "Initial sensed joint configuration does not have " + std::cerr << "Initial sensed joint configuration does not have " << ndof_red << "degrees of freedom, stopping ..." << std::endl; exit(0); } @@ -138,7 +141,7 @@ int main(int argc, char **argv) // Initialize names of command joints for (unsigned int i=0; isetGain(300.0); @@ -168,7 +171,7 @@ int main(int argc, char **argv) Eigen::VectorXd P_right_elbow; Eigen::VectorXd P_left_wrist; Eigen::VectorXd P_left_elbow; - + solver.pushTask(taskrh); solver.pushTask(tasklh); solver.pushTask(taskre); @@ -188,10 +191,10 @@ int main(int argc, char **argv) //Datos del Nao double L1 = 0.108; //Del hombro al codo double L2 = 0.111; //Del codo a la mano - + //Recepcion del brazo derecho for (unsigned k=0;k<(P.size()/2);k++) - { + { P[k].resize(3); //A partir de eso P[0][k]=(0,0,0) P[k][0] = (-kpoints.getPoints()->body[k].z)-(-kpoints.getPoints()->body[0].z); @@ -228,12 +231,12 @@ int main(int argc, char **argv) // Recepcion del brazo izquierdo for (unsigned k=(P.size()/2);kbody[k].z)-(-kpoints.getPoints()->body[3].z); P[k][1] = (-kpoints.getPoints()->body[k].x)-(-kpoints.getPoints()->body[3].x); P[k][2] = (kpoints.getPoints()->body[k].y)-(kpoints.getPoints()->body[3].y); - } + } //Hallamos el modulo M1 de P4 double M3 = sqrt(pow(P[4][0], 2.0) + pow(P[4][1], 2.0) + pow(P[4][2], 2.0)); //Hallamos el modulo M2 de (P2-P1) @@ -263,32 +266,32 @@ int main(int argc, char **argv) P_left_wrist.resize(3); P_right_elbow.resize(3); P_left_elbow.resize(3); - + //Elbow Izquierdo P_left_elbow[0] = P[1][0]; P_left_elbow[1] = P[1][1]; P_left_elbow[2] = P[1][2]; - + //Left hand P_left_wrist[0] = P[2][0]; P_left_wrist[1] = P[2][1]; P_left_wrist[2] = P[2][2]; - + //Right elbow P_right_elbow[0] = P[4][0]; P_right_elbow[1] = P[4][1]; P_right_elbow[2] = P[4][2]; - + //Right hand P_right_wrist[0] = P[5][0]; P_right_wrist[1] = P[5][1]; P_right_wrist[2] = P[5][2]; - + taskle->setDesiredValue(P_left_elbow); tasklh->setDesiredValue(P_left_wrist); taskre->setDesiredValue(P_right_elbow); taskrh->setDesiredValue(P_right_wrist); - + solver.getPositionControl(qsensed, qdes); jcmd.header.stamp = ros::Time::now(); reducedJointModel(jcmd.joint_angles, qdes); @@ -304,12 +307,12 @@ int main(int argc, char **argv) jcmd.joint_angles[25]=0.0; } pub.publish(jcmd); - + qsensed = qdes; } ros::spinOnce(); rate.sleep(); - + } //#######################################################3 return 0; diff --git a/src/tools.cpp b/src/tools.cpp index e68de43..2411bb0 100644 --- a/src/tools.cpp +++ b/src/tools.cpp @@ -1,6 +1,6 @@ -/* +/* * Copyright 2016 - * J.Avalos, S.Cortez, O.Ramos. + * J.Avalos, S.Cortez, O.Ramos. * Universidad de Ingenieria y Tecnologia - UTEC * * This file is part of nao_kinect_teleop. diff --git a/src/windows/kinect_acquisition.cpp b/src/windows/kinect_acquisition.cpp index 79c8d36..0e1e6bf 100644 --- a/src/windows/kinect_acquisition.cpp +++ b/src/windows/kinect_acquisition.cpp @@ -1,6 +1,6 @@ -/* +/* * Copyright 2016 - * J.Avalos, S.Cortez + * J.Avalos, S.Cortez * Universidad de Ingenieria y Tecnologia - UTEC * * This file is part of nao_kinect_teleop. @@ -38,16 +38,16 @@ int main() HRESULT hResult = S_OK; hResult = GetDefaultKinectSensor(&pSensor); hResult = pSensor->Open(); - + IBodyFrameSource* pBodySource; hResult = pSensor->get_BodyFrameSource(&pBodySource); - + IBodyFrameReader* pBodyReader; hResult = pBodySource->OpenReader(&pBodyReader); - + ICoordinateMapper* pCoordinateMapper; hResult = pSensor->get_CoordinateMapper(&pCoordinateMapper); - + //Inicializacion del envio de datos (Ubuntu) ros::NodeHandle nh; char *ros_master = "10.100.144.116"; @@ -69,31 +69,31 @@ int main() while (1) { Sleep(100); - + IBodyFrame* pBodyFrame = nullptr; hResult = pBodyReader->AcquireLatestFrame(&pBodyFrame); if (SUCCEEDED(hResult)){ IBody* pBody[BODY_COUNT] = { 0 }; hResult = pBodyFrame->GetAndRefreshBodyData(BODY_COUNT, pBody); - + if (pBodyFrame != nullptr) { for (int count = 0; count < BODY_COUNT; count++) { BOOLEAN bTracked = false; hResult = pBody[count]->get_IsTracked(&bTracked); - + if (SUCCEEDED(hResult) && bTracked) { Joint joint[JointType::JointType_Count]; hResult = pBody[count]->GetJoints(JointType::JointType_Count, joint); - + if (SUCCEEDED(hResult)) { // Estado de mano izquierda HandState leftHandState = HandState::HandState_Unknown; hResult = pBody[count]->get_HandLeftState(&leftHandState); - + if (SUCCEEDED(hResult)){ if (leftHandState == HandState::HandState_Open){ body_msg.left_hand.data = true; @@ -102,11 +102,11 @@ int main() body_msg.left_hand.data = false; } } - + // Estado de mano derecha HandState rightHandState = HandState::HandState_Unknown; hResult = pBody[count]->get_HandRightState(&rightHandState); - + if (SUCCEEDED(hResult)){ if (rightHandState == HandState::HandState_Open){ body_msg.right_hand.data = true; @@ -115,12 +115,12 @@ int main() body_msg.right_hand.data = false; } } - + //Articulaciones for (int i = 0; i < 6; i++){ if (joint[orden[i]].TrackingState != TrackingState::TrackingState_NotTracked){ CameraSpacePoint cameraSpacePoint = joint[orden[i]].Position; - + body_msg.body[i].x = joint[orden[i]].Position.X; body_msg.body[i].y = joint[orden[i]].Position.Y; body_msg.body[i].z = joint[orden[i]].Position.Z;