Commit 55ff8d77 authored by levi's avatar levi
Browse files

Added mm base..

parent a2fdfeff
......@@ -17,18 +17,45 @@ find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(lifecycle_msgs REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(mm_controller_interfaces REQUIRED)
find_package(geometry_msgs REQUIRED)
include_directories(
include
)
# Node exe build
add_executable(mm-controller src/standalone_node.cpp src/mm_node.cpp src/mm_api.c)
# Utils library
add_library(mm-utilscpp
src/mm_utils.cpp
)
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
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)
ament_target_dependencies(mm-controller
"rclcpp"
"rclcpp_lifecycle"
mm_controller_interfaces
"geometry_msgs"
)
#Build
install(TARGETS
mm-controller
DESTINATION lib/${PROJECT_NAME})
......
marvelmind_controller:
ros__parameters:
device: "/dev/ttyACM0" #Do we need to handle windows COM?
test: "aaaaaaaaaaaaaaaaaaa"
statusPublisherTopicName: "status"
robotBeaconPublisherTopicName: "position"
wakeDeviceServiceName: "wake_device"
sleepDeviceServiceName: "sleep_device"
servicePrefixName: "marvelmind_controller"
......@@ -241,7 +241,7 @@ bool mmDeviceIsModem(uint8_t deviceType);
bool mmDeviceIsBeacon(uint8_t deviceType);
bool mmDeviceIsHedgehog(uint8_t deviceType);
void marvelmindAPILoad();
bool marvelmindAPILoad();
void marvelmindAPIFree();
#endif // __MM_API_H_
#ifndef __MM_DEVICES_H_
#define __MM_DEVICES_H_
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <stdint.h>
#include "mm_controller/mm_api.h"
#include "mm_controller/mm_utils.h"
typedef struct
{
uint32_t distance_mm;
} MarvelmindDevDistance;
typedef struct
{
MarvelmindDeviceInfo info;
MarvelmindDeviceVersion version;
bool devConnected;
MMDeviceType deviceType;
#if MM_LOCATIONS_VERSION == 1
MarvelmindDeviceLocation pos;
#endif // MM_LOCATIONS_VERSION
#if MM_LOCATIONS_VERSION == 2
MarvelmindDeviceLocation2 pos;
#endif // MM_LOCATIONS_VERSION
MarvelmindDevDistance distances[MM_MAX_DEVICES_COUNT];
} MarvelmindDevice;
// Read Marvelmind devices list from modem
void marvelmindDevicesReadIfNeeded(); //Move to node
// Get device data structure
MarvelmindDevice *getMarvelmindDevice(uint8_t address);
#if MM_LOCATIONS_VERSION == 1
// Update device location
MarvelmindDevice *marvelmindUpdateLocation(uint8_t address, MarvelmindDeviceLocation *ppos);
#endif
#if MM_LOCATIONS_VERSION == 2
// Update device location
MarvelmindDevice *marvelmindUpdateLocation(uint8_t address, MarvelmindDeviceLocation2 *ppos);
#endif
// Update distance to device
MarvelmindDevice *marvelmindUpdateDistance(uint8_t addressRx, uint8_t addressTx, uint32_t d);
// Initialize structure of Marvelmind devices
void initMarvelmindDevicesList(); //move to node
#endif // __MARVELMIND_DEVICES_H_
......@@ -5,30 +5,73 @@
#include <rclcpp/publisher.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <rclcpp_lifecycle/lifecycle_publisher.hpp>
#include "mm_controller_interfaces/msg/mm_controller_status.hpp"
#include "mm_controller_interfaces/srv/power_device.hpp"
#include <chrono>
#include <string>
#include <memory>
#include <geometry_msgs/msg/point.hpp>
#ifdef __cplusplus
extern "C"
{
#endif
#include "mm_controller/mm_api.h"
#ifdef __cplusplus
#include "mm_controller/mm_devices.h"
}
#endif
namespace mm
{
class MMNode : public rclcpp_lifecycle::LifecycleNode
{
public:
explicit MMNode(const std::string &node_name, bool intra_process_comms = false);
~MMNode();
//API library and device connection initialiser.
bool Start();
//Node Lifecycle callbacks
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State &);
//rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &);
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &);
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State &);
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &);
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_error(const rclcpp_lifecycle::State &);
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_exit(const rclcpp_lifecycle::State &);
private:
//Timer/Service callbacks
void StatusPublisherCallback();
void WakeDeviceCallback(const std::shared_ptr<mm_controller_interfaces::srv::PowerDevice::Request> request, std::shared_ptr<mm_controller_interfaces::srv::PowerDevice::Response> response);
void SleepDeviceCallback(const std::shared_ptr<mm_controller_interfaces::srv::PowerDevice::Request> request, std::shared_ptr<mm_controller_interfaces::srv::PowerDevice::Response> response);
rclcpp::Service<mm_controller_interfaces::srv::PowerDevice>::SharedPtr _sleepDeviceSrvPtr;
rclcpp::Service<mm_controller_interfaces::srv::PowerDevice>::SharedPtr _wakeDeviceSrvPtr;
//lifecycle test objects
rclcpp_lifecycle::State state;
//rclcpp_lifecycle::Transition transition;
//API helper functions
bool GetDevicesListFromModem();
//Publisher Helper functions
mm_controller_interfaces::msg::DeviceStatus CreateDeviceStatus(MarvelmindDeviceInfo *dev);
//API objects
MarvelmindDeviceVersion _usbDeviceVersion;
//Node publishers, timers and messages..
rclcpp_lifecycle::LifecyclePublisher<mm_controller_interfaces::msg::MMControllerStatus>::SharedPtr _statusPubPtr;
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Point>::SharedPtr _positionPubPtr;
rclcpp::TimerBase::SharedPtr _statusPubTimerPtr;
mm_controller_interfaces::msg::MMControllerStatus _statusMsg;
int _testCounter = 0;
//Config Parameters
std::string _paramDevice;
std::string _paramStatusPublisherTopicName;
std::string _paramRobotBeaconPublisherTopicName;
std::string _paramWakeDeviceServiceName;
std::string _paramSleepDeviceServiceName;
std::string _paramServicePrefixName;
};
}
......
#ifndef __MM_UTILS_H_
#define __MM_UTILS_H_
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <stdint.h>
#include "mm_controller/mm_api.h"
typedef enum
{
modem,
beacon,
hedgehog,
unknown
} MMDeviceType;
void printBoolEnabled(char *prefix, bool v);
int boolAsInt(bool v);
void sleep_ms(int ms);
void trim(char *const a);
MMDeviceType getMMDeviceType(uint8_t deviceType);
void printMMDeviceVersionAndId(MarvelmindDeviceVersion *dv);
void printMMDeviceType(MMDeviceType *dt);
#ifndef WIN32
double getPassedTime(struct timespec *t1, struct timespec *t2);
#endif
#endif // __MM_UTILS_H_
#ifndef __MM_UTILS_HPP_
#define __MM_UTILS_HPP_
#include <string.h>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <iomanip>
extern "C"
{
#include <mm_controller/mm_api.h>
}
namespace mm
{
enum MMDeviceType
{
modem,
beacon,
hedgehog,
unknown
};
std::string GetDeviceModelFromDeviceID(const uint8_t dev);
std::string ComputeFirmwareString(const uint8_t major, const uint8_t minor, const uint8_t minor2);
void PrintDeviceInformation(const rclcpp::Logger logger, const MarvelmindDeviceVersion dev);
template <typename T>
std::string int_to_hex(T i);
std::string GenerateExecutionResult(bool executionSucces, std::string msg);
}
#endif // __MM_UTILS_HPP_
......@@ -9,6 +9,8 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>mm_controller_interfaces</depend>
<build_depend>lifecycle_msgs</build_depend>
<build_depend>rclcpp_lifecycle</build_depend>
<build_depend>std_msgs</build_depend>
......
......@@ -802,11 +802,14 @@ bool mmDeviceIsHedgehog(uint8_t deviceType)
//////
void marvelmindAPILoad()
bool marvelmindAPILoad()
{
#ifdef WIN32
mmLibrary = LoadLibrary("dashapi.dll");
if (mmLibrary == NULL)
{
return false;
}
pmm_api_version = (pt_mm_api_version)GetProcAddress(mmLibrary, "mm_api_version");
pmm_open_port = (pt_mm_open_port)GetProcAddress(mmLibrary, "mm_open_port");
......@@ -862,7 +865,10 @@ void marvelmindAPILoad()
#else
// not WIN32
mmLibrary = dlopen("libdashapi.so", RTLD_LAZY);
if (mmLibrary == NULL)
{
return false;
}
pmm_api_version = dlsym(mmLibrary, "mm_api_version");
pmm_open_port = dlsym(mmLibrary, "mm_open_port");
......@@ -915,10 +921,16 @@ void marvelmindAPILoad()
pmm_device_is_beacon = dlsym(mmLibrary, "mm_device_is_beacon");
pmm_device_is_hedgehog = dlsym(mmLibrary, "mm_device_is_hedgehog");
#endif
return true;
}
void marvelmindAPIFree()
{
if (mmLibrary == NULL)
{
return;
}
#ifdef WIN32
FreeLibrary(mmLibrary);
#else
......
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include "mm_controller/mm_devices.h"
typedef struct
{
uint8_t numDevices;
MarvelmindDevice devices[MM_MAX_DEVICES_COUNT];
} MarvelmindDevicesListExt;
static MarvelmindDevicesListExt mmDevList;
#ifdef WIN32
static clock_t prevReadTime;
#else
static struct timespec prevReadTime;
#endif
// Returns true if devices info is same
static bool sameDevice(MarvelmindDeviceInfo *dev1, MarvelmindDeviceInfo *dev2)
{
if (dev1->address != dev2->address)
return false;
if (dev1->isDuplicatedAddress != dev2->isDuplicatedAddress)
return false;
if (dev1->isSleeping != dev2->isSleeping)
return false;
if (dev1->fwVerMajor != dev2->fwVerMajor)
return false;
if (dev1->fwVerMinor != dev2->fwVerMinor)
return false;
if (dev1->fwVerMinor2 != dev2->fwVerMinor2)
return false;
if (dev1->fwVerDeviceType != dev2->fwVerDeviceType)
return false;
if (dev1->fwOptions != dev2->fwOptions)
return false;
if (dev1->flags != dev2->flags)
return false;
return true;
}
// Update device in list
static void updateDevice(uint8_t index, MarvelmindDeviceInfo *info)
{
bool connected = ((info->flags & 0x01) != 0);
mmDevList.devices[index].devConnected = connected;
printf("Device %d updated\r\n", (int)info->address);
if (connected)
{
MarvelmindDeviceVersion version;
if (!mmGetVersionAndId(info->address, &version))
{
printf("Failed read version of device: %d\r\n", (int)info->address);
return;
}
mmDevList.devices[index].version = version;
printMMDeviceVersionAndId(&version);
mmDevList.devices[index].deviceType = getMMDeviceType(version.fwVerDeviceType);
printMMDeviceType(&mmDevList.devices[index].deviceType);
}
else
{
if (info->isSleeping)
{
printf("Device %d is sleeping\r\n", (int)info->address);
}
else
{
printf("Device %d connecting...\r\n", (int)info->address);
}
}
mmDevList.devices[index].info = *info;
}
// Remove device from list
static void removeDevice(uint8_t index)
{
if (mmDevList.numDevices == 0)
return;
printf("Device updated: %d\r\n", (int)mmDevList.devices[index].info.address);
uint8_t i;
mmDevList.numDevices--;
if (mmDevList.numDevices > 0)
{
for (i = index; i < mmDevList.numDevices; i++)
{
mmDevList.devices[i] = mmDevList.devices[i + 1];
}
}
}
// Add device to list
static void addDevice(MarvelmindDeviceInfo *info)
{
if (mmDevList.numDevices >= MM_MAX_DEVICES_COUNT)
return;
updateDevice(mmDevList.numDevices, info);
printf("Device added: %d\r\n", (int)info->address);
#if MM_LOCATIONS_VERSION == 1
MarvelmindDeviceLocation *ppos = &mmDevList.devices[mmDevList.numDevices].pos;
ppos->x_mm = 0;
ppos->y_mm = 0;
ppos->z_mm = 0;
#endif
#if MM_LOCATIONS_VERSION == 2
MarvelmindDeviceLocation2 *ppos = &mmDevList.devices[mmDevList.numDevices].pos;
ppos->x_mm = 0;
ppos->y_mm = 0;
ppos->z_mm = 0;
#endif
mmDevList.numDevices++;
}
// Remove devices not present in new list
static void removeAbsentDevices(MarvelmindDevicesList *pNewList)
{
bool cont;
uint8_t i, j;
for (i = 0; i < mmDevList.numDevices; i++)
{
uint8_t address = mmDevList.devices[i].info.address;
MarvelmindDeviceInfo *info_i = &mmDevList.devices[i].info;
cont = false;
for (j = 0; j < pNewList->numDevices; j++)
{
if (sameDevice(info_i, &pNewList->devices[j]))
{
cont = true;
break;
}
if (address == pNewList->devices[j].address)
{
updateDevice(i, &pNewList->devices[j]);
cont = true;
break;
}
} //for j
if (cont)
continue;
// device not found in new list
removeDevice(i);
} //for i
}
// Add new devices from new list
static void addNewDevices(MarvelmindDevicesList *pNewList)
{
bool cont;
uint8_t i, j;
for (i = 0; i < pNewList->numDevices; i++)
{
uint8_t address = pNewList->devices[i].address;
MarvelmindDeviceInfo *info_i = &pNewList->devices[i];
cont = false;
for (j = 0; j < mmDevList.numDevices; j++)
{
if (sameDevice(info_i, &mmDevList.devices[j].info))
{
cont = true;
break;
}
if (address == mmDevList.devices[j].info.address)
{
updateDevice(j, info_i);
cont = true;
break;
}
} //for j
if (cont)
continue;
// device not found in current list
addDevice(info_i);
} //for i
}
// check lists identity
static bool checkDevicesList(MarvelmindDevicesList *pNewList)
{
uint8_t n = pNewList->numDevices;
if (n == 0)
{
return true;
}
uint8_t i;
for (i = 0; i < n; i++)
{
if (!sameDevice(&pNewList->devices[i], &mmDevList.devices[i].info))
{
if (pNewList->devices[i].address == mmDevList.devices[i].info.address)
{
updateDevice(i, &pNewList->devices[i]);
}
else
{
return false;
}
}
} //for i
return true;
}
// Read Marvelmind devices list from modem
void marvelmindDevicesReadIfNeeded()
{
#ifdef WIN32
clock_t curTime = clock();
double passedSec = ((double)(curTime - prevReadTime)) / CLOCKS_PER_SEC;
#else
struct timespec curTime;
clock_gettime(CLOCK_REALTIME, &curTime);
double passedSec = getPassedTime(&prevReadTime, &curTime);
#endif
if (passedSec < 1.0)
{
return;
}
prevReadTime = curTime;
MarvelmindDevicesList newList;
if (!mmGetDevicesList(&newList))
{
return; // failed read
}
if (newList.numDevices == mmDevList.numDevices)
{
// check lists identity
if (checkDevicesList(&newList))
return;
}
removeAbsentDevices(&newList);
addNewDevices(&newList);
}
// Get device data structure
MarvelmindDevice *getMarvelmindDevice(uint8_t address)
{
uint8_t i;
if (mmDevList.numDevices == 0)
return NULL;
for (i = 0; i < mmDevList.numDevices; i++)
{
if (mmDevList.devices[i].info.address == address)
{
return &mmDevList.devices[i];
}
}