Commit 15385d9a authored by Víctor Silos Sánchez's avatar Víctor Silos Sánchez
Browse files

Until 28 April

parent 36bc2f0f
......@@ -28,7 +28,6 @@
#include <iri_base_algorithm/iri_base_algorithm.h>
#include "foot_extended_alg.h"
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
// [publisher subscriber headers]
#include <std_msgs/String.h>
......@@ -45,13 +44,10 @@ class FootExtendedAlgNode : public algorithm_base::IriBaseAlgorithm<FootExtended
{
private:
tf::TransformListener listener;
tf::TransformBroadcaster broadcaster;
tf::StampedTransform transform1;
tf::StampedTransform transform2;
tf::StampedTransform transform3;
tf::StampedTransform transform4;
tf::StampedTransform transform5;
tf::StampedTransform transform6;
tf::Vector3 rightfoot;
tf::Vector3 leftfoot;
tf::Vector3 leftknee;
......@@ -60,8 +56,11 @@ class FootExtendedAlgNode : public algorithm_base::IriBaseAlgorithm<FootExtended
tf::Vector3 sumarfoot;
tf::Vector3 sumalknee;
tf::Vector3 sumarknee;
tf::Vector3 distancesafe;
// [publisher attributes]
ros::Publisher feetextended_publisher_;
std_msgs::String feetextended_String_msg_;
// [subscriber attributes]
......
......@@ -50,8 +50,6 @@ class FootExtendedAlgNode : public algorithm_base::IriBaseAlgorithm<FootExtended
tf::StampedTransform transform2;
tf::StampedTransform transform3;
tf::StampedTransform transform4;
tf::StampedTransform transform5;
tf::StampedTransform transform6;
tf::Vector3 rightfoot;
tf::Vector3 leftfoot;
tf::Vector3 leftknee;
......@@ -60,8 +58,11 @@ class FootExtendedAlgNode : public algorithm_base::IriBaseAlgorithm<FootExtended
tf::Vector3 sumarfoot;
tf::Vector3 sumalknee;
tf::Vector3 sumarknee;
tf::Vector3 distancesafe;
// [publisher attributes]
ros::Publisher feetextended_publisher_;
std_msgs::String feetextended_String_msg_;
// [subscriber attributes]
......
<launch>
<node pkg="tf" type="static_transform_publisher" name="tfcameratorobot" args="1.21 -0.22 1.36 0 3.14159265359 -1.57079632679 /brix_2_camera_frame world 100" />
<node pkg="iri_foot_extended" type ="iri_foot_extended" name="iri_foot_extended"/>
</launch>
<launch>
<node pkg="tf" type="static_transform_publisher" name="tfcameratorobot" args="1.21 -0.22 1.36 0 3.14159265359 -1.57079632679 /brix_2_camera_frame world 100" />
<node pkg="iri_foot_extended" type ="iri_foot_extended" name="iri_foot_extended"/>
</launch>
<?xml version="1.0"?>
<package>
<name>iri_foot_extended</name>
<version>0.0.0</version>
<description>The iri_foot_extended package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="vsilos@todo.todo">vsilos</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>LGPL</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/iri_foot_extended</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>iri_base_algorithm</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf</build_depend>
<run_depend>iri_base_algorithm</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>tf</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
......@@ -41,7 +41,9 @@
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>iri_base_algorithm</build_depend>
<build_depend>std_msgs</build_depend>
<run_depend>iri_base_algorithm</run_depend>
<run_depend>std_msgs</run_depend>
<!-- The export tag contains other, unspecified, tags -->
......
......@@ -21,7 +21,7 @@ void FootExtendedAlgorithm::config_update(Config& config, uint32_t level)
}
// FootExtendedAlgorithm Public API
void FootAlgorithm::angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4, double& angleft, double& angright)
void FootExtendedAlgorithm::angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4, double& angleft, double& angright)
{
double denleft = p2.getY() - p1.getY();
double denright = p4.getY() - p3.getY();
......
......@@ -8,6 +8,7 @@ FootExtendedAlgNode::FootExtendedAlgNode(void) :
//init class attributes if necessary
//this->loop_rate_ = 2;//in [Hz]
// [init publishers]
this->feetextended_publisher_ = this->public_node_handle_.advertise<std_msgs::String>("feetextended", 10);
// [init subscribers]
......@@ -27,24 +28,19 @@ FootExtendedAlgNode::~FootExtendedAlgNode(void)
void FootExtendedAlgNode::mainNodeThread(void)
{
string child_leftfoot("/brix_2/user_1/left_foot");
string child_leftknee("/brix_2/user_1/left_knee");
string child_rightfoot("/brix_2/user_1/right_foot");
string child_rightknee("/brix_2/user_1/right_knee");
string parent_cameraframe("/brix_2_camera_frame");
string child_distancesafeleft("/brix_2/user_1/distancesafeleft");
string child_distancesaferight("/brix_2/user_1/distancesaferight");
string child_robot("iri_wam_link_base");
string my_var = "";
try{
ros::Time zero = ros::Time(0);
ros::Time now = ros::Time::now();
ros::Duration three_seconds = ros::Duration(3.0);
sumalfoot.setX(0); sumalfoot.setY(0); sumalfoot.setZ(0);
sumarfoot.setX(0); sumarfoot.setY(0); sumarfoot.setZ(0);
sumalknee.setX(0); sumalknee.setY(0); sumalknee.setZ(0);
sumarknee.setX(0); sumarknee.setY(0); sumarknee.setZ(0);
string child_leftfoot("/brix_2/user_1/left_foot");
string child_leftknee("/brix_2/user_1/left_knee");
string child_rightfoot("/brix_2/user_1/right_foot");
string child_rightknee("/brix_2/user_1/right_knee");
string parent_cameraframe("/brix_2_camera_frame");
for(int i=0; i<100; ++i)
{
......@@ -77,19 +73,8 @@ void FootExtendedAlgNode::mainNodeThread(void)
leftknee = sumalknee/100;
rightfoot = sumarfoot/100;
rightknee = sumarknee/100;
//safe distance leftfoot
transform5.setOrigin(tf::Vector3(0.1, 0.1, 0.1));
transform5.setRotation(tf::Quaternion(1, 0, 0, 0));
broadcaster.sendTransform(tf::StampedTransform(transform5, now, child_leftfoot, child_distancesafeleft));
//safe distance rightfoot
transform6.setOrigin(tf::Vector3(0.1, 0.1, 0.1));
transform6.setRotation(tf::Quaternion(1, 0, 0, 0));
broadcaster.sendTransform(tf::StampedTransform(transform6, now, child_rightfoot, child_distancesaferight));
//Apply the function (angle) that measures the angle between the knee and the foot. When pass the limit (angle equals 30 degrees) the publisher says us that the foot is extended.
//Apply the function (angle) that measures the angle between the knee and the foot. When pass the limit (angle equals 35 degrees) the publisher says us that the foot is extended.
double angleft = 0.0;
double angright = 0.0;
......@@ -126,12 +111,19 @@ void FootExtendedAlgNode::mainNodeThread(void)
// [fill msg structures]
// Initialize the topic message structure
this->feetextended_String_msg_.data = my_var;
// [fill srv structure and make request to the server]
// [fill action structure and make request to the action server]
// [publish messages]
// Uncomment the following line to publish the topic message
this->feetextended_publisher_.publish(this->feetextended_String_msg_);
}
/* [subscriber callbacks] */
/* [service callbacks] */
......
......@@ -8,6 +8,7 @@ FootExtendedAlgNode::FootExtendedAlgNode(void) :
//init class attributes if necessary
//this->loop_rate_ = 2;//in [Hz]
// [init publishers]
this->feetextended_publisher_ = this->public_node_handle_.advertise<std_msgs::String>("feetextended", 10);
// [init subscribers]
......@@ -27,24 +28,19 @@ FootExtendedAlgNode::~FootExtendedAlgNode(void)
void FootExtendedAlgNode::mainNodeThread(void)
{
string child_leftfoot("/brix_2/user_1/left_foot");
string child_leftknee("/brix_2/user_1/left_knee");
string child_rightfoot("/brix_2/user_1/right_foot");
string child_rightknee("/brix_2/user_1/right_knee");
string parent_cameraframe("/brix_2_camera_frame");
string child_distancesafeleft("/brix_2/user_1/distancesafeleft");
string child_distancesaferight("/brix_2/user_1/distancesaferight");
string child_robot("iri_wam_link_base");
string my_var = "";
try{
ros::Time zero = ros::Time(0);
ros::Time now = ros::Time::now();
ros::Duration three_seconds = ros::Duration(3.0);
sumalfoot.setX(0); sumalfoot.setY(0); sumalfoot.setZ(0);
sumarfoot.setX(0); sumarfoot.setY(0); sumarfoot.setZ(0);
sumalknee.setX(0); sumalknee.setY(0); sumalknee.setZ(0);
sumarknee.setX(0); sumarknee.setY(0); sumarknee.setZ(0);
string child_leftfoot("/brix_2/user_1/left_foot");
string child_leftknee("/brix_2/user_1/left_knee");
string child_rightfoot("/brix_2/user_1/right_foot");
string child_rightknee("/brix_2/user_1/right_knee");
string parent_cameraframe("/brix_2_camera_frame");
for(int i=0; i<100; ++i)
{
......@@ -77,19 +73,8 @@ void FootExtendedAlgNode::mainNodeThread(void)
leftknee = sumalknee/100;
rightfoot = sumarfoot/100;
rightknee = sumarknee/100;
//safe distance leftfoot
transform5.setOrigin(tf::Vector3(0.1, 0.1, 0.1));
transform5.setRotation(tf::Quaternion(1, 0, 0, 0));
broadcaster.sendTransform(tf::StampedTransform(transform5, now, child_leftfoot, child_distancesafeleft));
//safe distance rightfoot
transform6.setOrigin(tf::Vector3(0.1, 0.1, 0.1));
transform6.setRotation(tf::Quaternion(1, 0, 0, 0));
broadcaster.sendTransform(tf::StampedTransform(transform6, now, child_rightfoot, child_distancesaferight));
//Apply the function (angle) that measures the angle between the knee and the foot. When pass the limit (angle equals 30 degrees) the publisher says us that the foot is extended.
//Apply the function (angle) that measures the angle between the knee and the foot. When pass the limit (angle equals 35 degrees) the publisher says us that the foot is extended.
double angleft = 0.0;
double angright = 0.0;
......@@ -126,12 +111,19 @@ void FootExtendedAlgNode::mainNodeThread(void)
// [fill msg structures]
// Initialize the topic message structure
this->feetextended_String_msg_.data = my_var;
// [fill srv structure and make request to the server]
// [fill action structure and make request to the action server]
// [publish messages]
// Uncomment the following line to publish the topic message
this->feetextended_publisher_.publish(this->feetextended_String_msg_);
}
/* [subscriber callbacks] */
/* [service callbacks] */
......
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