Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Functionality for PCL transmission to ROS and automatic registration using ROS subscribers. #22

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
Open

Functionality for PCL transmission to ROS and automatic registration using ROS subscribers. #22

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
0503c7e
Modifications for PCL publishing.
Mar 20, 2014
a24e215
Worked on running multiple versions of the state_estimation node base…
dgitz Apr 8, 2014
4a1241a
Got state_estimation to work with vrep video, simultaneously with liv…
Apr 8, 2014
f84e7a5
Worked on pcl stuff, image transform.
Apr 10, 2014
46ccd2f
Modified code to allow for SLAM registration based on a ROS topic sub…
dgitz Apr 16, 2014
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
173 changes: 162 additions & 11 deletions src/stateestimation/EstimationNode.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,21 +41,43 @@
#include "MapView.h"
#include <sys/stat.h>
#include <string>
#include "geometry_msgs/PoseStamped.h" //DPG 18-MARCH-2014
#include "sensor_msgs/PointField.h" //DPG 18-MARCH-2014
#include "sensor_msgs/PointCloud2.h" //DPG 18-MARCH-2014

using namespace std;

pthread_mutex_t EstimationNode::logIMU_CS = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t EstimationNode::logPTAM_CS = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t EstimationNode::logFilter_CS = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t EstimationNode::logPTAMRaw_CS = PTHREAD_MUTEX_INITIALIZER;
extern string drone_mode;

EstimationNode::EstimationNode()
{
navdata_channel = nh_.resolveName("ardrone/navdata");
control_channel = nh_.resolveName("cmd_vel");
output_channel = nh_.resolveName("ardrone/predictedPose");
video_channel = nh_.resolveName("ardrone/image_raw");

if (drone_mode.compare("Live") == 0)
{
video_channel = nh_.resolveName("ardrone/image_raw");
navdata_channel = nh_.resolveName("ardrone/navdata");
navdata_sub = nh_.subscribe(navdata_channel, 10, &EstimationNode::navdataCb, this);

}
else if (drone_mode.compare("Sim") == 0)
{
video_channel = nh_.resolveName("vrep/front_cam");
navdata_channel = nh_.resolveName("vrep/navdata");
navdata_sub = nh_.subscribe(navdata_channel, 10, &EstimationNode::navdataCb, this);
}
ptam_channel = nh_.resolveName("cmd_register");
ptam_sub = nh_.subscribe(ptam_channel,1,&EstimationNode::ptamCb, this);
control_channel = nh_.resolveName("cmd_vel");
output_channel = nh_.resolveName("ardrone/predictedPose");
std_pose_channel = nh_.resolveName("/ardrone/std_pose"); //DPG 18-MARCH-2014
point_cloud_channel = nh_.resolveName("/ardrone/point_cloud"); //DPG 18-MARCH-2014

command_channel = nh_.resolveName("tum_ardrone/com");

packagePath = ros::package::getPath("tum_ardrone");

std::string val;
Expand All @@ -80,10 +102,11 @@ EstimationNode::EstimationNode()
cout << "set calibFile to DEFAULT" << endl;


navdata_sub = nh_.subscribe(navdata_channel, 10, &EstimationNode::navdataCb, this);

vel_sub = nh_.subscribe(control_channel,10, &EstimationNode::velCb, this);
vid_sub = nh_.subscribe(video_channel,10, &EstimationNode::vidCb, this);

drone_std_pose_pub = nh_.advertise<geometry_msgs::PoseStamped>(std_pose_channel,1); //DPG 18-MARCH-2014
drone_point_cloud_pub = nh_.advertise<sensor_msgs::PointCloud2>(point_cloud_channel,1); //DPG 18-MARCH-2014
dronepose_pub = nh_.advertise<tum_ardrone::filter_state>(output_channel,1);

tum_ardrone_pub = nh_.advertise<std_msgs::String>(command_channel,50);
Expand All @@ -96,6 +119,7 @@ EstimationNode::EstimationNode()
currentLogID = 0;
lastDroneTS = 0;
lastRosTS = 0;
lastKF = 0; //DPG 18-MARCH-2014
droneRosTSOffset = 0;
lastNavStamp = ros::Time(0);
filter = new DroneKalmanFilter(this);
Expand All @@ -117,8 +141,11 @@ EstimationNode::~EstimationNode()

//delete infoQueue;
}

