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

Iri foot

parent da6e4822
Hide whitespace changes
Inline Side-by-side
iri_foot/launch/foot.launch
View file @ 68976381
......@@ -8,6 +8,7 @@
<!-- 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="iri_foot" type ="iri_foot" name="iri_foot"/>
......
iri_foot/launch/foot.launch~
View file @ 68976381
......@@ -8,6 +8,7 @@
<!-- 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="iri_foot" type ="iri_foot" name="iri_foot"/>
......
iri_foot/src/foot_alg_node.cpp
View file @ 68976381
......@@ -33,7 +33,7 @@ void FootAlgNode::mainNodeThread(void)
string my_var = "";
try{
ros::Time now = ros::Time(0);
ros::Duration five_seconds = ros::Duration(3.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");
......@@ -44,36 +44,37 @@ void FootAlgNode::mainNodeThread(void)
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<30; ++i)
for(int i=0; i<100; ++i)
{
//left_foot
listener.waitForTransform(parent_cameraframe, child_leftfoot, now, five_seconds);listener.lookupTransform(parent_cameraframe, child_leftfoot, now, transform1);
listener.waitForTransform(parent_cameraframe, child_leftfoot, now, three_seconds);
listener.lookupTransform(parent_cameraframe, child_leftfoot, now, transform1);
leftfoot = transform1.getOrigin();
sumalfoot = sumalfoot + leftfoot;
//left_knee
listener.waitForTransform(parent_cameraframe, child_leftknee, now, five_seconds);
listener.waitForTransform(parent_cameraframe, child_leftknee, now, three_seconds);
listener.lookupTransform(parent_cameraframe, child_leftknee, now, transform2);
leftknee = transform2.getOrigin();
sumalknee = sumalknee + leftknee;
//right_foot
listener.waitForTransform(parent_cameraframe, child_rightfoot, now, five_seconds);
listener.waitForTransform(parent_cameraframe, child_rightfoot, now, three_seconds);
listener.lookupTransform(parent_cameraframe, child_rightfoot, now, transform3);
rightfoot = transform3.getOrigin();
sumarfoot = sumarfoot + rightfoot;
//right_knee
listener.waitForTransform(parent_cameraframe, child_rightknee, now, five_seconds);
listener.waitForTransform(parent_cameraframe, child_rightknee, now, three_seconds);
listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4);
rightknee = transform4.getOrigin();
sumarknee = sumarknee + rightknee;
}
leftfoot = sumalfoot/30;
leftknee = sumalknee/30;
rightfoot = sumarfoot/30;
rightknee = sumarknee/30;
leftfoot = sumalfoot/100;
leftknee = sumalknee/100;
rightfoot = sumarfoot/100;
rightknee = sumarknee/100;
......@@ -83,19 +84,19 @@ void FootAlgNode::mainNodeThread(void)
double angright = 0.0;
alg_.angle(leftfoot, leftknee, rightfoot, rightknee, angleft, angright);
//stringstream converter; converter << leftfoot(1); my_var = converter.str();
stringstream converter; converter << angleft; my_var = converter.str();
//stringstream converter; converter << angleft; my_var = converter.str();
/*if ((angleft >= 45.0) and (angright >= 45.0))
if ((angleft >= 35.0) and (angright >= 35.0))
{
my_var = "Left foot and right foot extended";
}
else if (angleft >= 45.0)
else if (angleft >= 35.0)
{
my_var = "Left foot extended";
}
else if (angright >= 45.0)
else if (angright >= 35.0)
{
my_var = "Right foot extended";
}
......@@ -104,20 +105,24 @@ void FootAlgNode::mainNodeThread(void)
{
my_var = "No foot extended";
}
*/
}
//Apply the distance (20 cm) between the foot and the gripper (The safe distance).
//leftfoot = transform1.getOrigin() ;
//rightfoot = transform3.getOrigin();
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)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
// [fill msg structures]
// [fill msg structures]
// Initialize the topic message structure
this->myfeet_String_msg_.data = my_var;
......
iri_foot/src/foot_alg_node.cpp~
View file @ 68976381
......@@ -33,47 +33,48 @@ void FootAlgNode::mainNodeThread(void)
string my_var = "";
try{
ros::Time now = ros::Time(0);
ros::Duration five_seconds = ros::Duration(3.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");
sumalfoot = 0;
sumarfoot = 0;
sumalknee = 0;
sumarknee = 0;
for(int i=0; i<30; ++i)
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, five_seconds);listener.lookupTransform(parent_cameraframe, child_leftfoot, now, transform1);
listener.waitForTransform(parent_cameraframe, child_leftfoot, now, three_seconds);
listener.lookupTransform(parent_cameraframe, child_leftfoot, now, transform1);
leftfoot = transform1.getOrigin();
sumalfoot = sumalfoot + leftfoot;
//left_knee
listener.waitForTransform(parent_cameraframe, child_leftknee, now, five_seconds);
listener.waitForTransform(parent_cameraframe, child_leftknee, now, three_seconds);
listener.lookupTransform(parent_cameraframe, child_leftknee, now, transform2);
leftknee = transform2.getOrigin();
sumalknee = sumalknee + leftknee;
//right_foot
listener.waitForTransform(parent_cameraframe, child_rightfoot, now, five_seconds);
listener.waitForTransform(parent_cameraframe, child_rightfoot, now, three_seconds);
listener.lookupTransform(parent_cameraframe, child_rightfoot, now, transform3);
rightfoot = transform3.getOrigin();
sumarfoot = sumarfoot + rightfoot;
//right_knee
listener.waitForTransform(parent_cameraframe, child_rightknee, now, five_seconds);
listener.waitForTransform(parent_cameraframe, child_rightknee, now, three_seconds);
listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4);
rightknee = transform4.getOrigin();
sumarknee = sumarknee + rightknee;
}
leftfoot = sumalfoot/30;
leftknee = sumalknee/30;
rightfoot = sumarfoot/30;
rightknee = sumarknee/30;
leftfoot = sumalfoot/100;
leftknee = sumalknee/100;
rightfoot = sumarfoot/100;
rightknee = sumarknee/100;
......@@ -83,19 +84,19 @@ void FootAlgNode::mainNodeThread(void)
double angright = 0.0;
alg_.angle(leftfoot, leftknee, rightfoot, rightknee, angleft, angright);
//stringstream converter; converter << leftfoot(1); my_var = converter.str();
stringstream converter; converter << angleft; my_var = converter.str();
//stringstream converter; converter << angleft; my_var = converter.str();
/*if ((angleft >= 45.0) and (angright >= 45.0))
if ((angleft >= 35.0) and (angright >= 35.0))
{
my_var = "Left foot and right foot extended";
}
else if (angleft >= 45.0)
else if (angleft >= 35.0)
{
my_var = "Left foot extended";
}
else if (angright >= 45.0)
else if (angright >= 35.0)
{
my_var = "Right foot extended";
}
......@@ -104,20 +105,23 @@ void FootAlgNode::mainNodeThread(void)
{
my_var = "No foot extended";
}
*/
//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);
}
//Apply the distance (20 cm) between the foot and the gripper (The safe distance).
//leftfoot = transform1.getOrigin() ;
//rightfoot = transform3.getOrigin();
catch (tf::TransformException &ex)
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
// [fill msg structures]
// [fill msg structures]
// Initialize the topic message structure
this->myfeet_String_msg_.data = my_var;
......
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