Skip to content

Commit 399cdc6

Browse files
stepellestepelle
committed
Accepted
1 parent 1144322 commit 399cdc6

File tree

4 files changed

+74
-58
lines changed

4 files changed

+74
-58
lines changed

‎src/startBody.cpp‎

Lines changed: 35 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -30,31 +30,46 @@ WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
3030
#include <iconv.h>
3131

3232
std::string topicName = "bodyArray";
33-
size_t streamSize = 56008;
34-
size_t readSkipSize = 56000;
35-
size_t stringSize = 28000;
33+
int readSkipSize = 56000;
34+
int stringSize = 28000;
35+
int streamSize = readSkipSize + sizeof(double);
3636

3737
int main(int argC,char **argV)
3838
{
3939
ros::init(argC,argV,"startBody");
4040
ros::NodeHandle n;
4141
std::string serverAddress;
4242
n.getParam("/serverNameOrIP",serverAddress);
43-
Socket mySocket(serverAddress.c_str(),"9003",streamSize);
44-
iconv_t charConverter = iconv_open("UTF-8","UTF-16");
43+
Socket mySocket(serverAddress.c_str(),const_cast<char*>("9003"), streamSize);
4544
ros::Publisher bodyPub = n.advertise<k2_client::BodyArray>(topicName,1);
46-
char jsonCharArray[readSkipSize];
47-
45+
char utf16Array[readSkipSize];
46+
char jsonCharArray[stringSize];
47+
char *jsonCharArrayPtr;
48+
char *utf16ArrayPtr;
49+
ros::Rate r(30);
4850
while(ros::ok())
4951
{
50-
mySocket.readData();
51-
char *jsonCharArrayPtr;
52-
char *socketCharArrayPtr;
52+
mySocket.readData();
5353
jsonCharArrayPtr = jsonCharArray;
54-
socketCharArrayPtr = mySocket.mBuffer;
55-
iconv(charConverter,&socketCharArrayPtr,&readSkipSize,&jsonCharArrayPtr,&stringSize);
56-
double utcTime;
57-
memcpy(&utcTime,&mySocket.mBuffer[readSkipSize],sizeof(double));
54+
utf16ArrayPtr = utf16Array;
55+
memset(utf16Array, 0, sizeof(utf16Array));
56+
memcpy(utf16Array, mySocket.mBuffer, sizeof(utf16Array));
57+
size_t iconv_in = static_cast<size_t>(readSkipSize);
58+
size_t iconv_out = static_cast<size_t>(stringSize);
59+
iconv_t charConverter = iconv_open("UTF-8","UTF-16");
60+
size_t nconv = iconv(charConverter, &utf16ArrayPtr, &iconv_in, &jsonCharArrayPtr, &iconv_out);
61+
iconv_close(charConverter);
62+
if (nconv == (size_t) - 1)
63+
{
64+
if (errno == EINVAL)
65+
ROS_ERROR("An incomplete multibyte sequence has been encountered in the input.");
66+
else if (errno == EILSEQ)
67+
ROS_ERROR("An invalid multibyte sequence has been encountered in the input.");
68+
else
69+
ROS_ERROR("There is not sufficient room at jsonCharArray");
70+
}
71+
double utcTime = 0.0;
72+
memcpy(&utcTime,&mySocket.mBuffer[readSkipSize], sizeof(double));
5873
std::string jsonString(jsonCharArray);
5974
Json::Value jsonObject;
6075
Json::Reader jsonReader;
@@ -113,7 +128,7 @@ int main(int argC,char **argV)
113128
case 12: fieldName = "HipLeft";break;
114129
case 13: fieldName = "KneeLeft";break;
115130
case 14: fieldName = "AnkleLeft";break;
116-
case 15: fieldName = "FootLeft";break;
131+
case 15: fieldName = "SpineBase";break;
117132
case 16: fieldName = "HipRight";break;
118133
case 17: fieldName = "KneeRight";break;
119134
case 18: fieldName = "AnkleRight";break;
@@ -124,13 +139,13 @@ int main(int argC,char **argV)
124139
case 23: fieldName = "HandTipRight";break;
125140
case 24: fieldName = "ThumbRight";break;
126141
}
127-
142+
128143
JOAT.orientation.x = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["X"].asDouble();
129144
JOAT.orientation.y = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["Y"].asDouble();
130145
JOAT.orientation.z = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["Z"].asDouble();
131146
JOAT.orientation.w = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["W"].asDouble();
132147
JOAT.jointType = jsonObject[i]["JointOrientations"][fieldName]["JointType"].asInt();
133-
148+
134149
JPAS.trackingState = jsonObject[i]["Joints"][fieldName]["TrackingState"].asBool();
135150
JPAS.position.x = jsonObject[i]["Joints"][fieldName]["Position"]["X"].asDouble();
136151
JPAS.position.y = jsonObject[i]["Joints"][fieldName]["Position"]["Y"].asDouble();
@@ -149,6 +164,8 @@ int main(int argC,char **argV)
149164
continue;
150165
}
151166
bodyPub.publish(bodyArray);
167+
ros::spinOnce();
168+
r.sleep();
152169
}
153170
return 0;
154-
}
171+
}