void EstimationNode::navdataCb(const ardrone_autonomy::NavdataConstPtr navdataPtr)
{
//static tf::TransformBroadcaster br;

lastNavdataReceived = *navdataPtr;
if(ros::Time::now() - lastNavdataReceived.header.stamp > ros::Duration(30.0))
lastNavdataReceived.header.stamp = ros::Time::now();
Expand Down Expand Up @@ -153,7 +180,8 @@ void EstimationNode::navdataCb(const ardrone_autonomy::NavdataConstPtr navdataPt
lastRosTS = rosTS;
lastDroneTS = droneTS;


//DPG 10-APR-2014
if (drone_mode.compare("Sim") == 0) {} //Code to convert from Quaternion to RPY
// convert to originally sent drone values (undo ardrone_autonomy changes)
lastNavdataReceived.rotZ *= -1; // yaw inverted
lastNavdataReceived.rotY *= -1; // pitch inverted
Expand Down Expand Up @@ -216,7 +244,30 @@ void EstimationNode::vidCb(const sensor_msgs::ImageConstPtr img)
// give to PTAM
ptamWrapper->newImage(img);
}

void EstimationNode::ptamCb(const std_msgs::StringConstPtr str)
{
//printf("Received: %d\n",str->data.length());
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

as already mentioned by @lesire remove such comments.

if (str->data.length() > 2 && str->data.substr(0,5) == "reset")
{
printf("Reset\n");
publishCommand("p reset");
//PTAMWrapper->publishCommand("p reset");
}
else if (str->data.length() > 2 && str->data.substr(0,6) == "finish")
{
printf("Finishing Registration\n");
publishCommand("p space");
}
else if (str->data.length() > 2 && str->data.substr(0,5) == "start")
{
printf("Starting Registration\n");
publishCommand("p space");
}
else
{
printf("Unknown\n");
}
}
void EstimationNode::comCb(const std_msgs::StringConstPtr str)
{
if(str->data.length() > 2 && str->data.substr(0,2) == "p ")
Expand Down Expand Up @@ -268,6 +319,7 @@ void EstimationNode::Loop()
// get filter state msg
pthread_mutex_lock( &filter->filter_CS );
tum_ardrone::filter_state s = filter->getPoseAt(ros::Time().now() + predTime);
tum_ardrone::filter_state t_pose = filter->getCurrentPoseSpeed(); //DPG 18-MARCH-2014
pthread_mutex_unlock( &filter->filter_CS );

// fill metadata
Expand All @@ -281,7 +333,93 @@ void EstimationNode::Loop()
// publish!
dronepose_pub.publish(s);

//DPG 18-MARCH-2014
geometry_msgs::PoseStamped pose_stmp;
pose_stmp.header.stamp = ros::Time().now();
pose_stmp.header.frame_id = "/cam_front";
pose_stmp.pose.position.x = t_pose.x;
pose_stmp.pose.position.y = t_pose.y;
pose_stmp.pose.position.z = t_pose.z;
geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(t_pose.roll*3.1415/180,t_pose.pitch*3.1415/180,(-t_pose.yaw+90)*3.1415/180);

pose_stmp.pose.orientation = q;

drone_std_pose_pub.publish(pose_stmp);



// POINT CLOUD 2

int currentKF = ptamWrapper->keyFramesTransformed.size() ;
//std::cout << currentKF << " Key Frames Transformed." << std::endl; // DPG 18-MARCH-2014
if (currentKF > lastKF) {
lastKF = currentKF;

sensor_msgs::PointCloud2 point_cloud;

pthread_mutex_lock(&ptamWrapper->shallowMapCS);

std::vector<tvec3> mpl = ptamWrapper->mapPointsTransformed;
int dimension = 3;

point_cloud.width = mpl.size();

point_cloud.header.stamp = ros::Time().now();
point_cloud.header.frame_id = "/cam_front";
point_cloud.height = 1;

point_cloud.fields.resize(dimension);

point_cloud.fields[0].name = "x";
point_cloud.fields[0].offset = 0*sizeof(uint32_t);
point_cloud.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
point_cloud.fields[0].count = 1;

point_cloud.fields[1].name = "y";
point_cloud.fields[1].offset = 1*sizeof(uint32_t);
point_cloud.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
point_cloud.fields[1].count = 1;

point_cloud.fields[2].name = "z";
point_cloud.fields[2].offset = 2*sizeof(uint32_t);
point_cloud.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
point_cloud.fields[2].count = 1;

/* DPG 20-MARCH-2014
point_cloud.fields[3].name = "rgb";
point_cloud.fields[3].offset = 3*sizeof(uint32_t);
point_cloud.fields[3].datatype = sensor_msgs::PointField::INT32;
point_cloud.fields[3].count = 1;*/



point_cloud.point_step = dimension*sizeof(uint32_t);
point_cloud.row_step = point_cloud.point_step * point_cloud.width;

point_cloud.data.resize(point_cloud.row_step * point_cloud.height);

point_cloud.is_dense = false;

unsigned char* dat = &(point_cloud.data[0]);

for(unsigned int i=0 ; i < point_cloud.width; i++)
{
float pos[] = {mpl[i][0], mpl[i][1], mpl[i][2]};
memcpy(dat, pos ,3*sizeof(float));

//DPG 20-MARCH-2014
//uint32_t colorlvl = 0xffffffff;
//memcpy(dat+3*sizeof(uint32_t),&colorlvl,sizeof(uint32_t));

dat+=point_cloud.point_step;

}

pthread_mutex_unlock(&ptamWrapper->shallowMapCS);

drone_point_cloud_pub.publish(point_cloud);
printf("Size of pc: %d\n",point_cloud.width);
}
// --------- if need be: add fake PTAM obs --------
// if PTAM updates hang (no video or e.g. init), filter is never permanently rolled forward -> queues get too big.
// dont allow this to happen by faking a ptam observation if queue gets too big (500ms = 100 observations)
Expand Down Expand Up @@ -363,12 +501,24 @@ void EstimationNode::publishTf(TooN::SE3<> trans, ros::Time stamp, int seq, std:
v[0] = trans.get_translation()[0];
v[1] = trans.get_translation()[1];
v[2] = trans.get_translation()[2];



//Tried: 102,012,

tf::Transform tr = tf::Transform(m,v);
tf::StampedTransform t = tf::StampedTransform(tr,stamp,"map",system);

//tf::StampedTransform t = tf::StampedTransform(tr,stamp,"map",system); //DPG 19-MARCH-2014
tf::StampedTransform t = tf::StampedTransform(tr,ros::Time::now(),"/base_link","/cam_front"); //DPG 19-MARCH-2014
tf_broadcaster.sendTransform(t);

// DPG 18-MARCH-2014
//std::cout << system << "" << std::endl;
/*static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin(tf::Vector3(0.0,0.0,0.0));
transform.setRotation(tf::Quaternion(0.0,0.0,0.0));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", system));*/




if(logfilePTAMRaw != NULL)
Expand Down Expand Up @@ -576,3 +726,4 @@ void EstimationNode::reSendInfo()

publishCommand(buf);
}

Loading