Skip to content
GitLab
Commit 75a7fc2e authored by Víctor Silos Sánchez's avatar Víctor Silos Sánchez
Browse files

The safe distance is finished

parent 68976381
Hide whitespace changes
Inline Side-by-side
iri_foot/include/foot_alg_node.h
View file @ 75a7fc2e
......@@ -28,6 +28,7 @@
#include <iri_base_algorithm/iri_base_algorithm.h>
#include "foot_alg.h"
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
// [publisher subscriber headers]
#include <std_msgs/String.h>
......@@ -45,10 +46,13 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
{
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;
......@@ -57,6 +61,7 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::Vector3 sumarfoot;
tf::Vector3 sumalknee;
tf::Vector3 sumarknee;
tf::Vector3 distancesafe;
// [publisher attributes]
ros::Publisher myfeet_publisher_;
......
iri_foot/include/foot_alg_node.h~
View file @ 75a7fc2e
......@@ -28,6 +28,7 @@
#include <iri_base_algorithm/iri_base_algorithm.h>
#include "foot_alg.h"
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
// [publisher subscriber headers]
#include <std_msgs/String.h>
......@@ -45,14 +46,21 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
{
private:
tf::TransformListener listener;
tf::TransformBroadcaster broadcaster;
tf::StampedTransform transform1;
tf::StampedTransform transform2;
tf::StampedTransform transform3;
tf::StampedTransform transform4;
tf::StampedTransform transform5;
tf::Vector3 rightfoot;
tf::Vector3 leftfoot;
tf::Vector3 leftknee;
tf::Vector3 rightknee;
tf::Vector3 sumalfoot;
tf::Vector3 sumarfoot;
tf::Vector3 sumalknee;
tf::Vector3 sumarknee;
tf::Vector3 distancesafe;
// [publisher attributes]
ros::Publisher myfeet_publisher_;
......
iri_foot/launch/foot.launch
View file @ 75a7fc2e
<launch>
<arg name="tracker_name" default="brix_2" />
<node pkg="kinect2_tracker" type="kinect2_tracker_node" name="kinect2_tracker_node2" output="screen">
<param name="tf_prefix" value="$(arg tracker_name)" />
<param name="relative_frame" value="/$(arg tracker_name)_camera_frame" />
</node>
<!-- TF Static Transforms to World -->
<node pkg="tf" type="static_transform_publisher" name="brix_to_global2"
args="0.295729 -0.463885 -0.0325348 0 0 0 global_space /$(arg tracker_name)_camera_frame 100"/>
<node pkg="tf" type="static_transform_publisher" name="tfcameratorobot" args="1.05 0.29 1.65 0 0 -3.14159265359/2 brix_2_camera_frame ROBOT 100" />
<!--tx: 0.295729 ty: -0.463885 tz: -0.0325348 rw: 1 rx: 0 ry: 0 rz: 0-->
<node pkg="tf" type="static_transform_publisher" name="tfcameratorobot" args="0.3 0.29 0.3 0 0 -1.57079632679 brix_2_camera_frame world 100" />
<node pkg="iri_foot" type ="iri_foot" name="iri_foot"/>
</launch>
iri_foot/launch/foot.launch~
View file @ 75a7fc2e
<launch>
<arg name="tracker_name" default="brix_2" />
<node pkg="kinect2_tracker" type="kinect2_tracker_node" name="kinect2_tracker_node2" output="screen">
<param name="tf_prefix" value="$(arg tracker_name)" />
<param name="relative_frame" value="/$(arg tracker_name)_camera_frame" />
</node>
<!-- TF Static Transforms to World -->
<node pkg="tf" type="static_transform_publisher" name="brix_to_global2"
args="0.295729 -0.463885 -0.0325348 0 0 0 global_space /$(arg tracker_name)_camera_frame 100"/>
<node pkg="tf" type="static_transform_publisher" name="tfcameratorobot" args="1.05 0.29 1.65 0 0 -3.14159265359/2 global_space brix_2_camera_frame 100" />
<!--tx: 0.295729 ty: -0.463885 tz: -0.0325348 rw: 1 rx: 0 ry: 0 rz: 0-->
<node pkg="tf" type="static_transform_publisher" name="tfcameratorobot" args="1.05 0.29 1.65 0 0 -1.57079632679 brix_2_camera_frame world 100" />
<node pkg="iri_foot" type ="iri_foot" name="iri_foot"/>
</launch>
iri_foot/src/foot_alg_node.cpp
View file @ 75a7fc2e
......@@ -32,43 +32,55 @@ void FootAlgNode::mainNodeThread(void)
{
string my_var = "";
try{
ros::Time now = ros::Time(0);
ros::Time zero = ros::Time(0);
ros::Duration three_seconds = ros::Duration(3.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");
string child_distancesafeleft("/brix_2/user_1/distancesafeleft");
string child_distancesaferight("/brix_2/user_1/distancesaferight");
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);
for(int i=0; i<100; ++i)
{
//left_foot
listener.waitForTransform(parent_cameraframe, child_leftfoot, now, three_seconds);
listener.lookupTransform(parent_cameraframe, child_leftfoot, now, transform1);
listener.waitForTransform(parent_cameraframe, child_leftfoot, zero, three_seconds);
listener.lookupTransform(parent_cameraframe, child_leftfoot, zero, transform1);
leftfoot = transform1.getOrigin();
sumalfoot = sumalfoot + leftfoot;
//left_knee
listener.waitForTransform(parent_cameraframe, child_leftknee, now, three_seconds);
listener.lookupTransform(parent_cameraframe, child_leftknee, now, transform2);
listener.waitForTransform(parent_cameraframe, child_leftknee, zero, three_seconds);
listener.lookupTransform(parent_cameraframe, child_leftknee, zero, transform2);
leftknee = transform2.getOrigin();
sumalknee = sumalknee + leftknee;
//right_foot
listener.waitForTransform(parent_cameraframe, child_rightfoot, now, three_seconds);
listener.lookupTransform(parent_cameraframe, child_rightfoot, now, transform3);
listener.waitForTransform(parent_cameraframe, child_rightfoot, zero, three_seconds);
listener.lookupTransform(parent_cameraframe, child_rightfoot, zero, transform3);
rightfoot = transform3.getOrigin();
sumarfoot = sumarfoot + rightfoot;
//right_knee
listener.waitForTransform(parent_cameraframe, child_rightknee, now, three_seconds);
listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4);
listener.waitForTransform(parent_cameraframe, child_rightknee, zero, three_seconds);
listener.lookupTransform(parent_cameraframe, child_rightknee, zero, transform4);
rightknee = transform4.getOrigin();
sumarknee = sumarknee + rightknee;
//safe distance leftfoot
transform5.setOrigin(tf::Vector3(0.2, 0.2, 0.2));
transform5.setRotation(tf::Quaternion(0, 0, 0, 1));
broadcaster.sendTransform(tf::StampedTransform(transform5, ros::Time::now(), child_leftfoot, child_distancesafeleft));
//safe distance rightfoot
transform6.setOrigin(tf::Vector3(0.2, 0.2, 0.2));
transform6.setRotation(tf::Quaternion(0, 0, 0, 1));
broadcaster.sendTransform(tf::StampedTransform(transform6, ros::Time::now(), child_rightfoot, child_distancesaferight));
}
leftfoot = sumalfoot/100;
......@@ -86,17 +98,17 @@ void FootAlgNode::mainNodeThread(void)
//stringstream converter; converter << leftfoot(1); my_var = converter.str();
//stringstream converter; converter << angleft; my_var = converter.str();
if ((angleft >= 35.0) and (angright >= 35.0))
if ((angleft >= 25.0) and (angright >= 25.0))
{
my_var = "Left foot and right foot extended";
}
else if (angleft >= 35.0)
else if (angleft >= 25.0)
{
my_var = "Left foot extended";
}
else if (angright >= 35.0)
else if (angright >= 25.0)
{
my_var = "Right foot extended";
}
......@@ -107,12 +119,12 @@ void FootAlgNode::mainNodeThread(void)
}
//Apply the distance (20 cm) between the foot and the gripper (The safe distance).
leftfoot.setX(leftfoot.getX() + 0.2);
/*leftfoot.setX(leftfoot.getX() + 0.2);
leftfoot.setY(leftfoot.getY() + 0.2);
leftfoot.setZ(leftfoot.getZ() + 0.2);
rightfoot.setX(rightfoot.getX() + 0.2);
rightfoot.setY(rightfoot.getY() + 0.2);
rightfoot.setZ(rightfoot.getZ() + 0.2);
rightfoot.setZ(rightfoot.getZ() + 0.2);*/
}
......
iri_foot/src/foot_alg_node.cpp~
View file @ 75a7fc2e
......@@ -32,43 +32,55 @@ void FootAlgNode::mainNodeThread(void)
{
string my_var = "";
try{
ros::Time now = ros::Time(0);
ros::Time zero = ros::Time(0);
ros::Duration three_seconds = ros::Duration(3.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");
string child_distancesafeleft("/brix_2/user_1/distancesafeleft");
string child_distancesaferight("/brix_2/user_1/distancesaferight");
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);
for(int i=0; i<100; ++i)
{
//left_foot
listener.waitForTransform(parent_cameraframe, child_leftfoot, now, three_seconds);
listener.lookupTransform(parent_cameraframe, child_leftfoot, now, transform1);
listener.waitForTransform(parent_cameraframe, child_leftfoot, zero, three_seconds);
listener.lookupTransform(parent_cameraframe, child_leftfoot, zero, transform1);
leftfoot = transform1.getOrigin();
sumalfoot = sumalfoot + leftfoot;
//left_knee
listener.waitForTransform(parent_cameraframe, child_leftknee, now, three_seconds);
listener.lookupTransform(parent_cameraframe, child_leftknee, now, transform2);
listener.waitForTransform(parent_cameraframe, child_leftknee, zero, three_seconds);
listener.lookupTransform(parent_cameraframe, child_leftknee, zero, transform2);
leftknee = transform2.getOrigin();
sumalknee = sumalknee + leftknee;
//right_foot
listener.waitForTransform(parent_cameraframe, child_rightfoot, now, three_seconds);
listener.lookupTransform(parent_cameraframe, child_rightfoot, now, transform3);
listener.waitForTransform(parent_cameraframe, child_rightfoot, zero, three_seconds);
listener.lookupTransform(parent_cameraframe, child_rightfoot, zero, transform3);
rightfoot = transform3.getOrigin();
sumarfoot = sumarfoot + rightfoot;
//right_knee
listener.waitForTransform(parent_cameraframe, child_rightknee, now, three_seconds);
listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4);
listener.waitForTransform(parent_cameraframe, child_rightknee, zero, three_seconds);
listener.lookupTransform(parent_cameraframe, child_rightknee, zero, transform4);
rightknee = transform4.getOrigin();
sumarknee = sumarknee + rightknee;
//safe distance leftfoot
transform5.setOrigin(tf::Vector3(0.2, 0.2, 0.2));
transform5.setRotation(tf::Quaternion(0, 0, 0, 1));
broadcaster.sendTransform(tf::StampedTransform(transform5, ros::Time::now(), child_leftfoot, child_distancesafeleft));
//safe distance rightfoot
transform6.setOrigin(tf::Vector3(0.2, 0.2, 0.2));
transform6.setRotation(tf::Quaternion(0, 0, 0, 1));
broadcaster.sendTransform(tf::StampedTransform(transform6, ros::Time::now(), child_rightfoot, child_distancesaferight));
}
leftfoot = sumalfoot/100;
......@@ -86,17 +98,17 @@ void FootAlgNode::mainNodeThread(void)
//stringstream converter; converter << leftfoot(1); my_var = converter.str();
//stringstream converter; converter << angleft; my_var = converter.str();
if ((angleft >= 35.0) and (angright >= 35.0))
if ((angleft >= 25.0) and (angright >= 25.0))
{
my_var = "Left foot and right foot extended";
}
else if (angleft >= 35.0)
else if (angleft >= 25.0)
{
my_var = "Left foot extended";
}
else if (angright >= 35.0)
else if (angright >= 25.0)
{
my_var = "Right foot extended";
}
......@@ -105,17 +117,18 @@ void FootAlgNode::mainNodeThread(void)
{
my_var = "No foot extended";
}
//Apply the distance (20 cm) between the foot and the gripper (The safe distance).
//Apply the distance (20 cm) between the foot and the gripper (The safe distance).
leftfoot.setX(leftfoot.getX() + 0.2);
leftfoot.setY(leftfoot.getY() + 0.2);
leftfoot.setZ(leftfoot.getZ() + 0.2);
rightfoot.setX(rightfoot.getX() + 0.2);
rightfoot.setY(rightfoot.getY() + 0.2);
rightfoot.setZ(rightfoot.getZ() + 0.2);
/*leftfoot.setX(leftfoot.getX() + 0.2);
leftfoot.setY(leftfoot.getY() + 0.2);
leftfoot.setZ(leftfoot.getZ() + 0.2);
rightfoot.setX(rightfoot.getX() + 0.2);
rightfoot.setY(rightfoot.getY() + 0.2);
rightfoot.setZ(rightfoot.getZ() + 0.2);*/
}
catch (tf::TransformException &ex)
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
......
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