To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 04edea33 authored by remartin's avatar remartin
Browse files

Basic interaction with the ROS/UWB system is enabled.

parent bed3f551
......@@ -196,6 +196,14 @@ include_directories(
### --- Executables --- ###
# UWBSpotlight_main - Sources
set(MY_CPP_SOURCES_UWBS
${PROJECT_SOURCE_DIR}/src/UWBSpotlight_main.cpp
${PROJECT_SOURCE_DIR}/src/spotlightManager.cpp
${PROJECT_SOURCE_DIR}/src/SerialLight.cpp
)
# UWBManagerService -- Add sources here
set(MY_CPP_SOURCES_UWBMS # compilation of sources
${PROJECT_SOURCE_DIR}/src/UWBManagerService.cpp
......@@ -203,7 +211,6 @@ set(MY_CPP_SOURCES_UWBMS # compilation of sources
${PROJECT_SOURCE_DIR}/src/DataListener.cpp
)
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
......@@ -213,6 +220,12 @@ add_dependencies(ViconDataPublisher d_fall_pps_generate_messages_cpp ${catkin_EX
target_link_libraries(ViconDataPublisher ${catkin_LIBRARIES})
target_link_libraries(ViconDataPublisher ${VICON_LIBRARY})
add_executable(UWBSpotlight_main ${MY_CPP_SOURCES_UWBS})
add_dependencies(UWBSpotlight_main
d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
target_link_libraries(UWBSpotlight_main
${catkin_LIBRARIES})
add_executable(UWBManagerService ${MY_CPP_SOURCES_UWBMS})
add_dependencies(UWBManagerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
target_link_libraries(UWBManagerService ${catkin_LIBRARIES})
......
#ifndef UWBSPOTLIGHT_MAIN_H
#define UWBSPOTLIGHT_MAIN_H
//#include <fstream>
#include <std_msgs/Float32MultiArray.h>
#include "std_msgs/Int32.h"
#include "std_msgs/UInt32MultiArray.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/UWBAnchor.h"
#include "d_fall_pps/UWBAnchorArray.h"
namespace d_fall_pps{
void updateTargetPositionCallback(const CrazyflieData &data);
void anchorArrangementChangedCallback(const std_msgs::Int32& msg);
void placeSpotlight();
void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
}
#endif
......@@ -27,6 +27,17 @@ public:
// uses actual coordinates
void testPTupdater();
void setTargetPosition(double x, double y, double z);
void setSpotlightPosition(double x, double y, double z);
// private afterwards:
void updateSpherical();
void updatePTvalues();
void setPositions(double spotlightPosition[3], double targetPosition[3]);
void calibrateOrientation();
private:
int posPT;
......@@ -42,10 +53,7 @@ private:
double spotlightPos[3];
double targetPos[3];
double spherical[3]; // saving that is bad practice?
void updateSpherical();
void updatePTvalues();
void calibrateOrientation();
//void calibrateOrientation();
// construct strings that can be decoded by the arduino
std::string buildMessage(); // 'bad practice to import namespace std in header'
......
<launch>
<node pkg="d_fall_pps" name="UWBL_main" output="screen" type="UWBL_main">
<param launchingViaRos=true/>
</node>
</launch>
<launch>
<node pkg="d_fall_pps" name="UWBSpotlight_main" output="screen" type="UWBSpotlight_main">
<rosparam command="load" file="$(find d_fall_pps)/param/AnchorPos.yaml" />
</node>
</launch>
\ No newline at end of file
#include "SerialLight.h"
SerialLight::SerialLight(){
_isConnected = false;
_port = "";
......
......@@ -46,8 +46,8 @@
using namespace d_fall_pps;
ros::Publisher studentIDs_publisher;
ros::Publisher CFData_publisher;
//ros::Publisher studentIDs_publisher;
//ros::Publisher CFData_publisher;
/************************************************************
......
//#include<iostream>
#include<stdio.h>
#include "SerialLight.h"
#include "spotlightManager.h"
using namespace std;
int main(){
for(int i = 0; i<10; i++){
printf("Globifisch\n");
//cout << "GlobiSchnaegg";
}
spotlightManager slM;
// printf("Testing the message builder: ");
// printf((slM.buildMessage()).c_str());
// printf("\n");
//slM.setPTmanually(false);
slM.testPTupdater();
//slM.someDefaultSettings();
//slM.setPTmanually();
// need ros stuff before -> put into an init() method?
//slM.calibrateOrientation();
//slM.trivialDemo();
//slM.lineOnWall(true, 50);
return 0;
}
//#include<iostream>
//#include <stdlib.h>
//#include <chrono>
//#include <iostream>
#include<stdio.h>
#include "SerialLight.h"
#include "spotlightManager.h"
#include <string>
#include <vector>
#include <ros/package.h>
#include <rosbag/bag.h>
#include <ros/ros.h>
#include "d_fall_pps/CMQuery.h"
#include "d_fall_pps/Anchors.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/CrazyflieContext.h"
#include "UWBSpotlight_main.h"
// using namespace std;
using namespace d_fall_pps;
spotlightManager slM;
int spotlightAnchor = 4;
bool placedSpotlight = false;
bool foundTarget = false;
ros::ServiceClient uwbManager;
int main(int argc, char* argv[]){
for(int i = 0; i<10; i++){
printf("Globifisch\n");
//cout << "GlobiSchnaegg";
}
// ROS stuff:
ros::init(argc, argv, "UWBSpotlight_main");
ros::NodeHandle nodeHandle("~");
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
// load context parameters
// need to load parameters?
// to get the anchor positions/detect whether they have changed.
uwbManager = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false);
ros::Subscriber enableUWBSubscriber = nodeHandle.subscribe("/my_GUI/enableUWB", 1, anchorArrangementChangedCallback);
// subscribe to the UWB data publisher, which provides info on the target = crazyflie.
ros::Subscriber targetSubscriber = namespaceNodeHandle.subscribe("UWBDataPublisher/UWBData", 100, updateTargetPositionCallback);
ROS_INFO_STREAM("[UWBSpotlight_main] subscribed to target UWB stuff. ");
ROS_INFO_STREAM("[UWBSpotlight_main] started. ");
// Just for testing!
slM.calibrateOrientation();
ros::spin(); // spinning 'till it goes out -> need initialisation procedure on first call?
ROS_WARN("[UWBSpotlight_main] stopped. ");
return 0;
}
void d_fall_pps::updateTargetPositionCallback(const CrazyflieData &data){
/*
CrazyflieData thingy:
string crazyflieName
float64 x
float64 y
float64 z
float64 roll
float64 pitch
float64 yaw
float64 acquiringTime #delta t
bool occluded
*/
// convert from float64?
slM.setTargetPosition(data.x, data.y, data.z);
slM.updatePTvalues();
foundTarget = true;
}
void d_fall_pps::anchorArrangementChangedCallback(const std_msgs::Int32& msg){
placeSpotlight();
}
void d_fall_pps::placeSpotlight(){
Anchors a;
if(uwbManager.call(a)){
UWBAnchorArray anchors = a.response.anchorArray;
// only need one!
UWBAnchor sl = anchors.data[spotlightAnchor-1];
slM.setSpotlightPosition(sl.x, sl.y, sl.z);
placedSpotlight = true;
// Just for testing!
slM.calibrateOrientation();
}else{
ROS_ERROR("[UWBSpotlight_main] Could not grab the positions of all the ankers. ");
}
/*
Anchors a;
if(uwbManager.call(a))
{
anchors = a.response.anchorArray;
}
else
{
ROS_ERROR("[UWBDataPublisher] Could not update UWB Settings!");
}
for(int i = 0; i < UWB_NUM_ANCHORS; ++i)
{
anchorPositions[3*i] = anchors.data[i].x;
anchorPositions[3*i + 1] = anchors.data[i].y;
anchorPositions[3*i + 2] = anchors.data[i].z;
}
*/
}
// include instead
void d_fall_pps::loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length) {
if(!nodeHandle.getParam(name, val)){
ROS_ERROR_STREAM("missing parameter '" << name << "'");
}
if(val.size() != length) {
ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
}
}
......@@ -13,6 +13,8 @@
//#define THETA_DMX_TILT //110/90, DMX steps per degree.
using namespace std;
//using namespace d_fall_pps;
spotlightManager::spotlightManager(){
......@@ -225,6 +227,18 @@ void spotlightManager::updateSpherical(){
}
void spotlightManager::setTargetPosition(double x, double y, double z){
targetPos[0] = x;
targetPos[1] = y;
targetPos[2] = z;
}
void spotlightManager::setSpotlightPosition(double x, double y, double z){
spotlightPos[0] = x;
spotlightPos[1] = y;
spotlightPos[2] = z;
}
void spotlightManager::updatePTvalues(){
......@@ -282,6 +296,17 @@ void spotlightManager::updatePTvalues(){
printf("PT values updated as follows: %s \n", buildPT().c_str());
}
void spotlightManager::setPositions(double spotlightPosition[3], double targetPosition[3]){
//spotlightPos = spotlightPosition;
// making sure that it works:
spotlightPos[0] = spotlightPosition[0];
spotlightPos[1] = spotlightPosition[1];
spotlightPos[2] = spotlightPosition[2];
targetPos[0] = targetPosition[0];
targetPos[1] = targetPosition[1];
targetPos[2] = targetPosition[2];
}
void spotlightManager::calibrateOrientation(){
......@@ -333,6 +358,7 @@ void spotlightManager::calibrateOrientation(){
// moves back and forth on a line starting at an initial position.
void spotlightManager::testPTupdater(){
//setPositions({0.0, 0,0, 0,0}, {0.0, 5.0, 0.0});
spotlightPos[0] = 0;
spotlightPos[1] = 0;
spotlightPos[2] = 0;
......
Markdown is supported
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