Commit ab7c959d authored by Nicolas Federico Furquez Morena's avatar Nicolas Federico Furquez Morena
Browse files

Merge branch 'branch_nico' into 'develop'

Branch nico

See merge request !9
parents 58fa5919 00a8ec7f
......@@ -19,7 +19,7 @@ services:
#- /home/oskysal/fing/git/superrobot/alfred:/root/fing/git/superrobot/alfred
- /home/oskysal/docker/ros/container/root/.ros:/root/.ros
- /home/oskysal/docker/ros/container/root/catkin_ws:/root/catkin_ws
- /home/oskysal/git/Repo-pgLocRobIn2018/Desarrollo/Integracion ROS/localization_localino:/root/catkin_ws/src/localization_localino
- /home/oskysal/git/Repo-pgLocRobIn2018/Desarrollo/Integracion ROS/localization_ros:/root/catkin_ws/src/localization_ros
#command: "/etc/bootstrap.sh -d -namenode"
command: "roscore"
......@@ -42,13 +42,13 @@ services:
#- /home/oskysal/fing/git/superrobot/alfred:/root/fing/git/superrobot/alfred
- /home/oskysal/docker/ros/container/root/.ros:/root/.ros
- /home/oskysal/docker/ros/container/root/catkin_ws:/root/catkin_ws
- /home/oskysal/git/Repo-pgLocRobIn2018/Desarrollo/Integracion ROS/localization_localino:/root/catkin_ws/src/localization_localino
- /home/oskysal/git/Repo-pgLocRobIn2018/Desarrollo/Integracion ROS/localization_ros:/root/catkin_ws/src/localization_ros
#entrypoint: "tail -f /dev/null"
command: bash -c "source /root/catkin_ws/devel/setup.bash && roslaunch --wait localization_localino publish_anchors_tf.launch anchors_csv:=/root/catkin_ws/src/localization_localino/scripts/anchors.csv"
#command: bash -c "sleep 10s && roslaunch localization_localino publish_anchors_tf.launch anchors_csv:=/root/catkin_ws/src/localization_localino/scripts/anchors.csv ; roscore"
command: bash -c "source /root/catkin_ws/devel/setup.bash && roslaunch --wait localization_ros publish_anchors_tf.launch anchors_csv:=/root/catkin_ws/src/localization_ros/scripts/anchors.csv"
#command: bash -c "sleep 10s && roslaunch localization_ros publish_anchors_tf.launch anchors_csv:=/root/catkin_ws/src/localization_ros/scripts/anchors.csv ; roscore"
#command: bash -c "sleep 10s && roscore"
#command: tail -f /dev/null
#command: "roslaunch localization_localino publish_anchors_tf.launch anchors_csv:=/root/catkin_ws/src/localization_localino/scripts/anchors.csv"
#command: "roslaunch localization_ros publish_anchors_tf.launch anchors_csv:=/root/catkin_ws/src/localization_ros/scripts/anchors.csv"
devices:
- "/dev/ttyS0:/dev/ttyS0"
......
# Configuraciones necesarias en el HOST para poder visualizar las TFs estaticas en Rviz
ROS_MASTER_URI=http://172.19.0.2:11311
ROS_IP=172.19.0.1
version: "3.8"
services:
tf-publisher:
#image: osrf/ros:kinetic-desktop-full-xenial-devel-tmp
#container_name: ros-kinetic-devel-node-calibrator
image: fing:ros-kinetic-robot-xenial
container_name: ros-kinetic-robot-xenial-node-tf-publisher
environment:
- "ROS_IP=172.19.0.2"
- "ROS_MASTER_URI=http://172.19.0.2:11311"
hostname: master
networks:
default:
ipv4_address: 172.19.0.2
volumes:
#- /home/oskysal/docker/ros/:/root/.ros/
#- /home/oskysal/fing/git/superrobot/alfred:/root/fing/git/superrobot/alfred
- /home/oskysal/docker/ros/container/root/.ros:/root/.ros
- /home/oskysal/docker/ros/container/root/catkin_ws:/root/catkin_ws
- /home/oskysal/git/Repo-pgLocRobIn2018/Desarrollo/Integracion ROS/localization_ros:/root/catkin_ws/src/localization_ros
- /home/oskysal/git/Repo-pgLocRobIn2018:/root/pg
#entrypoint: "tail -f /dev/null"
command: bash -c "cd /root/catkin_ws/ && catkin_make && source /root/catkin_ws/devel/setup.bash && roslaunch localization_ros run_anchors_static_tf_publisher.launch"
#command: bash -c "sleep 10s && roslaunch localization_ros publish_anchors_tf.launch anchors_csv:=/root/catkin_ws/src/localization_ros/scripts/anchors.csv ; roscore"
#command: bash -c "sleep 10s && roscore"
#command: tail -f /dev/null
#command: "roslaunch localization_ros publish_anchors_tf.launch anchors_csv:=/root/catkin_ws/src/localization_ros/scripts/anchors.csv"
#devices:
# - "/dev/ttyACM0:/dev/ttyACM0"
networks:
default:
driver: bridge
#external:
#name: ros-network
ipam:
driver: default
config:
- subnet: 172.19.0.0/16
\ No newline at end of file
......@@ -10,13 +10,13 @@ services:
#- /home/oskysal/fing/git/superrobot/alfred:/root/fing/git/superrobot/alfred
- /home/oskysal/docker/ros/container/root/.ros:/root/.ros
- /home/oskysal/docker/ros/container/root/catkin_ws:/root/catkin_ws
- /home/oskysal/git/Repo-pgLocRobIn2018/Desarrollo/Integracion ROS/localization_localino:/root/catkin_ws/src/localization_localino
- /home/oskysal/git/Repo-pgLocRobIn2018/Desarrollo/Integracion ROS/localization_ros:/root/catkin_ws/src/localization_ros
#entrypoint: "tail -f /dev/null"
command: bash -c "cd /root/catkin_ws/ && catkin_make && source /root/catkin_ws/devel/setup.bash && roslaunch localization_localino run_calibrar.launch"
#command: bash -c "sleep 10s && roslaunch localization_localino publish_anchors_tf.launch anchors_csv:=/root/catkin_ws/src/localization_localino/scripts/anchors.csv ; roscore"
command: bash -c "cd /root/catkin_ws/ && catkin_make && source /root/catkin_ws/devel/setup.bash && roslaunch localization_ros run_calibrar.launch"
#command: bash -c "sleep 10s && roslaunch localization_ros publish_anchors_tf.launch anchors_csv:=/root/catkin_ws/src/localization_ros/scripts/anchors.csv ; roscore"
#command: bash -c "sleep 10s && roscore"
#command: tail -f /dev/null
#command: "roslaunch localization_localino publish_anchors_tf.launch anchors_csv:=/root/catkin_ws/src/localization_localino/scripts/anchors.csv"
#command: "roslaunch localization_ros publish_anchors_tf.launch anchors_csv:=/root/catkin_ws/src/localization_ros/scripts/anchors.csv"
devices:
- "/dev/ttyACM0:/dev/ttyACM0"
version: "3.8"
services:
simulator:
#image: osrf/ros:kinetic-desktop-full-xenial-devel-tmp
#container_name: ros-kinetic-devel-node-calibrator
image: fing:ros-kinetic-robot-xenial
container_name: ros-kinetic-robot-xenial-node-estimate
volumes:
#- /home/oskysal/docker/ros/:/root/.ros/
#- /home/oskysal/fing/git/superrobot/alfred:/root/fing/git/superrobot/alfred
- /home/oskysal/docker/ros/container/root/.ros:/root/.ros
- /home/oskysal/docker/ros/container/root/catkin_ws:/root/catkin_ws
- /home/oskysal/git/Repo-pgLocRobIn2018/Desarrollo/Integracion ROS/localization_ros:/root/catkin_ws/src/localization_ros
- /home/oskysal/git/Repo-pgLocRobIn2018:/root/pg
#entrypoint: "tail -f /dev/null"
command: bash -c "cd /root/catkin_ws/ && catkin_make && source /root/catkin_ws/devel/setup.bash && roslaunch localization_ros run_multi_filters.launch"
#command: bash -c "sleep 10s && roslaunch localization_ros publish_anchors_tf.launch anchors_csv:=/root/catkin_ws/src/localization_ros/scripts/anchors.csv ; roscore"
#command: bash -c "sleep 10s && roscore"
#command: tail -f /dev/null
#command: "roslaunch localization_ros publish_anchors_tf.launch anchors_csv:=/root/catkin_ws/src/localization_ros/scripts/anchors.csv"
#devices:
# - "/dev/ttyACM0:/dev/ttyACM0"
......@@ -2,7 +2,7 @@
docker container create \
-v "/home/oskysal/docker/ros/container/root/.ros/:/root/.ros/" \
-v "/home/oskysal/docker/ros/container/root/catkin_ws:/root/host/ros/workspace/" \
--mount type=bind,source="/home/oskysal/git/localino/Integracion ROS/localization_localino/",target="/root/catkin_ws/src/localization_localino/" \
--mount type=bind,source="/home/oskysal/git/localino/Integracion ROS/localization_ros/",target="/root/catkin_ws/src/localization_ros/" \
-p 11311:11311 \
--name ros-kinetic-devel-tmp2 \
osrf/ros:kinetic-desktop-full-xenial-devel-tmp \
......
......@@ -5,7 +5,7 @@
docker container run \
-it \
-v "/home/oskysal/docker/ros/runtime/:/root/.ros/" \
-v "/home/oskysal/catkin_ws/:/root/catkin_ws/src/localization_localino/" \
-v "/home/oskysal/catkin_ws/:/root/catkin_ws/src/localization_ros/" \
-p 11311:11311 \
--name ros-kinetic-devel-temp \
fing:ros-kinetic-robot-xenial \
......
......@@ -25,7 +25,7 @@
#include "DW1000Time.h"
#include "DW1000.h"
#include <localization_localino/Distance.h>
#include <localization_ros/Distance.h>
#include "LocalinoHardware.h"
#define turnOn(led) (digitalWrite(led, HIGH))
......@@ -40,8 +40,8 @@ void ROS_doRange(const std_msgs::Bool& value);
void ROS_performDump(const std_msgs::Empty& value);
std_msgs::String str_msg;
localization_localino::Distance distancia;
ros::Publisher localization("localization", &distancia);
localization_ros::Distance distancia;
ros::Publisher send_localization("localization_ros", &distancia);
ros::Publisher localization_status("localization_status", &str_msg);
ros::Subscriber<std_msgs::UInt32> subscriber("antenna_delay_param", &ROS_newAntennaDelayValue);
......@@ -109,7 +109,7 @@ void ROS_setup() {
_nh.subscribe(subscriber);
_nh.subscribe(subscriberDoRange);
_nh.subscribe(subscriberPerformDump);
_nh.advertise(localization);
_nh.advertise(send_localization);
_nh.advertise(localization_status);
}
......@@ -121,7 +121,7 @@ void loop() {
delay(1);
}
void DW1000Ranging_newRange() {
void DW1000Ranging_newRange(DW1000Device* device, unsigned long rangingRound) {
// indicate TX
turnOn(LocalinoHardware::LED2);
......@@ -189,7 +189,7 @@ void enviarSerialRos(int anchor, float distance, float power, int code, const ch
distancia.distance = distance;
distancia.power = power;
localization.publish( &distancia );
send_localization.publish( &distancia );
}
void enviarStatus(const char* msg) {
......
......@@ -24,7 +24,7 @@
#include "DW1000Time.h"
#include "DW1000.h"
#include <localization_localino/Distance.h>
#include <localization_ros/Distance.h>
#include "LocalinoHardware.h"
// ======================= ROS ==========================
......@@ -35,8 +35,8 @@ void ROS_newAntennaDelayValue(const std_msgs::UInt32& value);
void ROS_performDump(const std_msgs::Empty& value);
std_msgs::String str_msg;
localization_localino::Distance distancia;
ros::Publisher localization("localization", &distancia);
localization_ros::Distance distancia;
ros::Publisher send_localization("localization_ros", &distancia);
ros::Publisher localization_status("localization_status", &str_msg);
ros::Subscriber<std_msgs::UInt32> subscriberNewAntennaDelay("antenna_delay_param", &ROS_newAntennaDelayValue);
......@@ -112,7 +112,7 @@ void ROS_setup() {
_nh.initNode();
_nh.subscribe(subscriberNewAntennaDelay);
_nh.subscribe(subscriberPerformDump);
_nh.advertise(localization);
_nh.advertise(send_localization);
}
void loop() {
......@@ -187,8 +187,8 @@ void enviarSerialRos(int anchor, float distance, float power, int code, const ch
distancia.distance = distance;
distancia.power = power;
//localization.publish( &distancia );
Serial.println(msg);
send_localization.publish( &distancia );
//Serial.println(msg);
}
void ROS_performDump(const std_msgs::Empty& value) {
......
......@@ -13,7 +13,7 @@
#include <ros.h>
#include <localization_localino/Distance.h>
#include <localization_ros/Distance.h>
#include "LocalinoHardware.h"
......@@ -24,8 +24,8 @@ ros::NodeHandle_<LocalinoHardware> nh;
const unsigned int BUFFER_SIZE = 100;
localization_localino::Distance distancia;
ros::Publisher localization("localization", &distancia);
localization_ros::Distance distancia;
ros::Publisher send_localization("localization_ros", &distancia);
char _bufferSerial[BUFFER_SIZE];
// connection pins
......@@ -69,7 +69,7 @@ void setup() {
_currentShortAddress = shortAddress[1] * 256 + shortAddress[0];
nh.initNode();
nh.advertise(localization); //TODO cambiar si hacemos mensaje nuestro
nh.advertise(send_localization);
// finished
digitalWrite(LED1, HIGH);
......@@ -84,20 +84,20 @@ void loop() {
digitalWrite(LED2, LOW);
}
void newRange() {
void newRange(DW1000Device* device, unsigned long rangingRound) {
// indicate TX
digitalWrite(LED1, HIGH);
byte* _ownAddress = DW1000Ranging.getDistantDevice()->getByteAddress();
//byte* _ownAddress = device->getByteAddress();
//Mando al serial como si fuera desde un anchor
unsigned long timestamp = millis();
float tagRange = DW1000Ranging.getDistantDevice()->getRange();
float tagPower = DW1000Ranging.getDistantDevice()->getRXPower();
String msg = "R," + String(DW1000Ranging.getDistantDevice()->getShortAddress(), HEX) + "," + String(2) + "," + tagRange + "," + timestamp;
float tagRange = device->getRange();
float tagPower = device->getRXPower();
String msg = "R," + String(device->getShortAddress(), HEX) + "," + String(2) + "," + tagRange + "," + timestamp;
Serial1.println("SendToUDP " + String(hostip) + ",10001," + msg);
enviarSerialRos(DW1000Ranging.getDistantDevice()->getShortAddress(), tagRange,tagPower, 0, msg);
enviarSerialRos(device->getShortAddress(), tagRange,tagPower, 0, msg);
// indicate TX off
digitalWrite(LED1, LOW);
}
......@@ -126,7 +126,7 @@ void enviarSerialRos(int anchor, float distance, float power, int code, String m
distancia.distance = distance;
distancia.power = power;
localization.publish( &distancia );
send_localization.publish( &distancia );
nh.spinOnce();
......
/**
/**
@todo
- move strings to flash (less RAM consumption)
......@@ -12,10 +12,11 @@
*/
#include <ros.h>
#include <localization_localino/Distance.h>
#include "LocalinoHardware.h"
#include <localization_ros/Distance.h>
#include "DW1000Ranging.h"
// ======================= ROS ==========================
......@@ -23,8 +24,8 @@ ros::NodeHandle_<LocalinoHardware> nh;
const unsigned int BUFFER_SIZE = 100;
localization_localino::Distance distancia;
ros::Publisher localization("localization", &distancia);
localization_ros::Distance distancia;
ros::Publisher send_localization("localization_ros", &distancia);
// ======================================================
char tagid[90] = "00:01:5B:D5:A9:9A:E2:9C"; // defines the ID of the Anchor, MUST be unique
......@@ -80,7 +81,7 @@ void DW1000Ranging_setup() {
void ROS_setup() {
nh.initNode();
nh.advertise(localization); //TODO cambiar si hacemos mensaje nuestro
nh.advertise(send_localization);
}
void loop() {
......@@ -149,7 +150,7 @@ void enviarSerialRos(int anchor, float distance, float power, int code, const ch
distancia.distance = distance;
distancia.power = power;
localization.publish( &distancia );
send_localization.publish( &distancia );
//Serial.println(msg);
doMeasure = true;
}
<launch >
<!-- <include file="$(find localization_localino)/launch/run.launch"/> -->
<!-- <include file="$(find localization_localino)/launch/run_test.launch"/> -->
<include file="$(find localization_localino)/launch/publish_anchors_tf.launch"/>
<node name="visualizar" pkg="rviz" type="rviz" respawn="true" args="-d $(find localization_localino)/rviz/nodos.rviz"/>
<!-- <node name="fixed_frame" pkg="localization_localino" type="fixed_frame.py" respawn="true" args="-d $(find localization_localino)/rviz/nodos.rviz"/> -->
</launch>
\ No newline at end of file
cmake_minimum_required(VERSION 3.0.2)
project(localization_localino)
project(localization_ros)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
......@@ -104,7 +104,7 @@ generate_messages(
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES localization_localino
# LIBRARIES localization_ros
CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
# DEPENDS system_lib
)
......@@ -122,7 +122,7 @@ include_directories(
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/localization_localino.cpp
# src/${PROJECT_NAME}/localization_ros.cpp
# )
## Add cmake target dependencies of the library
......@@ -133,7 +133,7 @@ include_directories(
## 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
# add_executable(${PROJECT_NAME}_node src/localization_localino_node.cpp)
# add_executable(${PROJECT_NAME}_node src/localization_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
......@@ -197,7 +197,7 @@ include_directories(
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_localization_localino.cpp)
# catkin_add_gtest(${PROJECT_NAME}-test test/test_localization.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
......
......@@ -3,6 +3,6 @@
<node pkg="rosbag" type="play" name="player3" args="--wait-for-subscribers -q --clock --skip-empty=1 $(arg carpeta)/pto_19.bag" output="screen" />
<node pkg="localization_localino" required="true" type="comprobacion.py" name="salida" output="screen" />
<node pkg="localization_ros" required="true" type="comprobacion.py" name="salida" output="screen" />
</launch>
\ No newline at end of file
......@@ -2,10 +2,10 @@
<arg name="anchors_csv" doc="/home/nico/Proyecto/RepoFinal/Investigacion/error_localizacion/posiciones1.csv"/>
<!-- Output to screen -->
<!-- <node name="anchors_static_tf_publisher" pkg="localization_localino" type="anchors_static_tf_publisher.py" args="$(arg anchors_csv)" respawn="true" output="screen" /> -->
<!-- <node name="anchors_static_tf_publisher" pkg="localization_ros" type="anchors_static_tf_publisher.py" args="$(arg anchors_csv)" respawn="true" output="screen" /> -->
<!-- Output to log -->
<node name="anchors_static_tf_publisher" pkg="localization_localino" type="anchors_static_tf_publisher.py" args="/home/nico/Proyecto/RepoFinal/Investigacion/pruebas.csv" respawn="true" />
<node name="anchors_static_tf_publisher" pkg="localization_ros" type="anchors_static_tf_publisher.py" args="/home/nico/Proyecto/RepoFinal/Investigacion/prueba.csv" respawn="true" />
<!-- Original -->
<!-- <node name="anchors" pkg="localization_localino" type="anchors_static_tf_publisher.py" args="/root/catkin_ws/src/localization_localino/scripts/anchors.csv" respawn="true"/> -->
<!-- <node name="anchors" pkg="localization_ros" type="anchors_static_tf_publisher.py" args="/root/catkin_ws/src/localization_ros/scripts/anchors.csv" respawn="true"/> -->
</launch>
<launch>
<node name="reader" pkg="localization_localino" type="reader.py" output="screen">
<node name="reader" pkg="localization_ros" type="reader.py" output="screen">
<rosparam>
#0 sin filtro, 1 Afa beta, 2 Kalman, 3 Promedio
......
<launch>
<arg name="anchors_csv" default="/root/catkin_ws/src/localization_ros/scripts/puntos.csv" doc="ruta arvhico CSV con posiciones de los anchors" />
<!-- Output to screen -->
<node name="anchors_static_tf_publisher" pkg="localization_ros" type="anchors_static_tf_publisher.py" args="$(arg anchors_csv)" respawn="true" output="screen" />
<!-- Output to log -->
<!--<node name="anchors_static_tf_publisher" pkg="localization_ros" type="anchors_static_tf_publisher.py" args="/home/nico/Proyecto/RepoFinal/Investigacion/error_localizacion/posiciones1.csv" respawn="true" />-->
<!-- Original -->
<!-- <node name="anchors" pkg="localization_ros" type="anchors_static_tf_publisher.py" args="/root/catkin_ws/src/localization_ros/scripts/anchors.csv" respawn="true"/> -->
</launch>
<launch>
<node name="reader" pkg="localization_localino" type="reader.py" output="screen">
<node name="reader" pkg="localization_ros" type="reader.py" output="screen">
<rosparam>
#0 sin filtro, 1 Alfa beta, 2 Kalman, 3 Promedio
......@@ -46,7 +46,7 @@ anchor8: [-3.701,-0.824,1.493],
}
debug: True
#es la altura del tag, asumimos que el tag esta siempre a la misma altura
tagheight = 1.725
tagheight: 1.725
</rosparam>
</node>
<arg name="carpeta" value="/home/nico/Proyecto/RepoFinal/Investigacion/bags_distancias/" />
......
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