From b0c72023746cf8916f4c4aba6363c3b61e33ae7b Mon Sep 17 00:00:00 2001 From: stepelle Date: Thu, 5 Nov 2015 10:26:27 -0500 Subject: [PATCH 1/9] changes in src/startBody. The socket was not providing updated data --- CMakeLists.txt | 43 ++++++------ include/k2_client/k2_client.h | 123 ++++++++++++++++------------------ launch/k2_client.launch | 2 +- package.xml | 47 ++++++------- src/startAudio.cpp | 119 ++++++++++++++++---------------- src/startBody.cpp | 60 ++++++++--------- src/startDepth.cpp | 108 +++++++++++++++-------------- src/startIR.cpp | 110 +++++++++++++++--------------- src/startRGB.cpp | 123 +++++++++++++++++----------------- 9 files changed, 364 insertions(+), 371 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 94a50e4..46765e5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,6 @@ set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake/Modules") # RelWithDebInfo : w/ debug symbols, w/ optimization # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries - find_package(Jsoncpp REQUIRED) find_package(catkin REQUIRED COMPONENTS rospy @@ -34,8 +33,10 @@ add_message_files(FILES Body.msg ) -#uncomment if you have defined messages/services -generate_messages(DEPENDENCIES geometry_msgs) +generate_messages(DEPENDENCIES + geometry_msgs + std_msgs +) catkin_package( INCLUDE_DIRS include @@ -44,18 +45,19 @@ catkin_package( CATKIN_DEPENDS geometry_msgs std_msgs + message_runtime DEPENDS Jsoncpp ) -#set the default path for built executables to the "bin" directory -set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -#set the default path for built libraries to the "lib" directory -set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) +# Set the default path for built executables to the "bin" directory. +set(EXECUTABLE_OUTPUT_PATH "${PROJECT_SOURCE_DIR}/bin") +# Set the default path for built libraries to the "lib" directory. +set(LIBRARY_OUTPUT_PATH "${PROJECT_SOURCE_DIR}/lib") include_directories(SYSTEM + "${PROJECT_SOURCE_DIR}/include" ${catkin_INCLUDE_DIRS} - ${PROJECT_SOURCE_DIR}/include/k2_client ${JSONCPP_INCLUDE_DIR} ) @@ -63,17 +65,20 @@ link_directories( ${catkin_LIBRARY_DIRS} ) -add_executable(startRGB ${PROJECT_SOURCE_DIR}/src/startRGB.cpp) -add_executable(startIR ${PROJECT_SOURCE_DIR}/src/startIR.cpp) -add_executable(startDepth ${PROJECT_SOURCE_DIR}/src/startDepth.cpp) -add_executable(startAudio ${PROJECT_SOURCE_DIR}/src/startAudio.cpp) -add_executable(startBody ${PROJECT_SOURCE_DIR}/src/startBody.cpp) +add_executable(startAudio "${PROJECT_SOURCE_DIR}/src/startAudio.cpp") +add_executable(startBody "${PROJECT_SOURCE_DIR}/src/startBody.cpp") +add_executable(startDepth "${PROJECT_SOURCE_DIR}/src/startDepth.cpp") +add_executable(startIR "${PROJECT_SOURCE_DIR}/src/startIR.cpp") +add_executable(startRGB "${PROJECT_SOURCE_DIR}/src/startRGB.cpp") + +add_dependencies(startAudio ${k2_client_EXPORTED_TARGETS}) +add_dependencies(startBody ${k2_client_EXPORTED_TARGETS}) +add_dependencies(startDepth ${k2_client_EXPORTED_TARGETS}) +add_dependencies(startIR ${k2_client_EXPORTED_TARGETS}) +add_dependencies(startRGB ${k2_client_EXPORTED_TARGETS}) target_link_libraries(startAudio ${JSONCPP_LIBRARY} ${catkin_LIBRARIES}) -target_link_libraries(startBody ${JSONCPP_LIBRARY} ${catkin_LIBRARIES}) -target_link_libraries(startRGB ${JSONCPP_LIBRARY} ${catkin_LIBRARIES}) -target_link_libraries(startIR ${JSONCPP_LIBRARY} ${catkin_LIBRARIES}) +target_link_libraries(startBody ${JSONCPP_LIBRARY} ${catkin_LIBRARIES}) target_link_libraries(startDepth ${JSONCPP_LIBRARY} ${catkin_LIBRARIES}) - - - +target_link_libraries(startIR ${JSONCPP_LIBRARY} ${catkin_LIBRARIES}) +target_link_libraries(startRGB ${JSONCPP_LIBRARY} ${catkin_LIBRARIES}) diff --git a/include/k2_client/k2_client.h b/include/k2_client/k2_client.h index 33120ba..2b5c463 100644 --- a/include/k2_client/k2_client.h +++ b/include/k2_client/k2_client.h @@ -1,30 +1,28 @@ /************************************************************************************* -Copyright (c) 2013, Carnegie Mellon University -All rights reserved. -Authors: Anurag Jakhotia, Prasanna Velagapudi - -Redistribution and use in source and binary forms, with or without modification, are -permitted provided that the following conditions are met: - - - Redistributions of source code must retain the above copyright notice, this list - of conditions and the following disclaimer. - - Redistributions in binary form must reproduce the above copyright notice, this - list of conditions and the following disclaimer in the documentation and/or other - materials provided with the distribution. - - Neither the name of Carnegie Mellon University nor the names of its contributors - may be used to endorse or promote products derived from this software without - specific prior written permission. + C opyri*ght (c) 2013, Carnegie Mellon University + All rights reserved. + Authors: Anurag Jakhotia, Prasanna Velagapudi + Redistribution and use in source and binary forms, with or without modification, are + permitted provided that the following conditions are met: + - Redistributions of source code must retain the above copyright notice, this list + of conditions and the following disclaimer. + - Redistributions in binary form must reproduce the above copyright notice, this + list of conditions and the following disclaimer in the documentation and/or other + materials provided with the distribution. + - Neither the name of Carnegie Mellon University nor the names of its contributors + may be used to endorse or promote products derived from this software without + specific prior written permission. -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY -EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES -OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT -SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED -TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR -BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -***************************************************************************************/ + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY + EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT + SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED + TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR + BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY + WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ***************************************************************************************/ #ifndef _startKinect_H_ #define _startKinect_H_ @@ -37,11 +35,10 @@ WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH #include #include - //*******************************ROS specific Headers******************************// #include "ros/ros.h" -//***********************************Other Headers*********************************// +//***********************************Other Headers*********************************// #include "cv_bridge/cv_bridge.h" #include #include @@ -49,56 +46,54 @@ WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH #include "sensor_msgs/Image.h" #include "sensor_msgs/image_encodings.h" #include -#include -#include #include class Socket { public: - struct sockaddr_in inAddress; //IP and port container for easy readability - struct sockaddr socketAddress; //IP and port container for the code - struct addrinfo hints, *response, *iterator;//hint specifies what options to look for - //response is a linked list that getaddrinfo fills for us - //iterator is later on used to go through each of the response - int mSocket; - int mBufferSize; - char *mBuffer; - Socket(const char address[],char portNum[],int bufferSize); - void readData(); + struct sockaddr_in inAddress; //IP and port container for easy readability + struct sockaddr socketAddress; //IP and port container for the code + struct addrinfo hints, *response, *iterator;//hint specifies what options to look for + //response is a linked list that getaddrinfo fills for us + //iterator is later on used to go through each of the response + int mSocket; + int mBufferSize; + char *mBuffer; + Socket(const char address[], const char portNum[], int bufferSize); + void readData(); private: - + }; -Socket::Socket(const char address[],char portNum[],int bufferSize) +Socket::Socket(const char address[], const char portNum[], int bufferSize) { - memset(&(this->hints), 0, sizeof (this->hints)); - this->hints.ai_family = AF_INET; - this->hints.ai_socktype = SOCK_STREAM; - this->mBufferSize = bufferSize; - this->mBuffer = new char[this->mBufferSize]; - getaddrinfo(address,portNum,&(this->hints),&(this->response)); - for(this->iterator = this->response;this->iterator!=NULL;this->iterator = this->iterator->ai_next) - { - if(iterator->ai_family != AF_INET) - { - std::cout<<"Please disable IPv6"<ai_addr); - char ipString[INET_ADDRSTRLEN]; - inet_ntop(iterator->ai_family,&(address->sin_addr),ipString,sizeof ipString); - this->mSocket = socket(this->response->ai_family,this->response->ai_socktype,this->response->ai_protocol); - if(this->mSocket !=-1) - connect(this->mSocket,this->response->ai_addr,this->response->ai_addrlen); - } + memset(&(this->hints), 0, sizeof (this->hints)); + this->hints.ai_family = AF_INET; + this->hints.ai_socktype = SOCK_STREAM; + this->mBufferSize = bufferSize; + this->mBuffer = new char[this->mBufferSize]; + getaddrinfo(address,portNum,&(this->hints),&(this->response)); + for(this->iterator = this->response;this->iterator!=NULL;this->iterator = this->iterator->ai_next) + { + if(iterator->ai_family != AF_INET) + { + std::cout<<"Please disable IPv6"<ai_addr); + char ipString[INET_ADDRSTRLEN]; + inet_ntop(iterator->ai_family,&(address->sin_addr),ipString,sizeof ipString); + this->mSocket = socket(this->response->ai_family,this->response->ai_socktype,this->response->ai_protocol); + if(this->mSocket !=-1) + connect(this->mSocket,this->response->ai_addr,this->response->ai_addrlen); + } } void Socket::readData() { - memset(this->mBuffer,0,this->mBufferSize); - recv(this->mSocket,this->mBuffer,this->mBufferSize,MSG_WAITALL); + memset(this->mBuffer,0,this->mBufferSize); + recv(this->mSocket,this->mBuffer,this->mBufferSize,MSG_WAITALL); } -#endif +#endif \ No newline at end of file diff --git a/launch/k2_client.launch b/launch/k2_client.launch index 4b23c9d..33fe667 100644 --- a/launch/k2_client.launch +++ b/launch/k2_client.launch @@ -11,4 +11,4 @@ - + \ No newline at end of file diff --git a/package.xml b/package.xml index c0892f3..254d85a 100644 --- a/package.xml +++ b/package.xml @@ -1,25 +1,26 @@ - k2_client - 1.1.0 - - startKinect2 - - Anurag Jakhotia - Anurag Jakhotia - BSD - - http://ros.org/wiki/startKinect2 - - catkin - rospy - roscpp - std_msgs - geometry_msgs - cv_bridge - image_transport - camera_info_manager - - - - + k2_client + 1.1.0 + + Client ROS nodes for the PRL Kinect2 Server + + Anurag Jakhotia + Pras Velagapudi + Stefania Pellegrinelli + Pras Velagapudi + BSD + http://github.com/personalrobotics/k2_client + + catkin + message_generation + message_runtime + rospy + roscpp + std_msgs + geometry_msgs + cv_bridge + image_transport + camera_info_manager + + \ No newline at end of file diff --git a/src/startAudio.cpp b/src/startAudio.cpp index 53f0531..a4672d8 100644 --- a/src/startAudio.cpp +++ b/src/startAudio.cpp @@ -1,31 +1,30 @@ /************************************************************************************* -Copyright (c) 2013, Carnegie Mellon University -All rights reserved. -Authors: Anurag Jakhotia, Prasanna Velagapudi - -Redistribution and use in source and binary forms, with or without modification, are -permitted provided that the following conditions are met: - - - Redistributions of source code must retain the above copyright notice, this list - of conditions and the following disclaimer. - - Redistributions in binary form must reproduce the above copyright notice, this - list of conditions and the following disclaimer in the documentation and/or other - materials provided with the distribution. - - Neither the name of Carnegie Mellon University nor the names of its contributors - may be used to endorse or promote products derived from this software without - specific prior written permission. + C *opyright (c) 2013, Carnegie Mellon University + All rights reserved. + Authors: Anurag Jakhotia, Prasanna Velagapudi + Redistribution and use in source and binary forms, with or without modification, are + permitted provided that the following conditions are met: + - Redistributions of source code must retain the above copyright notice, this list + of conditions and the following disclaimer. + - Redistributions in binary form must reproduce the above copyright notice, this + list of conditions and the following disclaimer in the documentation and/or other + materials provided with the distribution. + - Neither the name of Carnegie Mellon University nor the names of its contributors + may be used to endorse or promote products derived from this software without + specific prior written permission. -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY -EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES -OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT -SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED -TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR -BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -***************************************************************************************/ -#include "k2_client.h" + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY + EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT + SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED + TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR + BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY + WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ***************************************************************************************/ +#include "k2_client/k2_client.h" +#include "k2_client/Audio.h" std::string topicName = "audio"; int twiceStreamSize = 8200; @@ -33,37 +32,37 @@ int streamSize = 4100; int main(int argC,char **argV) { - ros::init(argC,argV,"startAudio"); - ros::NodeHandle n; - std::string serverAddress; - n.getParam("/serverNameOrIP",serverAddress); - Socket mySocket(serverAddress.c_str(),"9004",twiceStreamSize); - ros::Publisher audioPub = n.advertise(topicName,1); - while(ros::ok()) - { - mySocket.readData(); - std::string jsonString; - for(int i=0;i(topicName,1); + while(ros::ok()) + { + mySocket.readData(); + std::string jsonString; + for(int i=0;i, Prasanna Velagapudi - -Redistribution and use in source and binary forms, with or without modification, are -permitted provided that the following conditions are met: - + C *opyright (c) 2013, Carnegie Mellon University + All rights reserved. + Authors: Anurag Jakhotia, Prasanna Velagapudi + Redistribution and use in source and binary forms, with or without modification, are + permitted provided that the following conditions are met: - Redistributions of source code must retain the above copyright notice, this list - of conditions and the following disclaimer. + of conditions and the following disclaimer. - Redistributions in binary form must reproduce the above copyright notice, this - list of conditions and the following disclaimer in the documentation and/or other - materials provided with the distribution. + list of conditions and the following disclaimer in the documentation and/or other + materials provided with the distribution. - Neither the name of Carnegie Mellon University nor the names of its contributors - may be used to endorse or promote products derived from this software without - specific prior written permission. + may be used to endorse or promote products derived from this software without + specific prior written permission. -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY -EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES -OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT -SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED -TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR -BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -***************************************************************************************/ -#include "k2_client.h" + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY + EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT + SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED + TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR + BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY + WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ***************************************************************************************/ +#include "k2_client/k2_client.h" +#include "k2_client/BodyArray.h" #include - std::string topicName = "bodyArray"; size_t streamSize = 56008; size_t readSkipSize = 56000; @@ -44,17 +42,19 @@ int main(int argC,char **argV) iconv_t charConverter = iconv_open("UTF-8","UTF-16"); ros::Publisher bodyPub = n.advertise(topicName,1); char jsonCharArray[readSkipSize]; - + while(ros::ok()) { mySocket.readData(); char *jsonCharArrayPtr; char *socketCharArrayPtr; + size_t readSkipSizeC = readSkipSize; + size_t stringSizeloC= stringSize; jsonCharArrayPtr = jsonCharArray; socketCharArrayPtr = mySocket.mBuffer; - iconv(charConverter,&socketCharArrayPtr,&readSkipSize,&jsonCharArrayPtr,&stringSize); + iconv(charConverter,&socketCharArrayPtr,&readSkipSizeC,&jsonCharArrayPtr,&stringSizeloC); double utcTime; - memcpy(&utcTime,&mySocket.mBuffer[readSkipSize],sizeof(double)); + memcpy(&utcTime,&mySocket.mBuffer[readSkipSizeC],sizeof(double)); std::string jsonString(jsonCharArray); Json::Value jsonObject; Json::Reader jsonReader; @@ -124,13 +124,13 @@ int main(int argC,char **argV) case 23: fieldName = "HandTipRight";break; case 24: fieldName = "ThumbRight";break; } - + JOAT.orientation.x = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["X"].asDouble(); JOAT.orientation.y = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["Y"].asDouble(); JOAT.orientation.z = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["Z"].asDouble(); JOAT.orientation.w = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["W"].asDouble(); JOAT.jointType = jsonObject[i]["JointOrientations"][fieldName]["JointType"].asInt(); - + JPAS.trackingState = jsonObject[i]["Joints"][fieldName]["TrackingState"].asBool(); JPAS.position.x = jsonObject[i]["Joints"][fieldName]["Position"]["X"].asDouble(); JPAS.position.y = jsonObject[i]["Joints"][fieldName]["Position"]["Y"].asDouble(); @@ -151,4 +151,4 @@ int main(int argC,char **argV) bodyPub.publish(bodyArray); } return 0; -} +} \ No newline at end of file diff --git a/src/startDepth.cpp b/src/startDepth.cpp index f4b8629..0dc4f50 100644 --- a/src/startDepth.cpp +++ b/src/startDepth.cpp @@ -1,31 +1,29 @@ /************************************************************************************* -Copyright (c) 2013, Carnegie Mellon University -All rights reserved. -Authors: Anurag Jakhotia, Prasanna Velagapudi - -Redistribution and use in source and binary forms, with or without modification, are -permitted provided that the following conditions are met: - - - Redistributions of source code must retain the above copyright notice, this list - of conditions and the following disclaimer. - - Redistributions in binary form must reproduce the above copyright notice, this - list of conditions and the following disclaimer in the documentation and/or other - materials provided with the distribution. - - Neither the name of Carnegie Mellon University nor the names of its contributors - may be used to endorse or promote products derived from this software without - specific prior written permission. + C *opyright (c) 2013, Carnegie Mellon University + All rights reserved. + Authors: Anurag Jakhotia, Prasanna Velagapudi + Redistribution and use in source and binary forms, with or without modification, are + permitted provided that the following conditions are met: + - Redistributions of source code must retain the above copyright notice, this list + of conditions and the following disclaimer. + - Redistributions in binary form must reproduce the above copyright notice, this + list of conditions and the following disclaimer in the documentation and/or other + materials provided with the distribution. + - Neither the name of Carnegie Mellon University nor the names of its contributors + may be used to endorse or promote products derived from this software without + specific prior written permission. -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY -EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES -OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT -SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED -TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR -BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -***************************************************************************************/ -#include "k2_client.h" + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY + EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT + SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED + TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR + BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY + WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ***************************************************************************************/ +#include "k2_client/k2_client.h" // this alternate resolution for an aligned depth image //int imageSize = 639392; @@ -37,38 +35,38 @@ std::string cameraFrame = ""; int main(int argC,char **argV) { - ros::init(argC,argV,"startDepth"); - ros::NodeHandle n(cameraName); - image_transport::ImageTransport imT(n); - std::string serverAddress; - n.getParam("/serverNameOrIP",serverAddress); + ros::init(argC,argV,"startDepth"); + ros::NodeHandle n(cameraName); + image_transport::ImageTransport imT(n); + std::string serverAddress; + n.getParam("/serverNameOrIP",serverAddress); n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) + - "/depth_frame", cameraFrame); - Socket mySocket(serverAddress.c_str(),"9001",streamSize); + "/depth_frame", cameraFrame); + Socket mySocket(serverAddress.c_str(),"9001",streamSize); image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera( - imageTopicSubName, 1); - camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName); - camInfoMgr.loadCameraInfo(""); - cv::Mat frame; - cv_bridge::CvImage cvImage; - sensor_msgs::Image rosImage; - while(ros::ok()) - { - mySocket.readData(); + imageTopicSubName, 1); + camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName); + camInfoMgr.loadCameraInfo(""); + cv::Mat frame; + cv_bridge::CvImage cvImage; + sensor_msgs::Image rosImage; + while(ros::ok()) + { + mySocket.readData(); // this alternate resolution was for an aligned depth image - //frame = cv::Mat(cv::Size(754,424),CV_16UC1,mySocket.mBuffer); + //frame = cv::Mat(cv::Size(754,424),CV_16UC1,mySocket.mBuffer); frame = cv::Mat(cv::Size(512,424), CV_16UC1,mySocket.mBuffer); - cv::flip(frame,frame,1); - double utcTime; - memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double)); + cv::flip(frame,frame,1); + double utcTime; + memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double)); cvImage.header.frame_id = cameraFrame.c_str(); - cvImage.encoding = "16UC1"; - cvImage.image = frame; - cvImage.toImageMsg(rosImage); - sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo(); - camInfo.header.frame_id = cvImage.header.frame_id; + cvImage.encoding = "16UC1"; + cvImage.image = frame; + cvImage.toImageMsg(rosImage); + sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo(); + camInfo.header.frame_id = cvImage.header.frame_id; cameraPublisher.publish(rosImage, camInfo, ros::Time(utcTime)); - ros::spinOnce(); - } - return 0; -} + ros::spinOnce(); + } + return 0; +} \ No newline at end of file diff --git a/src/startIR.cpp b/src/startIR.cpp index edee437..5090a5d 100644 --- a/src/startIR.cpp +++ b/src/startIR.cpp @@ -1,31 +1,29 @@ /************************************************************************************* -Copyright (c) 2013, Carnegie Mellon University -All rights reserved. -Authors: Anurag Jakhotia, Prasanna Velagapudi - -Redistribution and use in source and binary forms, with or without modification, are -permitted provided that the following conditions are met: - - - Redistributions of source code must retain the above copyright notice, this list - of conditions and the following disclaimer. - - Redistributions in binary form must reproduce the above copyright notice, this - list of conditions and the following disclaimer in the documentation and/or other - materials provided with the distribution. - - Neither the name of Carnegie Mellon University nor the names of its contributors - may be used to endorse or promote products derived from this software without - specific prior written permission. + C *opyright (c) 2013, Carnegie Mellon University + All rights reserved. + Authors: Anurag Jakhotia, Prasanna Velagapudi + Redistribution and use in source and binary forms, with or without modification, are + permitted provided that the following conditions are met: + - Redistributions of source code must retain the above copyright notice, this list + of conditions and the following disclaimer. + - Redistributions in binary form must reproduce the above copyright notice, this + list of conditions and the following disclaimer in the documentation and/or other + materials provided with the distribution. + - Neither the name of Carnegie Mellon University nor the names of its contributors + may be used to endorse or promote products derived from this software without + specific prior written permission. -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY -EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES -OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT -SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED -TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR -BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -***************************************************************************************/ -#include "k2_client.h" + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY + EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT + SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED + TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR + BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY + WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ***************************************************************************************/ +#include "k2_client/k2_client.h" int imageSize = 434176; int streamSize = imageSize + sizeof(double); @@ -35,36 +33,36 @@ std::string cameraFrame = ""; int main(int argC,char **argV) { - ros::init(argC,argV,"startIR"); - ros::NodeHandle n(cameraName); - image_transport::ImageTransport imT(n); - std::string serverAddress; - n.getParam("/serverNameOrIP",serverAddress); + ros::init(argC,argV,"startIR"); + ros::NodeHandle n(cameraName); + image_transport::ImageTransport imT(n); + std::string serverAddress; + n.getParam("/serverNameOrIP",serverAddress); n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) + - "/ir_frame", cameraFrame); - Socket mySocket(serverAddress.c_str(),"9002",streamSize); + "/ir_frame", cameraFrame); + Socket mySocket(serverAddress.c_str(),"9002",streamSize); image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera( - imageTopicSubName, 1); - camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName); - camInfoMgr.loadCameraInfo(""); - cv::Mat frame; - cv_bridge::CvImage cvImage; - sensor_msgs::Image rosImage; - while(ros::ok()) - { - mySocket.readData(); - frame= cv::Mat(cv::Size(512,424),CV_16UC1,(mySocket.mBuffer)); - cv::flip(frame,frame,1); - double utcTime; - memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double)); - cvImage.header.frame_id = cameraFrame.c_str(); - cvImage.encoding = "mono16"; - cvImage.image = frame; - cvImage.toImageMsg(rosImage); - sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo(); - camInfo.header.frame_id = cvImage.header.frame_id; + imageTopicSubName, 1); + camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName); + camInfoMgr.loadCameraInfo(""); + cv::Mat frame; + cv_bridge::CvImage cvImage; + sensor_msgs::Image rosImage; + while(ros::ok()) + { + mySocket.readData(); + frame= cv::Mat(cv::Size(512,424),CV_16UC1,(mySocket.mBuffer)); + cv::flip(frame,frame,1); + double utcTime; + memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double)); + cvImage.header.frame_id = cameraFrame.c_str(); + cvImage.encoding = "mono16"; + cvImage.image = frame; + cvImage.toImageMsg(rosImage); + sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo(); + camInfo.header.frame_id = cvImage.header.frame_id; cameraPublisher.publish(rosImage, camInfo, ros::Time(utcTime)); - ros::spinOnce(); - } - return 0; -} + ros::spinOnce(); + } + return 0; +} \ No newline at end of file diff --git a/src/startRGB.cpp b/src/startRGB.cpp index 64b726a..4d137bd 100644 --- a/src/startRGB.cpp +++ b/src/startRGB.cpp @@ -1,75 +1,72 @@ /************************************************************************************* -Copyright (c) 2013, Carnegie Mellon University -All rights reserved. -Authors: Anurag Jakhotia, Prasanna Velagapudi - -Redistribution and use in source and binary forms, with or without modification, are -permitted provided that the following conditions are met: - - - Redistributions of source code must retain the above copyright notice, this list - of conditions and the following disclaimer. - - Redistributions in binary form must reproduce the above copyright notice, this - list of conditions and the following disclaimer in the documentation and/or other - materials provided with the distribution. - - Neither the name of Carnegie Mellon University nor the names of its contributors - may be used to endorse or promote products derived from this software without - specific prior written permission. + C *opyright (c) 2013, Carnegie Mellon University + All rights reserved. + Authors: Anurag Jakhotia, Prasanna Velagapudi + Redistribution and use in source and binary forms, with or without modification, are + permitted provided that the following conditions are met: + - Redistributions of source code must retain the above copyright notice, this list + of conditions and the following disclaimer. + - Redistributions in binary form must reproduce the above copyright notice, this + list of conditions and the following disclaimer in the documentation and/or other + materials provided with the distribution. + - Neither the name of Carnegie Mellon University nor the names of its contributors + may be used to endorse or promote products derived from this software without + specific prior written permission. -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY -EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES -OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT -SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED -TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR -BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -***************************************************************************************/ -#include "k2_client.h" + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY + EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT + SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED + TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR + BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY + WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ***************************************************************************************/ +#include -int imageSize = 6220800; +int imageSize = 8294400; int streamSize = imageSize + sizeof(double); std::string cameraName = "rgb"; std::string imageTopicSubName = "image_color"; +std::string cameraInfoSubName = "camera_info"; std::string cameraFrame = ""; int main(int argC,char **argV) { - ros::init(argC,argV,"startRGB"); - ros::NodeHandle n(cameraName); - image_transport::ImageTransport imT(n); - std::string serverAddress; - n.getParam("/serverNameOrIP",serverAddress); - n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) + - "/rgb_frame", cameraFrame); - Socket mySocket(serverAddress.c_str(),"9000",streamSize); - image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera( - imageTopicSubName, 1); - camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName); - camInfoMgr.loadCameraInfo(""); - cv::Mat frame; - cv_bridge::CvImage cvImage; - sensor_msgs::Image rosImage; - while(ros::ok()) - { - printf("Got a frame.\n"); - - mySocket.readData(); - printf("Creating mat.\n"); - frame = cv::Mat(cv::Size(1920,1080), CV_8UC3, mySocket.mBuffer); - cv::flip(frame,frame,1); - printf("Getting time.\n"); - double utcTime; - memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double)); + ros::init(argC,argV,"startRGB"); + ros::NodeHandle n(cameraName); + image_transport::ImageTransport imT(n); + std::string serverAddress; + n.getParam("/serverNameOrIP",serverAddress); + n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) + "/rgb_frame", cameraFrame); + Socket mySocket(serverAddress.c_str(),const_cast("9000"),streamSize); + image_transport::Publisher imagePublisher = imT.advertise(imageTopicSubName, 1); + ros::Publisher cameraInfoPub = n.advertise(cameraInfoSubName,1); + camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName); + camInfoMgr.loadCameraInfo(""); + cv::Mat frame; + cv_bridge::CvImage cvImage; + sensor_msgs::Image rosImage; + while(ros::ok()) + { + mySocket.readData(); + frame = cv::Mat(cv::Size(1920,1080), CV_8UC4, mySocket.mBuffer); + cv::flip(frame,frame,1); + double utcTime; + memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double)); + cvImage.header.stamp = ros::Time(utcTime); cvImage.header.frame_id = cameraFrame.c_str(); - cvImage.encoding = "bgr8"; - cvImage.image = frame; - cvImage.toImageMsg(rosImage); - sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo(); - camInfo.header.frame_id = cvImage.header.frame_id; + cvImage.encoding = "bgra8"; + cvImage.image = frame; + cvImage.toImageMsg(rosImage); + sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo(); + camInfo.header.stamp = cvImage.header.stamp; + camInfo.header.frame_id = cvImage.header.frame_id; + cameraInfoPub.publish(camInfo); printf("Updating.\n"); - cameraPublisher.publish(rosImage, camInfo, ros::Time(utcTime)); - ros::spinOnce(); - } - return 0; -} + imagePublisher.publish(rosImage); + ros::spinOnce(); + } + return 0; +} \ No newline at end of file From 399cdc63785cb031ed771b4694b42460aee80f39 Mon Sep 17 00:00:00 2001 From: stepelle Date: Thu, 5 Nov 2015 11:45:42 -0500 Subject: [PATCH 2/9] Accepted --- src/startBody.cpp | 53 +++++++++++++++++++++----------- src/startDepth.cpp | 2 +- src/startIR.cpp | 2 +- src/startRGB.cpp | 75 +++++++++++++++++++++++----------------------- 4 files changed, 74 insertions(+), 58 deletions(-) diff --git a/src/startBody.cpp b/src/startBody.cpp index 1be4a64..a393317 100644 --- a/src/startBody.cpp +++ b/src/startBody.cpp @@ -30,9 +30,9 @@ WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH #include std::string topicName = "bodyArray"; -size_t streamSize = 56008; -size_t readSkipSize = 56000; -size_t stringSize = 28000; +int readSkipSize = 56000; +int stringSize = 28000; +int streamSize = readSkipSize + sizeof(double); int main(int argC,char **argV) { @@ -40,21 +40,36 @@ int main(int argC,char **argV) ros::NodeHandle n; std::string serverAddress; n.getParam("/serverNameOrIP",serverAddress); - Socket mySocket(serverAddress.c_str(),"9003",streamSize); - iconv_t charConverter = iconv_open("UTF-8","UTF-16"); + Socket mySocket(serverAddress.c_str(),const_cast("9003"), streamSize); ros::Publisher bodyPub = n.advertise(topicName,1); - char jsonCharArray[readSkipSize]; - + char utf16Array[readSkipSize]; + char jsonCharArray[stringSize]; + char *jsonCharArrayPtr; + char *utf16ArrayPtr; + ros::Rate r(30); while(ros::ok()) { - mySocket.readData(); - char *jsonCharArrayPtr; - char *socketCharArrayPtr; + mySocket.readData(); jsonCharArrayPtr = jsonCharArray; - socketCharArrayPtr = mySocket.mBuffer; - iconv(charConverter,&socketCharArrayPtr,&readSkipSize,&jsonCharArrayPtr,&stringSize); - double utcTime; - memcpy(&utcTime,&mySocket.mBuffer[readSkipSize],sizeof(double)); + utf16ArrayPtr = utf16Array; + memset(utf16Array, 0, sizeof(utf16Array)); + memcpy(utf16Array, mySocket.mBuffer, sizeof(utf16Array)); + size_t iconv_in = static_cast(readSkipSize); + size_t iconv_out = static_cast(stringSize); + iconv_t charConverter = iconv_open("UTF-8","UTF-16"); + size_t nconv = iconv(charConverter, &utf16ArrayPtr, &iconv_in, &jsonCharArrayPtr, &iconv_out); + iconv_close(charConverter); + if (nconv == (size_t) - 1) + { + if (errno == EINVAL) + ROS_ERROR("An incomplete multibyte sequence has been encountered in the input."); + else if (errno == EILSEQ) + ROS_ERROR("An invalid multibyte sequence has been encountered in the input."); + else + ROS_ERROR("There is not sufficient room at jsonCharArray"); + } + double utcTime = 0.0; + memcpy(&utcTime,&mySocket.mBuffer[readSkipSize], sizeof(double)); std::string jsonString(jsonCharArray); Json::Value jsonObject; Json::Reader jsonReader; @@ -113,7 +128,7 @@ int main(int argC,char **argV) case 12: fieldName = "HipLeft";break; case 13: fieldName = "KneeLeft";break; case 14: fieldName = "AnkleLeft";break; - case 15: fieldName = "FootLeft";break; + case 15: fieldName = "SpineBase";break; case 16: fieldName = "HipRight";break; case 17: fieldName = "KneeRight";break; case 18: fieldName = "AnkleRight";break; @@ -124,13 +139,13 @@ int main(int argC,char **argV) case 23: fieldName = "HandTipRight";break; case 24: fieldName = "ThumbRight";break; } - + JOAT.orientation.x = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["X"].asDouble(); JOAT.orientation.y = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["Y"].asDouble(); JOAT.orientation.z = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["Z"].asDouble(); JOAT.orientation.w = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["W"].asDouble(); JOAT.jointType = jsonObject[i]["JointOrientations"][fieldName]["JointType"].asInt(); - + JPAS.trackingState = jsonObject[i]["Joints"][fieldName]["TrackingState"].asBool(); JPAS.position.x = jsonObject[i]["Joints"][fieldName]["Position"]["X"].asDouble(); JPAS.position.y = jsonObject[i]["Joints"][fieldName]["Position"]["Y"].asDouble(); @@ -149,6 +164,8 @@ int main(int argC,char **argV) continue; } bodyPub.publish(bodyArray); + ros::spinOnce(); + r.sleep(); } return 0; -} +} \ No newline at end of file diff --git a/src/startDepth.cpp b/src/startDepth.cpp index 2248688..e69b271 100644 --- a/src/startDepth.cpp +++ b/src/startDepth.cpp @@ -44,7 +44,7 @@ int main(int argC,char **argV) n.getParam("/serverNameOrIP",serverAddress); n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) + "/depth_frame", cameraFrame); - Socket mySocket(serverAddress.c_str(),"9001",streamSize); + Socket mySocket(serverAddress.c_str(),const_cast("9001"),streamSize); image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera( imageTopicSubName, 1); camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName); diff --git a/src/startIR.cpp b/src/startIR.cpp index 7bb7d36..7ffb771 100644 --- a/src/startIR.cpp +++ b/src/startIR.cpp @@ -42,7 +42,7 @@ int main(int argC,char **argV) n.getParam("/serverNameOrIP",serverAddress); n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) + "/ir_frame", cameraFrame); - Socket mySocket(serverAddress.c_str(),"9002",streamSize); + Socket mySocket(serverAddress.c_str(),const_cast("9002"),streamSize); image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera( imageTopicSubName, 1); camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName); diff --git a/src/startRGB.cpp b/src/startRGB.cpp index a66bf3e..015f71c 100644 --- a/src/startRGB.cpp +++ b/src/startRGB.cpp @@ -25,51 +25,50 @@ BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ***************************************************************************************/ -#include "k2_client/k2_client.h" +#include -int imageSize = 6220800; +int imageSize = 8294400; int streamSize = imageSize + sizeof(double); std::string cameraName = "rgb"; std::string imageTopicSubName = "image_color"; +std::string cameraInfoSubName = "camera_info"; std::string cameraFrame = ""; int main(int argC,char **argV) { - ros::init(argC,argV,"startRGB"); - ros::NodeHandle n(cameraName); - image_transport::ImageTransport imT(n); - std::string serverAddress; - n.getParam("/serverNameOrIP",serverAddress); - n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) + - "/rgb_frame", cameraFrame); - Socket mySocket(serverAddress.c_str(),"9000",streamSize); - image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera( - imageTopicSubName, 1); - camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName); - camInfoMgr.loadCameraInfo(""); - cv::Mat frame; - cv_bridge::CvImage cvImage; - sensor_msgs::Image rosImage; - while(ros::ok()) - { - printf("Got a frame.\n"); - - mySocket.readData(); - printf("Creating mat.\n"); - frame = cv::Mat(cv::Size(1920,1080), CV_8UC3, mySocket.mBuffer); - cv::flip(frame,frame,1); - printf("Getting time.\n"); - double utcTime; - memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double)); + ros::init(argC,argV,"startRGB"); + ros::NodeHandle n(cameraName); + image_transport::ImageTransport imT(n); + std::string serverAddress; + n.getParam("/serverNameOrIP",serverAddress); + n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) + "/rgb_frame", cameraFrame); + Socket mySocket(serverAddress.c_str(),const_cast("9000"),streamSize); + image_transport::Publisher imagePublisher = imT.advertise(imageTopicSubName, 1); + ros::Publisher cameraInfoPub = n.advertise(cameraInfoSubName,1); + camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName); + camInfoMgr.loadCameraInfo(""); + cv::Mat frame; + cv_bridge::CvImage cvImage; + sensor_msgs::Image rosImage; + while(ros::ok()) + { + mySocket.readData(); + frame = cv::Mat(cv::Size(1920,1080), CV_8UC4, mySocket.mBuffer); + cv::flip(frame,frame,1); + double utcTime; + memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double)); + cvImage.header.stamp = ros::Time(utcTime); cvImage.header.frame_id = cameraFrame.c_str(); - cvImage.encoding = "bgr8"; - cvImage.image = frame; - cvImage.toImageMsg(rosImage); - sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo(); - camInfo.header.frame_id = cvImage.header.frame_id; + cvImage.encoding = "bgra8"; + cvImage.image = frame; + cvImage.toImageMsg(rosImage); + sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo(); + camInfo.header.stamp = cvImage.header.stamp; + camInfo.header.frame_id = cvImage.header.frame_id; + cameraInfoPub.publish(camInfo); printf("Updating.\n"); - cameraPublisher.publish(rosImage, camInfo, ros::Time(utcTime)); - ros::spinOnce(); - } - return 0; -} + imagePublisher.publish(rosImage); + ros::spinOnce(); + } + return 0; +} \ No newline at end of file From 983915d72ecd4ed7f2bb131cdb729bcaf2074770 Mon Sep 17 00:00:00 2001 From: Pras Velagapudi Date: Thu, 5 Nov 2015 19:02:52 -0500 Subject: [PATCH 3/9] Added trailing newline. --- launch/k2_client.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/k2_client.launch b/launch/k2_client.launch index 33fe667..4b23c9d 100644 --- a/launch/k2_client.launch +++ b/launch/k2_client.launch @@ -11,4 +11,4 @@ - \ No newline at end of file + From 902ce1d3760c1e662c1a87ec1834baf103a2c319 Mon Sep 17 00:00:00 2001 From: Pras Velagapudi Date: Thu, 5 Nov 2015 19:03:23 -0500 Subject: [PATCH 4/9] Added trailing newline. --- src/startRGB.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/startRGB.cpp b/src/startRGB.cpp index 015f71c..98d6db0 100644 --- a/src/startRGB.cpp +++ b/src/startRGB.cpp @@ -71,4 +71,4 @@ int main(int argC,char **argV) ros::spinOnce(); } return 0; -} \ No newline at end of file +} From 9a6600bd7379dae3692dd037609e91d9fb4efaca Mon Sep 17 00:00:00 2001 From: stepelle Date: Thu, 12 Nov 2015 13:46:45 -0500 Subject: [PATCH 5/9] Removal of const_cast in all start*.cpp files and rate limit in startBody --- src/startBody.cpp | 49 ++++++++++++++++------------------------------ src/startDepth.cpp | 2 +- src/startIR.cpp | 2 +- src/startRGB.cpp | 25 +++++++++++------------ 4 files changed, 32 insertions(+), 46 deletions(-) diff --git a/src/startBody.cpp b/src/startBody.cpp index a393317..da4e023 100644 --- a/src/startBody.cpp +++ b/src/startBody.cpp @@ -30,9 +30,9 @@ WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH #include std::string topicName = "bodyArray"; -int readSkipSize = 56000; -int stringSize = 28000; -int streamSize = readSkipSize + sizeof(double); +size_t streamSize = 56008; +size_t readSkipSize = 56000; +size_t stringSize = 28000; int main(int argC,char **argV) { @@ -40,36 +40,23 @@ int main(int argC,char **argV) ros::NodeHandle n; std::string serverAddress; n.getParam("/serverNameOrIP",serverAddress); - Socket mySocket(serverAddress.c_str(),const_cast("9003"), streamSize); + Socket mySocket(serverAddress.c_str(),"9003",streamSize); + iconv_t charConverter = iconv_open("UTF-8","UTF-16"); ros::Publisher bodyPub = n.advertise(topicName,1); - char utf16Array[readSkipSize]; - char jsonCharArray[stringSize]; - char *jsonCharArrayPtr; - char *utf16ArrayPtr; - ros::Rate r(30); + char jsonCharArray[readSkipSize]; + while(ros::ok()) { - mySocket.readData(); + mySocket.readData(); + char *jsonCharArrayPtr; + char *socketCharArrayPtr; + size_t readSkipSizeC = readSkipSize; + size_t stringSizeloC= stringSize; jsonCharArrayPtr = jsonCharArray; - utf16ArrayPtr = utf16Array; - memset(utf16Array, 0, sizeof(utf16Array)); - memcpy(utf16Array, mySocket.mBuffer, sizeof(utf16Array)); - size_t iconv_in = static_cast(readSkipSize); - size_t iconv_out = static_cast(stringSize); - iconv_t charConverter = iconv_open("UTF-8","UTF-16"); - size_t nconv = iconv(charConverter, &utf16ArrayPtr, &iconv_in, &jsonCharArrayPtr, &iconv_out); - iconv_close(charConverter); - if (nconv == (size_t) - 1) - { - if (errno == EINVAL) - ROS_ERROR("An incomplete multibyte sequence has been encountered in the input."); - else if (errno == EILSEQ) - ROS_ERROR("An invalid multibyte sequence has been encountered in the input."); - else - ROS_ERROR("There is not sufficient room at jsonCharArray"); - } - double utcTime = 0.0; - memcpy(&utcTime,&mySocket.mBuffer[readSkipSize], sizeof(double)); + socketCharArrayPtr = mySocket.mBuffer; + iconv(charConverter,&socketCharArrayPtr,&readSkipSizeC,&jsonCharArrayPtr,&stringSizeloC); + double utcTime; + memcpy(&utcTime,&mySocket.mBuffer[readSkipSizeC],sizeof(double)); std::string jsonString(jsonCharArray); Json::Value jsonObject; Json::Reader jsonReader; @@ -128,7 +115,7 @@ int main(int argC,char **argV) case 12: fieldName = "HipLeft";break; case 13: fieldName = "KneeLeft";break; case 14: fieldName = "AnkleLeft";break; - case 15: fieldName = "SpineBase";break; + case 15: fieldName = "FootLeft";break; case 16: fieldName = "HipRight";break; case 17: fieldName = "KneeRight";break; case 18: fieldName = "AnkleRight";break; @@ -164,8 +151,6 @@ int main(int argC,char **argV) continue; } bodyPub.publish(bodyArray); - ros::spinOnce(); - r.sleep(); } return 0; } \ No newline at end of file diff --git a/src/startDepth.cpp b/src/startDepth.cpp index e69b271..c3deda0 100644 --- a/src/startDepth.cpp +++ b/src/startDepth.cpp @@ -44,7 +44,7 @@ int main(int argC,char **argV) n.getParam("/serverNameOrIP",serverAddress); n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) + "/depth_frame", cameraFrame); - Socket mySocket(serverAddress.c_str(),const_cast("9001"),streamSize); + Socket mySocket(serverAddress.c_str(),"9001",streamSize); image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera( imageTopicSubName, 1); camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName); diff --git a/src/startIR.cpp b/src/startIR.cpp index 7ffb771..7ecfde6 100644 --- a/src/startIR.cpp +++ b/src/startIR.cpp @@ -42,7 +42,7 @@ int main(int argC,char **argV) n.getParam("/serverNameOrIP",serverAddress); n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) + "/ir_frame", cameraFrame); - Socket mySocket(serverAddress.c_str(),const_cast("9002"),streamSize); + Socket mySocket(serverAddress.c_str(),"9002",streamSize); image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera( imageTopicSubName, 1); camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName); diff --git a/src/startRGB.cpp b/src/startRGB.cpp index 015f71c..ed519aa 100644 --- a/src/startRGB.cpp +++ b/src/startRGB.cpp @@ -25,13 +25,12 @@ BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ***************************************************************************************/ -#include +#include "k2_client/k2_client.h" int imageSize = 8294400; int streamSize = imageSize + sizeof(double); std::string cameraName = "rgb"; std::string imageTopicSubName = "image_color"; -std::string cameraInfoSubName = "camera_info"; std::string cameraFrame = ""; int main(int argC,char **argV) @@ -41,10 +40,11 @@ int main(int argC,char **argV) image_transport::ImageTransport imT(n); std::string serverAddress; n.getParam("/serverNameOrIP",serverAddress); - n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) + "/rgb_frame", cameraFrame); - Socket mySocket(serverAddress.c_str(),const_cast("9000"),streamSize); - image_transport::Publisher imagePublisher = imT.advertise(imageTopicSubName, 1); - ros::Publisher cameraInfoPub = n.advertise(cameraInfoSubName,1); + n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) + + "/rgb_frame", cameraFrame); + Socket mySocket(serverAddress.c_str(),"9000",streamSize); + image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera( + imageTopicSubName, 1); camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName); camInfoMgr.loadCameraInfo(""); cv::Mat frame; @@ -52,22 +52,23 @@ int main(int argC,char **argV) sensor_msgs::Image rosImage; while(ros::ok()) { + printf("Got a frame.\n"); + mySocket.readData(); - frame = cv::Mat(cv::Size(1920,1080), CV_8UC4, mySocket.mBuffer); + printf("Creating mat.\n"); + frame = cv::Mat(cv::Size(1920,1080), CV_8UC3, mySocket.mBuffer); cv::flip(frame,frame,1); + printf("Getting time.\n"); double utcTime; memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double)); - cvImage.header.stamp = ros::Time(utcTime); cvImage.header.frame_id = cameraFrame.c_str(); - cvImage.encoding = "bgra8"; + cvImage.encoding = "bgr8"; cvImage.image = frame; cvImage.toImageMsg(rosImage); sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo(); - camInfo.header.stamp = cvImage.header.stamp; camInfo.header.frame_id = cvImage.header.frame_id; - cameraInfoPub.publish(camInfo); printf("Updating.\n"); - imagePublisher.publish(rosImage); + cameraPublisher.publish(rosImage, camInfo, ros::Time(utcTime)); ros::spinOnce(); } return 0; From 30070a0ae7e8242898284721dfd1d1002585657e Mon Sep 17 00:00:00 2001 From: stepelle Date: Thu, 12 Nov 2015 14:06:13 -0500 Subject: [PATCH 6/9] space/tab adjustment --- src/startDepth.cpp | 18 +++++------ src/startIR.cpp | 12 ++++---- src/startRGB.cpp | 74 +++++++++++++++++++++++----------------------- 3 files changed, 52 insertions(+), 52 deletions(-) diff --git a/src/startDepth.cpp b/src/startDepth.cpp index c3deda0..e312853 100644 --- a/src/startDepth.cpp +++ b/src/startDepth.cpp @@ -42,11 +42,11 @@ int main(int argC,char **argV) image_transport::ImageTransport imT(n); std::string serverAddress; n.getParam("/serverNameOrIP",serverAddress); - n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) + - "/depth_frame", cameraFrame); - Socket mySocket(serverAddress.c_str(),"9001",streamSize); - image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera( - imageTopicSubName, 1); + n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) + + "/depth_frame", cameraFrame); + Socket mySocket(serverAddress.c_str(),"9001",streamSize); + image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera( + imageTopicSubName, 1); camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName); camInfoMgr.loadCameraInfo(""); cv::Mat frame; @@ -55,19 +55,19 @@ int main(int argC,char **argV) while(ros::ok()) { mySocket.readData(); - // this alternate resolution was for an aligned depth image + // this alternate resolution was for an aligned depth image //frame = cv::Mat(cv::Size(754,424),CV_16UC1,mySocket.mBuffer); - frame = cv::Mat(cv::Size(512,424), CV_16UC1,mySocket.mBuffer); + frame = cv::Mat(cv::Size(512,424), CV_16UC1,mySocket.mBuffer); cv::flip(frame,frame,1); double utcTime; memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double)); - cvImage.header.frame_id = cameraFrame.c_str(); + cvImage.header.frame_id = cameraFrame.c_str(); cvImage.encoding = "16UC1"; cvImage.image = frame; cvImage.toImageMsg(rosImage); sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo(); camInfo.header.frame_id = cvImage.header.frame_id; - cameraPublisher.publish(rosImage, camInfo, ros::Time(utcTime)); + cameraPublisher.publish(rosImage, camInfo, ros::Time(utcTime)); ros::spinOnce(); } return 0; diff --git a/src/startIR.cpp b/src/startIR.cpp index 7ecfde6..92a0cdc 100644 --- a/src/startIR.cpp +++ b/src/startIR.cpp @@ -40,11 +40,11 @@ int main(int argC,char **argV) image_transport::ImageTransport imT(n); std::string serverAddress; n.getParam("/serverNameOrIP",serverAddress); - n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) + - "/ir_frame", cameraFrame); - Socket mySocket(serverAddress.c_str(),"9002",streamSize); - image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera( - imageTopicSubName, 1); + n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) + + "/ir_frame", cameraFrame); + Socket mySocket(serverAddress.c_str(),"9002",streamSize); + image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera( + imageTopicSubName, 1); camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName); camInfoMgr.loadCameraInfo(""); cv::Mat frame; @@ -63,7 +63,7 @@ int main(int argC,char **argV) cvImage.toImageMsg(rosImage); sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo(); camInfo.header.frame_id = cvImage.header.frame_id; - cameraPublisher.publish(rosImage, camInfo, ros::Time(utcTime)); + cameraPublisher.publish(rosImage, camInfo, ros::Time(utcTime)); ros::spinOnce(); } return 0; diff --git a/src/startRGB.cpp b/src/startRGB.cpp index 461f27f..1797943 100644 --- a/src/startRGB.cpp +++ b/src/startRGB.cpp @@ -35,41 +35,41 @@ std::string cameraFrame = ""; int main(int argC,char **argV) { - ros::init(argC,argV,"startRGB"); - ros::NodeHandle n(cameraName); - image_transport::ImageTransport imT(n); - std::string serverAddress; - n.getParam("/serverNameOrIP",serverAddress); - n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) + - "/rgb_frame", cameraFrame); - Socket mySocket(serverAddress.c_str(),"9000",streamSize); - image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera( - imageTopicSubName, 1); - camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName); - camInfoMgr.loadCameraInfo(""); - cv::Mat frame; - cv_bridge::CvImage cvImage; - sensor_msgs::Image rosImage; - while(ros::ok()) - { - printf("Got a frame.\n"); - - mySocket.readData(); - printf("Creating mat.\n"); - frame = cv::Mat(cv::Size(1920,1080), CV_8UC3, mySocket.mBuffer); - cv::flip(frame,frame,1); - printf("Getting time.\n"); - double utcTime; - memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double)); - cvImage.header.frame_id = cameraFrame.c_str(); - cvImage.encoding = "bgr8"; - cvImage.image = frame; - cvImage.toImageMsg(rosImage); - sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo(); - camInfo.header.frame_id = cvImage.header.frame_id; - printf("Updating.\n"); - cameraPublisher.publish(rosImage, camInfo, ros::Time(utcTime)); - ros::spinOnce(); - } - return 0; + ros::init(argC,argV,"startRGB"); + ros::NodeHandle n(cameraName); + image_transport::ImageTransport imT(n); + std::string serverAddress; + n.getParam("/serverNameOrIP",serverAddress); + n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) + + "/rgb_frame", cameraFrame); + Socket mySocket(serverAddress.c_str(),"9000",streamSize); + image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera( + imageTopicSubName, 1); + camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName); + camInfoMgr.loadCameraInfo(""); + cv::Mat frame; + cv_bridge::CvImage cvImage; + sensor_msgs::Image rosImage; + while(ros::ok()) + { + printf("Got a frame.\n"); + + mySocket.readData(); + printf("Creating mat.\n"); + frame = cv::Mat(cv::Size(1920,1080), CV_8UC3, mySocket.mBuffer); + cv::flip(frame,frame,1); + printf("Getting time.\n"); + double utcTime; + memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double)); + cvImage.header.frame_id = cameraFrame.c_str(); + cvImage.encoding = "bgr8"; + cvImage.image = frame; + cvImage.toImageMsg(rosImage); + sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo(); + camInfo.header.frame_id = cvImage.header.frame_id; + printf("Updating.\n"); + cameraPublisher.publish(rosImage, camInfo, ros::Time(utcTime)); + ros::spinOnce(); + } + return 0; } From 1cf3a1d6c30e1707623874cd431b80ee6ff3ee68 Mon Sep 17 00:00:00 2001 From: stepelle Date: Fri, 22 Jan 2016 15:55:04 -0500 Subject: [PATCH 7/9] Fixed string size to avoid failure to parse with more than one human --- launch/k2_client.launch | 11 +++++++---- package.xml | 1 + src/startBody.cpp | 22 ++++++++++++++++------ 3 files changed, 24 insertions(+), 10 deletions(-) diff --git a/launch/k2_client.launch b/launch/k2_client.launch index 4b23c9d..59fe1b4 100644 --- a/launch/k2_client.launch +++ b/launch/k2_client.launch @@ -1,14 +1,17 @@ - + + - + - + + + diff --git a/package.xml b/package.xml index 7e4d761..b3efafe 100644 --- a/package.xml +++ b/package.xml @@ -22,6 +22,7 @@ cv_bridge image_transport camera_info_manager + jsoncpp diff --git a/src/startBody.cpp b/src/startBody.cpp index da4e023..8143028 100644 --- a/src/startBody.cpp +++ b/src/startBody.cpp @@ -30,9 +30,12 @@ WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH #include std::string topicName = "bodyArray"; -size_t streamSize = 56008; -size_t readSkipSize = 56000; -size_t stringSize = 28000; +// size_t streamSize = 56008; +// size_t readSkipSize = 56000; +// size_t stringSize = 28000; +size_t streamSize = 80008; +size_t readSkipSize = 80000; +size_t stringSize = 40000; int main(int argC,char **argV) { @@ -47,13 +50,13 @@ int main(int argC,char **argV) while(ros::ok()) { - mySocket.readData(); + mySocket.readData(); char *jsonCharArrayPtr; char *socketCharArrayPtr; size_t readSkipSizeC = readSkipSize; size_t stringSizeloC= stringSize; jsonCharArrayPtr = jsonCharArray; - socketCharArrayPtr = mySocket.mBuffer; + socketCharArrayPtr = mySocket.mBuffer; iconv(charConverter,&socketCharArrayPtr,&readSkipSizeC,&jsonCharArrayPtr,&stringSizeloC); double utcTime; memcpy(&utcTime,&mySocket.mBuffer[readSkipSizeC],sizeof(double)); @@ -61,14 +64,19 @@ int main(int argC,char **argV) Json::Value jsonObject; Json::Reader jsonReader; bool parsingSuccessful = jsonReader.parse(jsonString,jsonObject,false); + if(!parsingSuccessful) { ROS_ERROR("Failure to parse"); - continue; } + + if (jsonObject.size() != 6) + continue; + k2_client::BodyArray bodyArray; try { + //ROS_ERROR("Parsing successulff"); for(int i=0;i<6;i++) { k2_client::Body body; @@ -142,6 +150,7 @@ int main(int argC,char **argV) body.jointOrientations.push_back(JOAT); body.jointPositions.push_back(JPAS); } + //std::cout << ros::Time::now() < Date: Fri, 29 Jan 2016 14:36:28 -0500 Subject: [PATCH 8/9] change to launch file --- launch/k2_client.launch | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/launch/k2_client.launch b/launch/k2_client.launch index 59fe1b4..9ca6a0d 100644 --- a/launch/k2_client.launch +++ b/launch/k2_client.launch @@ -6,11 +6,11 @@ - + - + From c05401f8818db83968ef34360fd9906271a66187 Mon Sep 17 00:00:00 2001 From: stepelle Date: Fri, 29 Jan 2016 14:37:53 -0500 Subject: [PATCH 9/9] change to launch file --- launch/k2_client.launch | 2 +- src/startBody.cpp | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/launch/k2_client.launch b/launch/k2_client.launch index 9ca6a0d..802817f 100644 --- a/launch/k2_client.launch +++ b/launch/k2_client.launch @@ -1,6 +1,6 @@ - + diff --git a/src/startBody.cpp b/src/startBody.cpp index 8143028..43efc2b 100644 --- a/src/startBody.cpp +++ b/src/startBody.cpp @@ -30,9 +30,6 @@ WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH #include std::string topicName = "bodyArray"; -// size_t streamSize = 56008; -// size_t readSkipSize = 56000; -// size_t stringSize = 28000; size_t streamSize = 80008; size_t readSkipSize = 80000; size_t stringSize = 40000;