Commit 8277059c authored by levi's avatar levi
Browse files

Removed development logging

parent c83166e8
...@@ -30,15 +30,6 @@ add_library(mm-utilscpp ...@@ -30,15 +30,6 @@ add_library(mm-utilscpp
) )
ament_target_dependencies(mm-utilscpp "rclcpp_lifecycle") 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 #Executeable dependencies
...@@ -46,9 +37,8 @@ add_executable(mm-controller ...@@ -46,9 +37,8 @@ add_executable(mm-controller
src/standalone_node.cpp src/standalone_node.cpp
src/mm_node.cpp src/mm_node.cpp
src/mm_api.c 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 ament_target_dependencies(mm-controller
"rclcpp" "rclcpp"
"rclcpp_lifecycle" "rclcpp_lifecycle"
......
...@@ -15,7 +15,7 @@ ...@@ -15,7 +15,7 @@
extern "C" extern "C"
{ {
#include "mm_controller/mm_api.h" #include "mm_controller/mm_api.h"
#include "mm_controller/mm_devices.h" //#include "mm_controller/mm_devices.h"
} }
namespace mm namespace mm
......
...@@ -120,8 +120,18 @@ namespace mm ...@@ -120,8 +120,18 @@ namespace mm
else else
{ {
RCLCPP_INFO(this->get_logger(), "Froze submap with id: %d", request->id); RCLCPP_INFO(this->get_logger(), "Froze submap with id: %d", request->id);
response->is_success = true; if (mmFreezeMap())
response->message = "Froze submap with id: " + std::to_string(request->id); {
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) 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 ...@@ -135,8 +145,17 @@ namespace mm
else else
{ {
RCLCPP_INFO(this->get_logger(), "Unfroze submap with id: %d", request->id); RCLCPP_INFO(this->get_logger(), "Unfroze submap with id: %d", request->id);
response->is_success = true; if (mmUnfreezeMap())
response->message = "Unfroze submap with id: " + std::to_string(request->id); {
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? // TODO: Wrapper function for sleep/wake device callback?
...@@ -292,23 +311,13 @@ namespace mm ...@@ -292,23 +311,13 @@ namespace mm
RCLCPP_WARN(this->get_logger(), "Failed to receive devices list information, from modem."); RCLCPP_WARN(this->get_logger(), "Failed to receive devices list information, from modem.");
return false; 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) if (newList.numDevices == 0)
{ {
RCLCPP_WARN(this->get_logger(), "No devices configurated for map on modem."); RCLCPP_WARN(this->get_logger(), "No devices configurated for map on modem.");
return false; return false;
} }
else // 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())
// { // {
// _statusMsg.devices.clear(); // _statusMsg.devices.clear();
// for (size_t i = 0; i < newList.numDevices; i++) // for (size_t i = 0; i < newList.numDevices; i++)
...@@ -318,15 +327,25 @@ namespace mm ...@@ -318,15 +327,25 @@ namespace mm
// RCLCPP_INFO(this->get_logger(), "New device list updated."); // RCLCPP_INFO(this->get_logger(), "New device list updated.");
// return true; // return true;
// } // }
// else if (newList.numDevices != _statusMsg.devices.size() || _statusMsg.devices.empty())
// { {
// for (size_t i = 0; i < newList.numDevices; i++) _statusMsg.devices.clear();
// { for (size_t i = 0; i < newList.numDevices; i++)
// _statusMsg.devices[i] = CreateDeviceStatus(&newList.devices[i]); {
// } _statusMsg.devices.push_back(CreateDeviceStatus(&newList.devices[i]));
// RCLCPP_INFO(this->get_logger(), "Updated devices list from modem."); }
// return true; 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) mm_controller_interfaces::msg::DeviceStatus MMNode::CreateDeviceStatus(MarvelmindDeviceInfo *dev)
{ {
...@@ -415,7 +434,7 @@ namespace mm ...@@ -415,7 +434,7 @@ namespace mm
void MMNode::GetSubmapSettings(uint8_t submapId) void MMNode::GetSubmapSettings(uint8_t submapId)
{ {
RCLCPP_INFO(this->get_logger(), "Submap START"); //RCLCPP_INFO(this->get_logger(), "Submap START");
MarvelmindSubmapSettings submapSettings; MarvelmindSubmapSettings submapSettings;
if (!mmGetSubmapSettings(submapId, &submapSettings)) if (!mmGetSubmapSettings(submapId, &submapSettings))
{ {
...@@ -425,7 +444,7 @@ namespace mm ...@@ -425,7 +444,7 @@ namespace mm
RCLCPP_INFO(this->get_logger(), "Starting Beacon: %d", submapSettings.startingBeacon); 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(), "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(), "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 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