Skip to content
GitLab
Commit 8277059c authored by levi's avatar levi
Browse files

Removed development logging

parent c83166e8
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
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment