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

Until Friday 21 April

parent 75a7fc2e
Hide whitespace changes
Inline Side-by-side
iri_foot/CMakeLists.txt
View file @ da4bd4d3
......@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
find_package(catkin REQUIRED COMPONENTS iri_base_algorithm std_msgs tf)
find_package(catkin REQUIRED COMPONENTS iri_base_algorithm geometry_msgs std_msgs tf)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
......@@ -60,7 +60,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
CATKIN_DEPENDS iri_base_algorithm
CATKIN_DEPENDS iri_base_algorithm geometry_msgs
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
......@@ -94,6 +94,7 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
# Add message headers dependencies
# ********************************************************************
#add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} geometry_msgs_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} std_msgs_generate_messages_cpp)
# ********************************************************************
# Add dynamic reconfigure dependencies
......
iri_foot/include/foot_alg.h~
View file @ da4bd4d3
......@@ -127,6 +127,7 @@ class FootAlgorithm
*
*/
~FootAlgorithm(void);
void angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4, double& angleft, double& angright);
};
......
iri_foot/include/foot_alg_node.h
View file @ da4bd4d3
......@@ -31,6 +31,7 @@
#include <tf/transform_broadcaster.h>
// [publisher subscriber headers]
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>
......@@ -53,6 +54,8 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::StampedTransform transform4;
tf::StampedTransform transform5;
tf::StampedTransform transform6;
tf::StampedTransform transformrobdistsafe;
tf::Transformer transformer1;
tf::Vector3 rightfoot;
tf::Vector3 leftfoot;
tf::Vector3 leftknee;
......@@ -62,8 +65,13 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::Vector3 sumalknee;
tf::Vector3 sumarknee;
tf::Vector3 distancesafe;
tf::Vector3 robot;
tf::Vector3 goal;
// [publisher attributes]
ros::Publisher pose_surface_publisher_;
geometry_msgs::PoseStamped pose_surface_PoseStamped_msg_;
ros::Publisher myfeet_publisher_;
std_msgs::String myfeet_String_msg_;
......
iri_foot/include/foot_alg_node.h~
View file @ da4bd4d3
......@@ -31,6 +31,7 @@
#include <tf/transform_broadcaster.h>
// [publisher subscriber headers]
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>
......@@ -52,6 +53,9 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::StampedTransform transform3;
tf::StampedTransform transform4;
tf::StampedTransform transform5;
tf::StampedTransform transform6;
tf::StampedTransform transformrobdistsafe;
tf::Transformer transformer1;
tf::Vector3 rightfoot;
tf::Vector3 leftfoot;
tf::Vector3 leftknee;
......@@ -61,8 +65,13 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::Vector3 sumalknee;
tf::Vector3 sumarknee;
tf::Vector3 distancesafe;
tf::Vector3 robot;
tf::Vector3 transformation;
// [publisher attributes]
ros::Publisher pose_surface_publisher_;
geometry_msgs::PoseStamped pose_surface_PoseStamped_msg_;
ros::Publisher myfeet_publisher_;
std_msgs::String myfeet_String_msg_;
......
iri_foot/launch/foot.launch
View file @ da4bd4d3
<launch>
<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="tf" type="static_transform_publisher" name="tfcameratorobot" args="1.32 -0.22 1.36 0 3.14159265359 -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 @ da4bd4d3
<launch>
<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="tf" type="static_transform_publisher" name="tfcameratorobot" args="1.32 -0.22 1.36 0 3.14159265359 -1.57079632679 brix_2_camera_frame world 100" />
<node pkg="iri_foot" type ="iri_foot" name="iri_foot"/>
</launch>
iri_foot/package.xml
View file @ da4bd4d3
......@@ -41,9 +41,11 @@
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>iri_base_algorithm</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf</build_depend>
<run_depend>iri_base_algorithm</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>tf</run_depend>
<!-- The export tag contains other, unspecified, tags -->
......
iri_foot/src/foot_alg_node.cpp
View file @ da4bd4d3
......@@ -10,6 +10,7 @@ FootAlgNode::FootAlgNode(void) :
//this->loop_rate_ = 2;//in [Hz]
// [init publishers]
this->pose_surface_publisher_ = this->public_node_handle_.advertise<geometry_msgs::PoseStamped>("pose_surface", 10);
this->myfeet_publisher_ = this->public_node_handle_.advertise<std_msgs::String>("myfeet", 10);
// [init subscribers]
......@@ -41,6 +42,7 @@ void FootAlgNode::mainNodeThread(void)
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");
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);
......@@ -81,6 +83,11 @@ void FootAlgNode::mainNodeThread(void)
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));
//obtain the transformation between the robot and the distance safe
transformer1.lookupTransform(child_distancesafeleft, child_robot, ros::Time::now(),transformrobdistsafe);
goal = transformrobdistsafe.getOrigin();
}
leftfoot = sumalfoot/100;
......@@ -98,17 +105,17 @@ void FootAlgNode::mainNodeThread(void)
//stringstream converter; converter << leftfoot(1); my_var = converter.str();
//stringstream converter; converter << angleft; my_var = converter.str();
if ((angleft >= 25.0) and (angright >= 25.0))
if ((angleft >= 35.0) and (angright >= 35.0))
{
my_var = "Left foot and right foot extended";
}
else if (angleft >= 25.0)
else if (angleft >= 35.0)
{
my_var = "Left foot extended";
}
else if (angright >= 25.0)
else if (angright >= 35.0)
{
my_var = "Right foot extended";
}
......@@ -117,15 +124,6 @@ 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);*/
}
catch (tf::TransformException &ex)
......@@ -135,6 +133,11 @@ void FootAlgNode::mainNodeThread(void)
}
// [fill msg structures]
// Initialize the topic message structure
this->pose_surface_PoseStamped_msg_ = goal;
// Initialize the topic message structure
// Initialize the topic message structure
this->myfeet_String_msg_.data = my_var;
......@@ -144,6 +147,11 @@ void FootAlgNode::mainNodeThread(void)
// [fill action structure and make request to the action server]
// [publish messages]
// Uncomment the following line to publish the topic message
this->pose_surface_publisher_.publish(this->pose_surface_PoseStamped_msg_);
// Uncomment the following line to publish the topic message
// Uncomment the following line to publish the topic message
this->myfeet_publisher_.publish(this->myfeet_String_msg_);
......
iri_foot/src/foot_alg_node.cpp~
View file @ da4bd4d3
......@@ -10,6 +10,7 @@ FootAlgNode::FootAlgNode(void) :
//this->loop_rate_ = 2;//in [Hz]
// [init publishers]
this->pose_surface_publisher_ = this->public_node_handle_.advertise<geometry_msgs::PoseStamped>("pose_surface", 10);
this->myfeet_publisher_ = this->public_node_handle_.advertise<std_msgs::String>("myfeet", 10);
// [init subscribers]
......@@ -41,6 +42,7 @@ void FootAlgNode::mainNodeThread(void)
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");
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);
......@@ -81,6 +83,11 @@ void FootAlgNode::mainNodeThread(void)
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));
//obtain the transformation between the robot and the distance safe
transformer1.lookupTransform(child_distancesafeleft, child_robot, ros::Time::now(),transformrobdistsafe);
goal = transformrobdistsafe.getOrigin();
}
leftfoot = sumalfoot/100;
......@@ -98,17 +105,17 @@ void FootAlgNode::mainNodeThread(void)
//stringstream converter; converter << leftfoot(1); my_var = converter.str();
//stringstream converter; converter << angleft; my_var = converter.str();
if ((angleft >= 25.0) and (angright >= 25.0))
if ((angleft >= 35.0) and (angright >= 35.0))
{
my_var = "Left foot and right foot extended";
}
else if (angleft >= 25.0)
else if (angleft >= 35.0)
{
my_var = "Left foot extended";
}
else if (angright >= 25.0)
else if (angright >= 35.0)
{
my_var = "Right foot extended";
}
......@@ -117,15 +124,6 @@ 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);*/
}
catch (tf::TransformException &ex)
......@@ -135,6 +133,11 @@ void FootAlgNode::mainNodeThread(void)
}
// [fill msg structures]
// Initialize the topic message structure
this->pose_surface_PoseStamped_msg = goal;
// Initialize the topic message structure
// Initialize the topic message structure
this->myfeet_String_msg_.data = my_var;
......@@ -144,6 +147,11 @@ void FootAlgNode::mainNodeThread(void)
// [fill action structure and make request to the action server]
// [publish messages]
// Uncomment the following line to publish the topic message
this->pose_surface_publisher_.publish(this->pose_surface_PoseStamped_msg_);
// Uncomment the following line to publish the topic message
// Uncomment the following line to publish the topic message
this->myfeet_publisher_.publish(this->myfeet_String_msg_);
......
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