‎src/startDepth.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ int main(int argC,char **argV)
4444
n.getParam("/serverNameOrIP",serverAddress);
4545
n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) +
4646
"/depth_frame", cameraFrame);
47-
Socket mySocket(serverAddress.c_str(),"9001",streamSize);
47+
Socket mySocket(serverAddress.c_str(),const_cast<char*>("9001"),streamSize);
4848
image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera(
4949
imageTopicSubName, 1);
5050
camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName);

‎src/startIR.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,7 @@ int main(int argC,char **argV)
4242
n.getParam("/serverNameOrIP",serverAddress);
4343
n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) +
4444
"/ir_frame", cameraFrame);
45-
Socket mySocket(serverAddress.c_str(),"9002",streamSize);
45+
Socket mySocket(serverAddress.c_str(),const_cast<char*>("9002"),streamSize);
4646
image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera(
4747
imageTopicSubName, 1);
4848
camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName);

‎src/startRGB.cpp‎

Lines changed: 37 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -25,51 +25,50 @@ BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
2525
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
2626
WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
2727
***************************************************************************************/
28-
#include "k2_client/k2_client.h"
28+
#include <k2_client/k2_client.h>
2929

30-
int imageSize = 6220800;
30+
int imageSize = 8294400;
3131
int streamSize = imageSize + sizeof(double);
3232
std::string cameraName = "rgb";
3333
std::string imageTopicSubName = "image_color";
34+
std::string cameraInfoSubName = "camera_info";
3435
std::string cameraFrame = "";
3536

3637
int main(int argC,char **argV)
3738
{
38-
ros::init(argC,argV,"startRGB");
39-
ros::NodeHandle n(cameraName);
40-
image_transport::ImageTransport imT(n);
41-
std::string serverAddress;
42-
n.getParam("/serverNameOrIP",serverAddress);
43-
n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) +
44-
"/rgb_frame", cameraFrame);
45-
Socket mySocket(serverAddress.c_str(),"9000",streamSize);
46-
image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera(
47-
imageTopicSubName, 1);
48-
camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName);
49-
camInfoMgr.loadCameraInfo("");
50-
cv::Mat frame;
51-
cv_bridge::CvImage cvImage;
52-
sensor_msgs::Image rosImage;
53-
while(ros::ok())
54-
{
55-
printf("Got a frame.\n");
56-
57-
mySocket.readData();
58-
printf("Creating mat.\n");
59-
frame = cv::Mat(cv::Size(1920,1080), CV_8UC3, mySocket.mBuffer);
60-
cv::flip(frame,frame,1);
61-
printf("Getting time.\n");
62-
double utcTime;
63-
memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double));
39+
ros::init(argC,argV,"startRGB");
40+
ros::NodeHandle n(cameraName);
41+
image_transport::ImageTransport imT(n);
42+
std::string serverAddress;
43+
n.getParam("/serverNameOrIP",serverAddress);
44+
n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) + "/rgb_frame", cameraFrame);
45+
Socket mySocket(serverAddress.c_str(),const_cast<char*>("9000"),streamSize);
46+
image_transport::Publisher imagePublisher = imT.advertise(imageTopicSubName, 1);
47+
ros::Publisher cameraInfoPub = n.advertise<sensor_msgs::CameraInfo>(cameraInfoSubName,1);
48+
camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName);
49+
camInfoMgr.loadCameraInfo("");
50+
cv::Mat frame;
51+
cv_bridge::CvImage cvImage;
52+
sensor_msgs::Image rosImage;
53+
while(ros::ok())
54+
{
55+
mySocket.readData();
56+
frame = cv::Mat(cv::Size(1920,1080), CV_8UC4, mySocket.mBuffer);
57+
cv::flip(frame,frame,1);
58+
double utcTime;
59+
memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double));
60+
cvImage.header.stamp = ros::Time(utcTime);
6461
cvImage.header.frame_id = cameraFrame.c_str();
65-
cvImage.encoding = "bgr8";
66-
cvImage.image = frame;
67-
cvImage.toImageMsg(rosImage);
68-
sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo();
69-
camInfo.header.frame_id = cvImage.header.frame_id;
62+
cvImage.encoding = "bgra8";
63+
cvImage.image = frame;
64+
cvImage.toImageMsg(rosImage);
65+
sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo();
66+
camInfo.header.stamp = cvImage.header.stamp;
67+
camInfo.header.frame_id = cvImage.header.frame_id;
68+
cameraInfoPub.publish(camInfo);
7069
printf("Updating.\n");
71-
cameraPublisher.publish(rosImage, camInfo, ros::Time(utcTime));
72-
ros::spinOnce();
73-
}
74-
return 0;
75-
}
70+
imagePublisher.publish(rosImage);
71+
ros::spinOnce();
72+
}
73+
return 0;
74+
}

0 commit comments

Comments
 (0)