Skip to content
GitLab
Menu
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
desktop_robotti
drobotti-processing-and-control
Commits
8277059c
Commit
8277059c
authored
Nov 03, 2021
by
levi
Browse files
Removed development logging
parent
c83166e8
Changes
3
Hide whitespace changes
Inline
Side-by-side
src/mm_controller/CMakeLists.txt
View file @
8277059c
...
...
@@ -30,15 +30,6 @@ add_library(mm-utilscpp
)
ament_target_dependencies
(
mm-utilscpp
"rclcpp_lifecycle"
)
add_library
(
mm-utilsc
src/mm_utils.c
)
add_library
(
mm-devices
src/mm_devices.c
)
#Executeable dependencies
...
...
@@ -46,9 +37,8 @@ add_executable(mm-controller
src/standalone_node.cpp
src/mm_node.cpp
src/mm_api.c
src/mm_devices.c
)
target_link_libraries
(
mm-controller mm-utilscpp
mm-devices mm-utilsc
)
target_link_libraries
(
mm-controller mm-utilscpp
)
ament_target_dependencies
(
mm-controller
"rclcpp"
"rclcpp_lifecycle"
...
...
src/mm_controller/include/mm_controller/mm_node.hpp
View file @
8277059c
...
...
@@ -15,7 +15,7 @@
extern
"C"
{
#include
"mm_controller/mm_api.h"
#include
"mm_controller/mm_devices.h"
//
#include "mm_controller/mm_devices.h"
}
namespace
mm
...
...
src/mm_controller/src/mm_node.cpp
View file @
8277059c
...
...
@@ -120,8 +120,18 @@ namespace mm
else
{
RCLCPP_INFO
(
this
->
get_logger
(),
"Froze submap with id: %d"
,
request
->
id
);
response
->
is_success
=
true
;
response
->
message
=
"Froze submap with id: "
+
std
::
to_string
(
request
->
id
);
if
(
mmFreezeMap
())
{
RCLCPP_INFO
(
this
->
get_logger
(),
"Froze map"
,
request
->
id
);
response
->
is_success
=
true
;
response
->
message
=
"Froze submap with id: "
+
std
::
to_string
(
request
->
id
)
+
", and froze map."
;
}
else
{
RCLCPP_INFO
(
this
->
get_logger
(),
"Failed to freeze map"
,
request
->
id
);
response
->
is_success
=
false
;
response
->
message
=
"Froze submap with id: "
+
std
::
to_string
(
request
->
id
)
+
", but failed to freeze map"
;
}
}
}
void
MMNode
::
UnfreezeMap
(
const
std
::
shared_ptr
<
mm_controller_interfaces
::
srv
::
NodeExecution
::
Request
>
request
,
std
::
shared_ptr
<
mm_controller_interfaces
::
srv
::
NodeExecution
::
Response
>
response
)
...
...
@@ -135,8 +145,17 @@ namespace mm
else
{
RCLCPP_INFO
(
this
->
get_logger
(),
"Unfroze submap with id: %d"
,
request
->
id
);
response
->
is_success
=
true
;
response
->
message
=
"Unfroze submap with id: "
+
std
::
to_string
(
request
->
id
);
if
(
mmUnfreezeMap
())
{
RCLCPP_INFO
(
this
->
get_logger
(),
"Unfroze map"
,
request
->
id
);
response
->
is_success
=
true
;
response
->
message
=
"Unfroze submap with id: "
+
std
::
to_string
(
request
->
id
)
+
", and unfroze the map."
;
}
else
{
response
->
is_success
=
false
;
response
->
message
=
"Unfroze submap with id: "
+
std
::
to_string
(
request
->
id
)
+
", but failed to unfreeze map."
;
}
}
}
// TODO: Wrapper function for sleep/wake device callback?
...
...
@@ -292,23 +311,13 @@ namespace mm
RCLCPP_WARN
(
this
->
get_logger
(),
"Failed to receive devices list information, from modem."
);
return
false
;
}
RCLCPP_INFO
(
this
->
get_logger
(),
"newList.numDevices: %d _statusMsg.devices.size(): %d "
,
newList
.
numDevices
,
_statusMsg
.
devices
.
size
());
//
RCLCPP_INFO(this->get_logger(), "newList.numDevices: %d _statusMsg.devices.size(): %d ", newList.numDevices, _statusMsg.devices.size());
if
(
newList
.
numDevices
==
0
)
{
RCLCPP_WARN
(
this
->
get_logger
(),
"No devices configurated for map on modem."
);
return
false
;
}
else
{
_statusMsg
.
devices
.
clear
();
for
(
size_t
i
=
0
;
i
<
newList
.
numDevices
;
i
++
)
{
_statusMsg
.
devices
.
push_back
(
CreateDeviceStatus
(
&
newList
.
devices
[
i
]));
}
RCLCPP_INFO
(
this
->
get_logger
(),
"New device list updated."
);
return
true
;
}
// if (newList.numDevices != _statusMsg.devices.size() || _statusMsg.devices.empty())
// else
// {
// _statusMsg.devices.clear();
// for (size_t i = 0; i < newList.numDevices; i++)
...
...
@@ -318,15 +327,25 @@ namespace mm
// RCLCPP_INFO(this->get_logger(), "New device list updated.");
// return true;
// }
// else
// {
// for (size_t i = 0; i < newList.numDevices; i++)
// {
// _statusMsg.devices[i] = CreateDeviceStatus(&newList.devices[i]);
// }
// RCLCPP_INFO(this->get_logger(), "Updated devices list from modem.");
// return true;
// }
if
(
newList
.
numDevices
!=
_statusMsg
.
devices
.
size
()
||
_statusMsg
.
devices
.
empty
())
{
_statusMsg
.
devices
.
clear
();
for
(
size_t
i
=
0
;
i
<
newList
.
numDevices
;
i
++
)
{
_statusMsg
.
devices
.
push_back
(
CreateDeviceStatus
(
&
newList
.
devices
[
i
]));
}
RCLCPP_INFO
(
this
->
get_logger
(),
"New device list updated."
);
return
true
;
}
else
{
for
(
size_t
i
=
0
;
i
<
newList
.
numDevices
;
i
++
)
{
_statusMsg
.
devices
[
i
]
=
CreateDeviceStatus
(
&
newList
.
devices
[
i
]);
}
//RCLCPP_INFO(this->get_logger(), "Updated devices list from modem.");
return
true
;
}
}
mm_controller_interfaces
::
msg
::
DeviceStatus
MMNode
::
CreateDeviceStatus
(
MarvelmindDeviceInfo
*
dev
)
{
...
...
@@ -415,7 +434,7 @@ namespace mm
void
MMNode
::
GetSubmapSettings
(
uint8_t
submapId
)
{
RCLCPP_INFO
(
this
->
get_logger
(),
"Submap START"
);
//
RCLCPP_INFO(this->get_logger(), "Submap START");
MarvelmindSubmapSettings
submapSettings
;
if
(
!
mmGetSubmapSettings
(
submapId
,
&
submapSettings
))
{
...
...
@@ -425,7 +444,7 @@ namespace mm
RCLCPP_INFO
(
this
->
get_logger
(),
"Starting Beacon: %d"
,
submapSettings
.
startingBeacon
);
RCLCPP_INFO
(
this
->
get_logger
(),
"Starting set of beacons, %d, %d, %d, %d"
,
submapSettings
.
startingSet_1
,
submapSettings
.
startingSet_2
,
submapSettings
.
startingSet_3
,
submapSettings
.
startingSet_4
);
RCLCPP_INFO
(
this
->
get_logger
(),
"frozen: %d locked: %d"
,
submapSettings
.
frozen
,
submapSettings
.
locked
);
RCLCPP_INFO
(
this
->
get_logger
(),
"Submap END"
);
//
RCLCPP_INFO(this->get_logger(), "Submap END");
}
rclcpp_lifecycle
::
node_interfaces
::
LifecycleNodeInterface
::
CallbackReturn
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment