Skip to content

Commit 411ca9f

Browse files
diebengaydiebengay
committed
Total Station client
1 parent 1c70c9e commit 411ca9f

File tree

7 files changed

+575
-0
lines changed

7 files changed

+575
-0
lines changed

‎total_station/.gitignore

+1
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
plans/

‎total_station/CMakeLists.txt

+205
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,205 @@
1+
cmake_minimum_required(VERSION 3.0.2)
2+
project(total_station)
3+
4+
## Compile as C++11, supported in ROS Kinetic and newer
5+
add_compile_options(-std=c++11)
6+
7+
## Find catkin macros and libraries
8+
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
9+
## is used, also find other catkin packages
10+
find_package(catkin REQUIRED COMPONENTS
11+
geometry_msgs
12+
roscpp
13+
)
14+
15+
## System dependencies are found with CMake's conventions
16+
# find_package(Boost REQUIRED COMPONENTS system)
17+
18+
19+
## Uncomment this if the package has a setup.py. This macro ensures
20+
## modules and global scripts declared therein get installed
21+
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
22+
# catkin_python_setup()
23+
24+
################################################
25+
## Declare ROS messages, services and actions ##
26+
################################################
27+
28+
## To declare and build messages, services or actions from within this
29+
## package, follow these steps:
30+
## * Let MSG_DEP_SET be the set of packages whose message types you use in
31+
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
32+
## * In the file package.xml:
33+
## * add a build_depend tag for "message_generation"
34+
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
35+
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
36+
## but can be declared for certainty nonetheless:
37+
## * add a exec_depend tag for "message_runtime"
38+
## * In this file (CMakeLists.txt):
39+
## * add "message_generation" and every package in MSG_DEP_SET to
40+
## find_package(catkin REQUIRED COMPONENTS ...)
41+
## * add "message_runtime" and every package in MSG_DEP_SET to
42+
## catkin_package(CATKIN_DEPENDS ...)
43+
## * uncomment the add_*_files sections below as needed
44+
## and list every .msg/.srv/.action file to be processed
45+
## * uncomment the generate_messages entry below
46+
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
47+
48+
## Generate messages in the 'msg' folder
49+
# add_message_files(
50+
# FILES
51+
# Message1.msg
52+
# Message2.msg
53+
# )
54+
55+
## Generate services in the 'srv' folder
56+
# add_service_files(
57+
# FILES
58+
# Service1.srv
59+
# Service2.srv
60+
# )
61+
62+
## Generate actions in the 'action' folder
63+
# add_action_files(
64+
# FILES
65+
# Action1.action
66+
# Action2.action
67+
# )
68+
69+
## Generate added messages and services with any dependencies listed here
70+
# generate_messages(
71+
# DEPENDENCIES
72+
# geometry_msgs
73+
# )
74+
75+
################################################
76+
## Declare ROS dynamic reconfigure parameters ##
77+
################################################
78+
79+
## To declare and build dynamic reconfigure parameters within this
80+
## package, follow these steps:
81+
## * In the file package.xml:
82+
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
83+
## * In this file (CMakeLists.txt):
84+
## * add "dynamic_reconfigure" to
85+
## find_package(catkin REQUIRED COMPONENTS ...)
86+
## * uncomment the "generate_dynamic_reconfigure_options" section below
87+
## and list every .cfg file to be processed
88+
89+
## Generate dynamic reconfigure parameters in the 'cfg' folder
90+
# generate_dynamic_reconfigure_options(
91+
# cfg/DynReconf1.cfg
92+
# cfg/DynReconf2.cfg
93+
# )
94+
95+
###################################
96+
## catkin specific configuration ##
97+
###################################
98+
## The catkin_package macro generates cmake config files for your package
99+
## Declare things to be passed to dependent projects
100+
## INCLUDE_DIRS: uncomment this if your package contains header files
101+
## LIBRARIES: libraries you create in this project that dependent projects also need
102+
## CATKIN_DEPENDS: catkin_packages dependent projects also need
103+
## DEPENDS: system dependencies of this project that dependent projects also need
104+
catkin_package(
105+
# INCLUDE_DIRS include
106+
# LIBRARIES total_station_client
107+
# CATKIN_DEPENDS geometry_msgs roscpp
108+
# DEPENDS system_lib
109+
)
110+
111+
###########
112+
## Build ##
113+
###########
114+
115+
## Specify additional locations of header files
116+
## Your package locations should be listed before other locations
117+
include_directories(
118+
include
119+
${catkin_INCLUDE_DIRS}
120+
)
121+
122+
## Declare a C++ library
123+
# add_library(${PROJECT_NAME}
124+
# src/${PROJECT_NAME}/total_station_client.cpp
125+
# )
126+
127+
## Add cmake target dependencies of the library
128+
## as an example, code may need to be generated before libraries
129+
## either from message generation or dynamic reconfigure
130+
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
131+
132+
## Declare a C++ executable
133+
## With catkin_make all packages are built within a single CMake context
134+
## The recommended prefix ensures that target names across packages don't collide
135+
add_executable(ts_client_node src/ts_client_node.cpp)
136+
137+
## Rename C++ executable without prefix
138+
## The above recommended prefix causes long target names, the following renames the
139+
## target back to the shorter version for ease of user use
140+
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
141+
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
142+
143+
## Add cmake target dependencies of the executable
144+
## same as for the library above
145+
add_dependencies(ts_client_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
146+
147+
## Specify libraries to link a library or executable target against
148+
target_link_libraries(ts_client_node
149+
${catkin_LIBRARIES}
150+
)
151+
152+
#############
153+
## Install ##
154+
#############
155+
156+
# all install targets should use catkin DESTINATION variables
157+
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
158+
159+
## Mark executable scripts (Python etc.) for installation
160+
## in contrast to setup.py, you can choose the destination
161+
# catkin_install_python(PROGRAMS
162+
# scripts/my_python_script
163+
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
164+
# )
165+
166+
## Mark executables for installation
167+
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
168+
# install(TARGETS ${PROJECT_NAME}_node
169+
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
170+
# )
171+
172+
## Mark libraries for installation
173+
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
174+
# install(TARGETS ${PROJECT_NAME}
175+
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
176+
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
177+
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
178+
# )
179+
180+
## Mark cpp header files for installation
181+
# install(DIRECTORY include/${PROJECT_NAME}/
182+
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
183+
# FILES_MATCHING PATTERN "*.h"
184+
# PATTERN ".svn" EXCLUDE
185+
# )
186+
187+
## Mark other files for installation (e.g. launch and bag files, etc.)
188+
# install(FILES
189+
# # myfile1
190+
# # myfile2
191+
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
192+
# )
193+
194+
#############
195+
## Testing ##
196+
#############
197+
198+
## Add gtest based cpp test target and link libraries
199+
# catkin_add_gtest(${PROJECT_NAME}-test test/test_total_station_client.cpp)
200+
# if(TARGET ${PROJECT_NAME}-test)
201+
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
202+
# endif()
203+
204+
## Add folders to be run by python nosetests
205+
# catkin_add_nosetests(test)

‎total_station/include/total_station/ts_client.h

+173
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,173 @@
1+
//----------------------------------------------------------------------------------------------------------------------
2+
// Total Station Client
3+
//----------------------------------------------------------------------------------------------------------------------
4+
// The MIT License (MIT)
5+
//
6+
// Copyright (c) 2022 GRVC University of Seville
7+
//
8+
// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated
9+
// documentation files (the "Software"), to deal in the Software without restriction, including without limitation the
10+
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
11+
// permit persons to whom the Software is furnished to do so, subject to the following conditions:
12+
//
13+
// The above copyright notice and this permission notice shall be included in all copies or substantial portions of the
14+
// Software.
15+
//
16+
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
17+
// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS
18+
// OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
19+
// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
20+
//----------------------------------------------------------------------------------------------------------------------
21+
22+
#ifndef _TS_CLIENT_H_
23+
#define _TS_CLIENT_H_
24+
25+
#include <sys/socket.h>
26+
#include <arpa/inet.h>
27+
#include <string>
28+
29+
#include <ros/ros.h>
30+
#include <geometry_msgs/PoseStamped.h>
31+
32+
/**
33+
* @brief Total Station server response message
34+
*
35+
*/
36+
struct {
37+
float x;
38+
float y;
39+
float z;
40+
uint64_t timestamp;
41+
} typedef ts_msg_t;
42+
43+
44+
/**
45+
* @brief Total Station client class
46+
*
47+
*/
48+
class TotalStationClient {
49+
private:
50+
/// ROS node handler
51+
ros::NodeHandle n_;
52+
53+
/// Publisher
54+
ros::Publisher pub_;
55+
56+
/// Address to send the data
57+
struct sockaddr_in remote_addr_;
58+
59+
/// Socket file descriptor
60+
int sock_;
61+
62+
/// Total Station server port
63+
int ts_server_port_;
64+
65+
/// Total Station server IP
66+
std::string ts_server_ip_;
67+
68+
/// Publishing topic name
69+
std::string pub_topic_name_;
70+
71+
public:
72+
73+
TotalStationClient():
74+
remote_addr_({0}),
75+
sock_(-1)
76+
{
77+
ros::param::param<std::string>("~topic_name", pub_topic_name_, "leica/pose");
78+
ros::param::param<std::string>("~ts_server_ip", ts_server_ip_, "127.0.0.1");
79+
ros::param::param<int>("~ts_server_port", ts_server_port_, 8000);
80+
}
81+
82+
83+
/**
84+
* @brief Total Station initialization method
85+
*
86+
*/
87+
void init() {
88+
89+
ROS_INFO("Waiting for Total Station server");
90+
91+
92+
// Creates address to send the data
93+
remote_addr_.sin_family = AF_INET;
94+
remote_addr_.sin_port = htons(ts_server_port_);
95+
remote_addr_.sin_addr.s_addr = inet_addr(ts_server_ip_.c_str());
96+
97+
// Creates the socket
98+
sock_ = socket(AF_INET, SOCK_DGRAM, 0);
99+
if (sock_ == -1) {
100+
ROS_ERROR("Total Station client socket creation error");
101+
exit(EXIT_FAILURE);
102+
}
103+
104+
// Sets socket timeout 500ms
105+
struct timeval sock_timeo_500ms = {0};
106+
sock_timeo_500ms.tv_sec = 1;
107+
sock_timeo_500ms.tv_usec = 0;
108+
setsockopt(sock_, SOL_SOCKET, SO_RCVTIMEO, &sock_timeo_500ms, sizeof(sock_timeo_500ms));
109+
110+
111+
// Waits until Total Station server is available for 10s
112+
ts_msg_t msg = {0};
113+
ssize_t n = -1;
114+
while (ros::ok()) {
115+
// Send a message to total station server to start sending us data
116+
sendto(sock_, "Hello there!", 13, 0, (const struct sockaddr *)&remote_addr_, sizeof(remote_addr_));
117+
// Waits for response
118+
if((n = recv(sock_, &msg, sizeof(ts_msg_t), 0)) > 0) break;
119+
// ROS_WARN("No Total Station server response. Retrying...");
120+
}
121+
122+
pub_ = n_.advertise<geometry_msgs::PoseStamped>(pub_topic_name_, 1);
123+
}
124+
125+
126+
127+
/**
128+
* @brief Total Station start method
129+
*
130+
*/
131+
void start() {
132+
ROS_INFO("Starting Total Station loop");
133+
134+
ts_msg_t msg = {0};
135+
uint64_t timestamp_a = 0;
136+
137+
// Main loop
138+
while (ros::ok()) {
139+
ssize_t n = recv(sock_, &msg, sizeof(msg), 0);
140+
141+
// If not data received sends a message to the server to tell it we are still alive
142+
if(n < 0) {
143+
ROS_WARN("No Total Station server response. Retrying...");
144+
sendto(sock_, "I'm still alive!", 17, 0, (const struct sockaddr *)&remote_addr_, sizeof(remote_addr_));
145+
continue;
146+
}
147+
148+
// Checks if the received data is the same as the previous one or is not
149+
// valid. When something goes wrong, TS server sends {0,0,0,0}.
150+
if (msg.timestamp == 0 || msg.timestamp == timestamp_a) continue;
151+
timestamp_a = msg.timestamp;
152+
153+
geometry_msgs::PoseStamped data;
154+
data.header.frame_id = "map";
155+
data.header.stamp = ros::Time::now();
156+
157+
data.pose.position.x = msg.x;
158+
data.pose.position.y = msg.y;
159+
data.pose.position.z = msg.z;
160+
161+
data.pose.orientation.w = 1;
162+
data.pose.orientation.x = 0;
163+
data.pose.orientation.y = 0;
164+
data.pose.orientation.z = 0;
165+
166+
pub_.publish(data);
167+
}
168+
169+
close(sock_);
170+
}
171+
};
172+
173+
#endif // _TS_CLIENT_H_

‎total_station/launch/ts_client.launch

+13
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
<launch>
2+
<arg name="topic_name" default="leica/pose"/>
3+
<arg name="ts_server_ip" default="192.168.0.188"/>
4+
<arg name="ts_server_port" default="8000"/>
5+
6+
7+
<node name="totalStationClientNode" pkg="total_station" type="ts_client_node" output="screen">
8+
<param name="topic_name" type="str" value="$(arg topic_name)"/>
9+
<param name="ts_server_ip" type="str" value="$(arg ts_server_ip)"/>
10+
<param name="ts_server_port" type="int" value="$(arg ts_server_port)"/>
11+
</node>
12+
13+
</launch>

0 commit comments

Comments
 (0)