//! Loop forever while sending drive commands based on keyboard input
bool driveKeyboard()
{
char cmd[50];
m3ctrl_msgs::M3JointCmd humanoid_cmd;
humanoid_cmd.chain.resize(1);
humanoid_cmd.stiffness.resize(1);
humanoid_cmd.position.resize(1);
humanoid_cmd.velocity.resize(1);
humanoid_cmd.effort.resize(1);
humanoid_cmd.control_mode.resize(1);
humanoid_cmd.smoothing_mode.resize(1);
humanoid_cmd.chain_idx.resize(1);
// center robot first
std::cout << "Example: commanding right arm J0.\n";
std::cout << "Press any key to move to zero position.\n";
std::cin.getline(cmd, 50);
//humanoid_cmd.chain[0] = (unsigned char)RIGHT_ARM; // chain name: RIGHT_ARM, HEAD, RIGHT_HAND, LEFT_ARM, or LEFT_HAND
humanoid_cmd.chain[0] = (unsigned char)LEFT_GRIPPER; // chain name: RIGHT_ARM, HEAD, or RIGHT_HAND
humanoid_cmd.chain_idx[0] = 0; //J0
humanoid_cmd.control_mode[0] = (unsigned char)JOINT_MODE_ROS_THETA; //Compliant position mode
//humanoid_cmd.control_mode[0] = (unsigned char)JOINT_MODE_ROS_THETA; //Use for HEAD
humanoid_cmd.smoothing_mode[0] = (unsigned char)SMOOTHING_MODE_SLEW; //Smooth trajectory
//humanoid_cmd.smoothing_mode[0] = (unsigned char)SMOOTHING_MODE_MIN_JERK; //Use for HEAD
humanoid_cmd.velocity[0] = 1.0; //Rad/s
humanoid_cmd.stiffness[0] = 1.0; //0-1.0
humanoid_cmd.effort[0] = 300.0; //Torque for hand fingers
humanoid_cmd.position[0] = 0.5; //Desired position (Rad)
humanoid_cmd.header.stamp = ros::Time::now();
humanoid_cmd.header.frame_id = "humanoid_cmd";
// printf("mo: %d\n",(int)humanoid_cmd.control_mode[0]);
cmd_pub_.publish(humanoid_cmd);
std::cout << "Type a command and then press enter. "
"Use 'z' to go to middle, '+' to move up, '-' to move down, "
"'.' to exit.\n";
while(nh_.ok()) {
std::cin.getline(cmd, 50);
if(cmd[0]!='+' && cmd[0]!='-' && cmd[0]!='z' && cmd[0]!='.')
{
std::cout << "unknown command:" << cmd << "\n";
continue;
}
//move forward
if(cmd[0]=='+') {
//humanoid_cmd.position[0] += 5.0 * 3.14/180.;
humanoid_cmd.position[0] += 2 * 3.14/180.;
}
//turn left (yaw) and drive forward at the same time
else if(cmd[0]=='-') {
//humanoid_cmd.position[0] -= 5 * 3.14/180.;
humanoid_cmd.position[0] -= 2 * 3.14/180.;
}
//turn right (yaw) and drive forward at the same time
else if(cmd[0]=='z') {
humanoid_cmd.position[0] = 0 * 3.14/180.;
}
//quit
else if(cmd[0]=='.') {
break;
}
humanoid_cmd.header.stamp = ros::Time::now();
//publish the assembled command
cmd_pub_.publish(humanoid_cmd);
}
humanoid_cmd.header.stamp = ros::Time::now();
cmd_pub_.publish(humanoid_cmd);
humanoid_cmd.control_mode[0] = (unsigned char)JOINT_MODE_ROS_OFF; //Turn OFF
return true;
}
void LaserLegsCallback(const people_msgs::PositionMeasurementArray::ConstPtr& msg)
{
bool validTrackLaser=false;
cmd_vel.linear.x = 0.0;
cmd_vel.angular.z = 0.0;
nbOfTracksLaser=msg->people.size();
if (nbOfTracksLaser>0) {
//Extract coordinates of first detected person
xLaserPerson=msg->people[0].pos.x;
yLaserPerson=msg->people[0].pos.y;
// ROS_INFO("xLaser: %f", xLaserPerson);
// ROS_INFO("yLaser: %f", yLaserPerson);
// ROS_INFO("AngleErrorLaser: %f", AngleErrorLaser);
if (nbOfTracksKinect==0) {
//Calculate angle error
AngleErrorLaser=atan2(yLaserPerson,xLaserPerson);
//Calculate distance error
DistanceErrorLaser=sqrt(pow(xLaserPerson,2)+pow(yLaserPerson,2));
YpathPointsLaser.insert(YpathPointsLaser.begin(),yLaserPerson);
if (YpathPointsLaser.size()>6){
yLast1Laser=YpathPointsLaser.at(5);
yLast2Laser=YpathPointsLaser.at(1);
xLast1Laser=xLaserPerson;
tempDistanceLaser=DistanceErrorLaser;
yDirectionLaser=yLast2Laser-yLast1Laser;
YpathPointsLaser.pop_back();
}
if(!laser_obstacle_flag){
angular_command=AngleErrorLaser*KpAngle;
if(angular_command>MaxTurn){angular_command=MaxTurn;}
if(angular_command<-MaxTurn){angular_command=-MaxTurn;}
cmd_vel.angular.z = angular_command;
double linearspeedLaser=(DistanceErrorLaser-DistanceTarget)*KpDistance;
if (linearspeedLaser>MaxSpeed)
{
linearspeedLaser=MaxSpeed;
}
if (linearspeedLaser<0){
linearspeedLaser=0;
}
cmd_vel.linear.x = linearspeedLaser;
cmd_vel_pub.publish(cmd_vel);
}
}
validTrackLaser=true;
laserTrack=true;
//////////////////////////////////marker
visualization_msgs::Marker marker;
marker.header.frame_id = "base_link";
marker.header.stamp = ros::Time();
marker.ns = "laser";
marker.id = 0;
marker.type = visualization_msgs::Marker::SPHERE;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = xLaserPerson;
marker.pose.position.y = yLaserPerson;
marker.pose.position.z = 0;
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = AngleErrorLaser;
marker.scale.x = 0.1;
marker.scale.y = 0.1;
marker.scale.z = 0.1;
marker.color.a = 1.0; // Don't forget to set the alpha!
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
vis_pub1.publish( marker );
////////////////////////
}
else if(!validTrackKinect){
laserTrack=false;
//////////////////////////////////
/* validTrackLaser=false;
xPathLaser= xRobot+cos(orientationRobot+AngleErrorLaser)*tempDistanceLaser;
yPathLaser= yRobot+sin(orientationRobot+AngleErrorLaser)*tempDistanceLaser;
AngleErrorFollowLaser=PI-atan2(yPathLaser-yRobot,(xPathLaser-xRobot))-PI+orientationRobot;
if(abs(AngleErrorFollowLaser)>PI){
if(AngleErrorFollowLaser<0){AngleErrorFollowLaser=AngleErrorFollowLaser+2*PI;}
else{AngleErrorFollowLaser=AngleErrorFollowLaser-2*PI;}
}
tempDistanceFollowLaser=sqrt(pow(xPathLaser-xRobot,2)+pow(yPathLaser-yRobot,2));
//Get the number of tracks in the TrackArray
nbOfTracksLaser=msg->people.size();
//If at least 1 track, proceed
if (nbOfTracksLaser>0) {
validTrackLaser=true;
}
if (!laser_obstacle_flag){
ros::Time start= ros::Time::now();
while((ros::Time::now()-start<ros::Duration(round(tempDistanceFollowLaser/0.3))) && (!validTrackLaser) && (!laser_obstacle_flag)){
cmd_vel.linear.x = 0.3;
cmd_vel.angular.z=0.0;}
cmd_vel.linear.x = 0.0;
if(!validTrackLaser){
if(yDirectionLaser>0){cmd_vel.angular.z=0.15;}
else{cmd_vel.angular.z=-0.15;}
}
//Stop for loop
// validTrack=true;
cmd_vel_pub.publish(cmd_vel);
// ROS_INFO("xLast1: %f", xLast1);
// ROS_INFO("yLast1: %f", yLast1);
// ROS_INFO("yDirection: %f", yDirection);
}
*/
/////////////////////////////////////
}
}
int main(int argc, char** argv) {
// Initialize ourselves as a ROS node.
ros::init(argc, argv, "planner");
ros::NodeHandle nh;
ros::Rate loop_rate(0.5); //hz
vis = nh.advertise<visualization_msgs::Marker> ("visualization_marker", 0);
// Subscribe to the map and people finder data.
ros::Subscriber map_sub;
ros::Subscriber people_finder_sub;
ros::Subscriber robot_pose_sub;
//ros::Subscriber costmap_sub;
//costmap_sub = nh.subscribe<nav_msgs::GridCells>("/move_base_node/local_costmap/obstacles", 1, updateCost);
map_sub = nh.subscribe<nav_msgs::OccupancyGrid> ("/map", 1, updateMap);
people_finder_sub = nh.subscribe<hide_n_seek::PeopleFinder> (
"fake_people_finder/people_finder", 1, updatePeople);
robot_pose_sub = nh.subscribe<nav_msgs::Odometry> (
"/base_pose_ground_truth", 1, updateRobotPose);
// map_inited = false;
pomdp.num_states = 400;
pomdp.num_lookahead = 1;
pomdp.states_inited = false;
//pomdp.current_state.pose.position.x = 0;
//pomdp.current_state.pose.position.y = 0;
ROS_INFO("States initialized.");
// spin the main loop
while (ros::ok()) {
ROS_INFO("service loop...");
{
State internalGoal,personGoal;
State s;
if (people.size() != 0) {
// If we have people to explore, exploring them is first priority.
personGoal.pose = people.at(0);
//this is a real world pose, now convert it into one of the pompdp state to calculate the euc dist from all poss states
personGoal.state_space_pose = realToStatePose(personGoal.pose);
s = makePlan(personGoal);
ROS_INFO("Person found at real(%f, %f)! Using them as the next goal.", personGoal.pose.position.x, internalGoal.pose.position.y);
} else if (pomdp.states_inited){
// get goal from POMDp with highest probability, if its an internal state no conversion needed
internalGoal = getMostLikelyState();
s = makePlan(internalGoal);
}
//internalGoal.state_space_pose = realToStatePose(internalGoal.pose);
// once we've initialized some states...
// make a plan
ROS_INFO("State received from makePlan: (%f, %f)", s.pose.position.x, s.pose.position.y);
if (people.size() != 0) {
people.erase(people.begin());
}
// print current goals
printGoals(vis);
// send a goal
visualization_msgs::Marker marker;
marker.header.frame_id = "map";//->header.frame_id;
marker.id = 67845;
marker.header.stamp = ros::Time();
marker.type = visualization_msgs::Marker::ARROW;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = s.pose.position.x;
marker.pose.position.y = s.pose.position.y;
marker.pose.position.z = 0;
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = -1.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
marker.scale.z = 2.0;
marker.scale.x = 2.0;
marker.scale.y = 2.0;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 1.0;
vis.publish(marker);
sendGoal(s);
ROS_INFO("Goal we're sending: (%f, %f)", s.pose.position.x, s.pose.position.y);
// set the current state
pomdp.current_state = s;
}
ros::spinOnce();
loop_rate.sleep();
}
}
int main(int argc, char** argv){
//myfile.open ("/home/bojan/svn/POW/ROS/Fuerte/Data/Beds/User1/Final_pow_freek_3_2011-04-01-14-21-48.bag.txt");
// myfile << "Yaw in deg" << " " << "Velocity in m/s" << " " << "Distance to bed in m" << "\n";
ros::init(argc, argv, "my_tf_listener");
ros::NodeHandle node;
//Visualize the BED
marker_pub = node.advertise<visualization_msgs::Marker>("BED", 10);
line.header.frame_id = "map";
line.header.stamp = ros::Time::now();
line.ns = "BED";
line.type = visualization_msgs::Marker::LINE_STRIP;
line.action = visualization_msgs::Marker::ADD;
line.id = 0;
line.color.b = 1.0;
line.color.a = 1.0;
line.scale.x = 0.1; //Sets the width of the LINE_STRIP
line.scale.y = 0.1; //Ignored if marker type is LINE_STRIP
line.points.resize(2);
//Visualize the PYLON1
//marker_pub1 = node.advertise<visualization_msgs::Marker>("PYLON", 10);
//cube1.header.frame_id = "map";
//cube1.header.stamp = ros::Time::now();
//cube1.ns = "PYLON";
//cube1.type = visualization_msgs::Marker::CUBE;
//cube1.action = visualization_msgs::Marker::ADD;
//cube1.id = 1;
//cube1.color.r = 1.0;
//cube1.color.a = 1.0;
//cube1.scale.x = 0.3; //Sets the width of the LINE_STRIP
//cube1.scale.y = 0.3; //Ignored if marker type is LINE_STRIP
//cube1.scale.z = 0.3;
// cube1.points.resize(2);
//Visualize the PYLON2
/*
marker_pub2 = node.advertise<visualization_msgs::Marker>("PYLON", 10);
cube2.header.frame_id = "map";
cube2.header.stamp = ros::Time::now();
cube2.ns = "PYLON";
cube2.type = visualization_msgs::Marker::CUBE;
cube2.action = visualization_msgs::Marker::ADD;
cube2.id = 2;
cube2.color.r = 1.0;
cube2.color.a = 1.0;
cube2.scale.x = 0.3; //Sets the width of the LINE_STRIP
cube2.scale.y = 0.3; //Ignored if marker type is LINE_STRIP
cube2.scale.z = 0.3;
cube2.points.resize(2);
//Visualize the PYLON3
marker_pub3 = node.advertise<visualization_msgs::Marker>("PYLON", 10);
cube3.header.frame_id = "map";
cube3.header.stamp = ros::Time::now();
cube3.ns = "PYLON";
cube3.type = visualization_msgs::Marker::CUBE;
cube3.action = visualization_msgs::Marker::ADD;
cube3.id = 3;
cube3.color.r = 1.0;
cube3.color.a = 1.0;
cube3.scale.x = 0.3; //Sets the width of the LINE_STRIP
cube3.scale.y = 0.3; //Ignored if marker type is LINE_STRIP
cube3.scale.z = 0.3;
cube3.points.resize(2);
//Visualize the PYLON4
marker_pub4 = node.advertise<visualization_msgs::Marker>("PYLON", 10);
cube4.header.frame_id = "map";
cube4.header.stamp = ros::Time::now();
cube4.ns = "PYLON";
cube4.type = visualization_msgs::Marker::CUBE;
cube4.action = visualization_msgs::Marker::ADD;
cube4.id = 4;
cube4.color.r = 1.0;
cube4.color.a = 1.0;
cube4.scale.x = 0.3; //Sets the width of the LINE_STRIP
cube4.scale.y = 0.3; //Ignored if marker type is LINE_STRIP
cube4.scale.z = 0.3;
cube4.points.resize(2);
*/
tf::TransformListener listener;
ros::Rate rate(10.0);
ros::Duration wait(5); wait.sleep();
int speed_count = 0; double vel,yaw, xlast = 0 ,ylast = 0;
while (node.ok())
{
//Speed calculation
tf::StampedTransform transform2;
if(speed_count==9)
{
try{
//listener.lookupTransform("/map", "/scanmatcher_frame",
listener.lookupTransform("/map", "/base_link",
ros::Time(0), transform2);
}
catch (tf::TransformException ex){};
vel = hypotenuse(xlast,ylast, transform2.getOrigin().x(),transform2.getOrigin().y());
if(vel<0.003){
yaw = tf::getYaw(transform2.getRotation());
}
speed_count = 0;
xlast = transform2.getOrigin().x(); ylast = transform2.getOrigin().y();
}
else speed_count++;
//Get the 4 corners of the wheelchair
tf::StampedTransform transform;
try{
listener.lookupTransform("/map", "/corner1",
ros::Time(0), transform);
}
catch (tf::TransformException ex){};
double x_vect_a = transform.getOrigin().x();
double y_vect_a = transform.getOrigin().y();
try{
listener.lookupTransform("/map", "/corner2",
ros::Time(0), transform);
}
catch (tf::TransformException ex){};
double x_vect_b = transform.getOrigin().x();
double y_vect_b = transform.getOrigin().y();
try{
listener.lookupTransform("/map", "/corner3",
ros::Time(0), transform);
}
catch (tf::TransformException ex){};
double x_vect_c = transform.getOrigin().x();
double y_vect_c = transform.getOrigin().y();
try{
listener.lookupTransform("/map", "/corner4",
ros::Time(0), transform);
}
catch (tf::TransformException ex){};
double x_vect_d = transform.getOrigin().x();
double y_vect_d = transform.getOrigin().y();
//Find the minimum distance between the four line segments of the rectangle(wheelchair) and bed frame (line segment)
//Line segment 1, between corner1 and corner2
//double p1x = -9.5;
//double p1y = 4.2;
//double p2x = -9.5;
//double p2y = 2.5;
//hector
double p1x = -3.3;
double p1y = -12.2;
double p2x = -1.3;
double p2y = -11.75;
double p3x = x_vect_a;
double p3y = y_vect_a;
double p4x = x_vect_b;
double p4y = y_vect_b;
double distanceSegment1;
DistanceFromSegment(p1x, p1y, p2x, p2y ,p3x, p3y, p4x, p4y, distanceSegment1);
//Line segment 2, between corner2 and corner3
double distanceSegment2;
DistanceFromSegment(p1x, p1y, p2x, p2y ,x_vect_b, y_vect_b, x_vect_c, y_vect_c, distanceSegment2);
//Line segment 3, between corner3 and corner4
double distanceSegment3;
DistanceFromSegment(p1x, p1y, p2x, p2y ,x_vect_c, y_vect_c, x_vect_d, y_vect_d, distanceSegment3);
//Line segment 4, between corner4 and corner1
double distanceSegment4;
DistanceFromSegment(p1x, p1y, p2x, p2y ,x_vect_d, y_vect_d, x_vect_a, y_vect_a, distanceSegment4);
//Find the minimum distance of all 8 line to segment distances
double distarray [4]= {distanceSegment1, distanceSegment2, distanceSegment3, distanceSegment4};
double mindistance = distarray [0];
for (int i = 1; i < 4; i++){
if (distarray[i] < mindistance){
mindistance = distarray[i];
}
}
line.points[1].x = -3.3; line.points[1].y = -12.2;
line.points[0].x = -1.3; line.points[0].y = -11.75; line.points[0].z = line.points[1].z = 0;
//marker_pub.publish(line);
//line.points[1].x = -3.5; line.points[1].y = -12.1;
//line.points[0].x = -1.6; line.points[0].y = -11.7; line.points[0].z = line.points[1].z = 0;
marker_pub.publish(line);
//line.points[1].x = -8.7; line.points[1].y = 3.2;
//line.points[0].x = -8.6; line.points[0].y = 1.4; line.points[0].z = line.points[1].z = 0;
//marker_pub.publish(line);
///PYLON
//sphere.points[3].x = 0; cube.points[3].y = 0.5;
//cube.points[2].x = 0.5; cube.points[2].y = 0.5; cube.points[0].z = cube.points[1].z = 0;
//cube1.pose.position.x = -2.0; cube1.pose.position.y= -2.1; cube1.pose.position.z = 1;
//sphere.points[0].x = -3; sphere.points[0].y = 5; sphere.points[0].z = 5;
/*
sphere.pose.position.x = 1;
sphere.pose.position.y = 1;
sphere.pose.position.z = 0;
sphere.pose.orientation.x = 0.0;
sphere.pose.orientation.y = 0.0;
sphere.pose.orientation.z = 0.0;
sphere.pose.orientation.w = 0.0;
*/
//cube1.pose.position.x = -2.0; cube1.pose.position.y= -2.1; cube1.pose.position.z = 1;
// cube2.pose.position.x = -2.4; cube2.pose.position.y = -0.1; cube2.pose.position.z = 1;
//cube3.pose.position.x = -2.8; cube3.pose.position.y = 1.9; cube3.pose.position.z = 1;
// cube4.pose.position.x = -3.2; cube4.pose.position.y = 3.9; cube4.pose.position.z = 1;
// U1R1
/*
cube1.pose.position.x = 1.5; cube1.pose.position.y = 0.9; cube1.pose.position.z = 1;
cube2.pose.position.x = 3.5; cube2.pose.position.y = 1.2; cube2.pose.position.z = 1;
cube3.pose.position.x = 5.5; cube3.pose.position.y = 1.5; cube3.pose.position.z = 1;
cube4.pose.position.x = 7.5; cube4.pose.position.y = 1.8; cube4.pose.position.z = 1;
*/
//U1R2
/*
cube1.pose.position.x = 1.1; cube1.pose.position.y = 1.1; cube1.pose.position.z = 1;
cube2.pose.position.x = 3.1; cube2.pose.position.y = 1.2; cube2.pose.position.z = 1;
cube3.pose.position.x = 5.1; cube3.pose.position.y = 1.3; cube3.pose.position.z = 1;
cube4.pose.position.x = 7.1; cube4.pose.position.y = 1.4; cube4.pose.position.z = 1;
*/
// U3R1
/*
cube1.pose.position.x = 1.9; cube1.pose.position.y = 1.1; cube1.pose.position.z = 1;
cube2.pose.position.x = 3.5; cube2.pose.position.y = 1.2; cube2.pose.position.z = 1;
cube3.pose.position.x = 5.3; cube3.pose.position.y = 1.3; cube3.pose.position.z = 1;
cube4.pose.position.x = 7.0; cube4.pose.position.y = 1.4; cube4.pose.position.z = 1;
*/
//marker_pub1.publish(cube1);
//marker_pub2.publish(cube2);
//marker_pub2.publish(cube3);
//marker_pub2.publish(cube4);
if(vel<0.0025){
if (mindistance < 2) {
ROS_INFO("Yaw: %f Distance: %f",yaw*57.29 ,mindistance);
//myfile << yaw*57.29 << " " << vel << " " << mindistance << "\n";
}
}
rate.sleep();
}
return 0;
};
/* handle a PointCloud2 message */
void pointcloud2_callback(sensor_msgs::PointCloud2 msg)
{
ROS_INFO("Got PointCloud2 msg with %u points\n", msg.width * msg.height);
if (!have_table){
ROS_WARN("No table, bro\n");
return;
}
// get the cloud in PCL format in the right coordinate frame
PointCloudXYZ cloud, cloud2, full_cloud;
msg.header.stamp = ros::Time(0);
pcl::fromROSMsg(msg, cloud);
if (!pcl_ros::transformPointCloud(target_frame, cloud, cloud2, *tf_listener)) {
ROS_WARN("Can't transform point cloud; aborting object detection.");
return;
}
else{
cloud2.header.frame_id = target_frame;
ROS_INFO("Changed the frame");
}
ROS_INFO("%s is the target_frame", target_frame.c_str());
full_cloud = cloud2;
ROS_DEBUG("full cloud is %zu points", full_cloud.points.size());
/*for (size_t i = 0; i<table_polygon.points.size(); i++){
ROS_INFO("%f %f %f",table_polygon.points[i].x, table_polygon.points[i].y, table_polygon.points[i].z);
}*/
// filter out points outside of the attention area, and only keep points above the table
ROS_INFO("Have table, filtering points on table...");
polygon_filter(table_polygon, cloud2, cloud);
if (cloud.points.size() < 100) {
ROS_WARN("Not enough points in table region. Only %zu points; aborting object detection.", cloud.points.size());
return;
}
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud.makeShared());
pass.setFilterFieldName("z");
float table_height = table_polygon.points[0].z;
//float table_height = adjust_table_height(cloud);
//this is where you set how much to chop off of the bottom of the objects.
//it's .02 right now..
pass.setFilterLimits(table_height + .02, 1000.);
pass.filter(cloud2);
ROS_INFO("Done filtering table.");
if (cloud2.points.size() < 100) {
ROS_WARN("Not enough points above table. Only %zu points; aborting object detection.", cloud2.points.size());
return;
}
ROS_INFO("Object Cloud is %zu points.", cloud2.points.size());
// find cylinders
ROS_INFO("%s is the cloud 2 frame right before CODE!!!!!!!!", cloud2.header.frame_id.c_str());
cloud2.header.frame_id = target_frame;
Cylinders C = find_cylinders(cloud2);
cylinders_pub.publish(C);
ROS_INFO("Published cylinders msg\n");
}
void publishAidHUI(const ublox_msgs::AidHUI& m)
{
static ros::Publisher publisher = nh->advertise<ublox_msgs::AidHUI>("aidhui", kROSQueueSize);
publisher.publish(m);
}
void publishNavSOL(const ublox_msgs::NavSOL& m)
{
static ros::Publisher publisher = nh->advertise<ublox_msgs::NavSOL>("navsol", kROSQueueSize);
publisher.publish(m);
}
void VrepJoy::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{
vrep_joy_pub_.publish(joy);
}
void RosAriaNode::publish()
{
// Note, this is called via SensorInterpTask callback (myPublishCB, named "ROSPublishingTask"). ArRobot object 'robot' sholud not be locked or unlocked.
pos = robot->getPose();
tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180), tf::Vector3(pos.getX()/1000,
pos.getY()/1000, 0)), position.pose.pose); //Aria returns pose in mm.
position.twist.twist.linear.x = robot->getVel()/1000.0; //Aria returns velocity in mm/s.
position.twist.twist.linear.y = robot->getLatVel()/1000.0;
position.twist.twist.angular.z = robot->getRotVel()*M_PI/180;
position.header.frame_id = frame_id_odom;
position.child_frame_id = frame_id_base_link;
position.header.stamp = ros::Time::now();
pose_pub.publish(position);
ROS_DEBUG("RosAria: publish: (time %f) pose x: %f, y: %f, angle: %f; linear vel x: %f, y: %f; angular vel z: %f",
position.header.stamp.toSec(),
(double)position.pose.pose.position.x,
(double)position.pose.pose.position.y,
(double)position.pose.pose.orientation.w,
(double) position.twist.twist.linear.x,
(double) position.twist.twist.linear.y,
(double) position.twist.twist.angular.z
);
// publishing transform odom->base_link
odom_trans.header.stamp = ros::Time::now();
odom_trans.header.frame_id = frame_id_odom;
odom_trans.child_frame_id = frame_id_base_link;
odom_trans.transform.translation.x = pos.getX()/1000;
odom_trans.transform.translation.y = pos.getY()/1000;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pos.getTh()*M_PI/180);
odom_broadcaster.sendTransform(odom_trans);
// getStallValue returns 2 bytes with stall bit and bumper bits, packed as (00 00 FrontBumpers RearBumpers)
int stall = robot->getStallValue();
unsigned char front_bumpers = (unsigned char)(stall >> 8);
unsigned char rear_bumpers = (unsigned char)(stall);
bumpers.header.frame_id = frame_id_bumper;
bumpers.header.stamp = ros::Time::now();
std::stringstream bumper_info(std::stringstream::out);
// Bit 0 is for stall, next bits are for bumpers (leftmost is LSB)
for (unsigned int i=0; i<robot->getNumFrontBumpers(); i++)
{
bumpers.front_bumpers[i] = (front_bumpers & (1 << (i+1))) == 0 ? 0 : 1;
bumper_info << " " << (front_bumpers & (1 << (i+1)));
}
ROS_DEBUG("RosAria: Front bumpers:%s", bumper_info.str().c_str());
bumper_info.str("");
// Rear bumpers have reverse order (rightmost is LSB)
unsigned int numRearBumpers = robot->getNumRearBumpers();
for (unsigned int i=0; i<numRearBumpers; i++)
{
bumpers.rear_bumpers[i] = (rear_bumpers & (1 << (numRearBumpers-i))) == 0 ? 0 : 1;
bumper_info << " " << (rear_bumpers & (1 << (numRearBumpers-i)));
}
ROS_DEBUG("RosAria: Rear bumpers:%s", bumper_info.str().c_str());
bumpers_pub.publish(bumpers);
//Publish battery information
// TODO: Decide if BatteryVoltageNow (normalized to (0,12)V) is a better option
std_msgs::Float64 batteryVoltage;
batteryVoltage.data = robot->getRealBatteryVoltageNow();
voltage_pub.publish(batteryVoltage);
if(robot->haveStateOfCharge())
{
std_msgs::Float32 soc;
soc.data = robot->getStateOfCharge()/100.0;
state_of_charge_pub.publish(soc);
}
// publish recharge state if changed
char s = robot->getChargeState();
if(s != recharge_state.data)
{
ROS_INFO("RosAria: publishing new recharge state %d.", s);
recharge_state.data = s;
recharge_state_pub.publish(recharge_state);
}
// publish motors state if changed
bool e = robot->areMotorsEnabled();
if(e != motors_state.data || !published_motors_state)
{
ROS_INFO("RosAria: publishing new motors state %d.", e);
motors_state.data = e;
motors_state_pub.publish(motors_state);
published_motors_state = true;
}
// Publish sonar information, if enabled.
if (publish_sonar || publish_sonar_pointcloud2)
{
sensor_msgs::PointCloud cloud; //sonar readings.
cloud.header.stamp = position.header.stamp; //copy time.
// sonar sensors relative to base_link
cloud.header.frame_id = frame_id_sonar;
std::stringstream sonar_debug_info; // Log debugging info
sonar_debug_info << "Sonar readings: ";
for (int i = 0; i < robot->getNumSonar(); i++) {
ArSensorReading* reading = NULL;
reading = robot->getSonarReading(i);
if(!reading) {
ROS_WARN("RosAria: Did not receive a sonar reading.");
continue;
}
// getRange() will return an integer between 0 and 5000 (5m)
sonar_debug_info << reading->getRange() << " ";
// local (x,y). Appears to be from the centre of the robot, since values may
// exceed 5000. This is good, since it means we only need 1 transform.
// x & y seem to be swapped though, i.e. if the robot is driving north
// x is north/south and y is east/west.
//
//ArPose sensor = reading->getSensorPosition(); //position of sensor.
// sonar_debug_info << "(" << reading->getLocalX()
// << ", " << reading->getLocalY()
// << ") from (" << sensor.getX() << ", "
// << sensor.getY() << ") ;; " ;
//add sonar readings (robot-local coordinate frame) to cloud
geometry_msgs::Point32 p;
p.x = reading->getLocalX() / 1000.0;
p.y = reading->getLocalY() / 1000.0;
p.z = 0.0;
cloud.points.push_back(p);
}
ROS_DEBUG_STREAM(sonar_debug_info.str());
// publish topic(s)
if(publish_sonar_pointcloud2)
{
sensor_msgs::PointCloud2 cloud2;
if(!sensor_msgs::convertPointCloudToPointCloud2(cloud, cloud2))
{
ROS_WARN("Error converting sonar point cloud message to point_cloud2 type before publishing! Not publishing this time.");
}
else
{
sonar_pointcloud2_pub.publish(cloud2);
}
}
if(publish_sonar)
{
sonar_pub.publish(cloud);
}
} // end if sonar_enabled
}
void* ControlThread(void *p)
{
COdometerCapture *pComm = (COdometerCapture*)p;
/// Handling events
const int EVENT_NUM = 4;
HANDLE hWait[EVENT_NUM];
hWait[0] = pComm->m_hEventOdoCalTimer;
hWait[1] = pComm->m_hEventOdoPubTimer;
hWait[2] = pComm->m_hEventSpeedTimer;
hWait[3] = pComm->m_hEventSpeedCheckTimer;
/// Real Send Speed
// double real_vx = 0.0;
// double real_vy = 0.0;
// double real_w = 0.0;
// allowance change value for a single cycle
// double allow_vx_change_per_cycle = 2.5; // cm/s
// double allow_vx_decrease_per_cycle = 5.75; //cm/s
// double allow_vy_change_per_cycle = 2.5; // cm/s
// double allow_w_increase_per_cycle = 4.5; // deg/s
// double allow_w_decrease_per_cycle = 4.5; // deg/s
//
// extern double g_closest_obstacle_x; //urg看到的最近障碍物点 x,y坐标
// extern double g_closest_obstacle_y; //urg看到的最近障碍物点
// extern double g_max_acc_inc_x; //最大加速度
// extern double g_max_acc_dec_x; //最减加速度
// extern double g_max_acc_w;
// extern double g_max_vel_x; //最大速度
// extern double g_max_vel_w; //最大角速度
extern boost::mutex g_mutex_obstacle; //
// double cur_ac_vx,cur_ac_vy,cur_ac_vw;
// double old_x = 0,old_y = 0,old_w = 0;
boost::posix_time::ptime prev_odom_time;
boost::mutex vel_mutex;
/// Handling loop
fd_set fdset;
//uint64_t exp;
struct timeval timeout;
timeout.tv_sec=5;
timeout.tv_usec=0;
while (pComm->m_initial)
{
FD_ZERO(&fdset);
int maxfp=0;
for(int k=0;k<EVENT_NUM;k++)
{
FD_SET(hWait[k],&fdset);
if(maxfp<hWait[k])
{
maxfp=hWait[k];
}
}
int dRes = select(maxfp+1,&fdset,NULL,NULL,&timeout);
switch (dRes)
{
//*********************************//
// 修改case后面的值 //
//********************************//
case -1 :
cout<<"error of select"<<endl;
case 0 :
cout<<"No Data within five seconds"<<endl;
default :
if(FD_ISSET(hWait[0],&fdset)) // 里程计合成
{
pComm->m_mutex_cap.Lock();
ODOCAL->onTimerCal2(); /// Timer on calculation - using position
pComm->m_mutex_cap.Unlock();
}
if(FD_ISSET(hWait[1],&fdset)) // 里程计发布
{
pComm->m_mutex_cap.Lock();
nav_msgs::Odometry odometerData;
pComm->GetCurOdometry(odometerData);
odometerData.header.stamp = ros::Time::now();
odometerData.header.frame_id = "odom";
odometerData.child_frame_id = "base_link";
static tf::TransformBroadcaster odom_broadcaster;
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(odometerData.pose.pose.orientation.z);
///first, we'll publish odom the transform over tf
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = ros::Time::now();
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
odom_trans.transform.translation.x = odometerData.pose.pose.position.x;
odom_trans.transform.translation.y = odometerData.pose.pose.position.y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
///send the transform
odom_broadcaster.sendTransform(odom_trans);
///publish laser transform over tf
////sensor_msgs::LaserScan LaserData;
// pComm->GetCurLaser(LaserData);
static tf::TransformBroadcaster laser_broadcaster;
tf::Transform laser_transform;
laser_transform.setOrigin( tf::Vector3(0.3, 0, 0.05) );
tf::Quaternion q;
q.setRPY(0, 0, 0);
laser_transform.setRotation(q);
laser_broadcaster.sendTransform(tf::StampedTransform(laser_transform, ros::Time::now(), "base_link", "laser"));
// geometry_msgs::Quaternion laser_quat = tf::createQuaternionMsgFromYaw(0);
// geometry_msgs::TransformStamped laser_trans;
// LaserData.header.stamp = ros::Time::now();
// laser_trans.transform.translation.x = 0.3;
// laser_trans.transform.translation.y = 0;
// laser_trans.transform.translation.z = 0.05;
// laser_trans.transform.rotation = laser_quat;
//s_laser.publish(LaserData);
m_odom.publish(odometerData);
pComm->m_mutex_cap.Unlock();
}
if(FD_ISSET(hWait[2],&fdset)) // 速度下发
{
geometry_msgs::Twist sendSpeed;
pComm->GetCurSpeed(sendSpeed);
double send_vx = sendSpeed.linear.x;
double send_vy = sendSpeed.linear.y;
double send_w = sendSpeed.angular.z;
//限速
send_vx = Utils::clip(send_vx,-1000.0,1000.0);
send_vy = Utils::clip(send_vy,-1000.0,1000.0);
send_w = Utils::clip(send_w,-50.0,50.0);
MECANUM->SendVelocities(send_vx/100.0, send_vy/100.0, send_w*3.1415926/180.0);
pComm->DoSafeSpeed();
}
if(FD_ISSET(hWait[3],&fdset)) // 速度下发检查
{
pComm->SpeedCheck();
}
break;
}
}
return 0;
}
void callback(const sensor_msgs::PointCloud2ConstPtr& pCloud)
{
pcl::StopWatch watch;
min_pt[0]=min_x - gridsize;
min_pt[1]=min_y - gridsize;
min_pt[2]=min_z - gridsize;
max_pt[0]=max_x + gridsize;
max_pt[1]=max_y + gridsize;
max_pt[2]=max_z + gridsize;
ball_grid_index[0] = 0;
ball_grid_index[1] = 0;
ball_grid_index[2] = 0;
// new cloud formation
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg (*pCloud, *cloud);
im_num++;
if (first_run < 5)
{
first_run++;
/////////////////////////////////////////
// Remove points above given distance and below ground
/////////////////////////////////////////
for (unsigned int i=0;i<cloud->height;i++)
{
for (unsigned int j=0;j<cloud->width;j++)
{
unsigned int index_image = i * cloud->width + j;
if (cloud->points[index_image].x > DEPTH_LIMIT)
{
cloud->points[index_image].x = sqrt(-1.0); // NaN
cloud->points[index_image].y = sqrt(-1.0);
cloud->points[index_image].z = sqrt(-1.0);
}
}
}
///////////////////////////////////////////////////////////////////////
// Grid definition (counting number of points in each voxel of the grid)
if (my_grid)
my_grid->~CGrid_3D(); // destructor called here
my_grid = new CGrid_3D(gridsize,min_pt,max_pt);
}
else
{
my_grid->reset();
}
my_grid->compute_grid(cloud);
//////////////////////////////////////////////////////////////////////////////////////////////
// Ball detection UFO
ball_centre_coord3D[0]=0;
ball_centre_coord3D[1]=0;
ball_centre_coord3D[2]=0;
my_grid->detect_ball(min_points, mask, MASKSIZE, ball_centre_coord3D,
ball_grid_index, 0, my_grid->xdim-MASKSIZE, 0,
my_grid->ydim-MASKSIZE, 2, my_grid->zdim-MASKSIZE, previous_ball);
// Ball detection UFO
//////////////////////////////////////////////////////////////////////////////////////////////
dt=dt+watch.getTimeSeconds(); // running time instant
geometry_msgs::PointStamped point_out;
if (ball_centre_coord3D[2] !=0)
{
// coordinates published at time instant
// KinectEstimated (KE) header stamp and frame id
point_out.header.stamp = ros::Time::now();
point_out.header.frame_id = "/ballCord";
point_out.point.x = ball_centre_coord3D[0];
point_out.point.y = ball_centre_coord3D[1];
point_out.point.z = ball_centre_coord3D[2];
ballCord.publish(point_out);
printf("\n%3d - Flying ball - %6f %6f %6f",im_num,ball_centre_coord3D[0],ball_centre_coord3D[1],ball_centre_coord3D[2]);
}
else
{
// coordinates published at time instant
// KinectEstimated (KE) header stamp and frame id
point_out.header.stamp = ros::Time::now();
point_out.header.frame_id = "/ballCord";
point_out.point.x = 0.0;
point_out.point.y = 0.0;
point_out.point.z = 0.0;
ballCord.publish(point_out);
}
// frequency print
pcl::console::print_highlight ("\n frequency: %f\n", 1/(watch.getTimeSeconds()));
}
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
// std::cout << "\n\n----------------Received point cloud!-----------------\n";
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsampled (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_planar (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_objects (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_red (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_green (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_blue (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_yellow (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_concat_clusters (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_concat_hulls (new pcl::PointCloud<pcl::PointXYZRGB>);
sensor_msgs::PointCloud2 downsampled2, planar2, objects2, filtered2, red2, green2, blue2, yellow2, concat_clusters2, concat_hulls2;
std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> color_clouds, cluster_clouds, hull_clouds;
std::vector<pcl::PointXYZRGB> labels;
// fromROSMsg(*input, *cloud);
// pub_input.publish(*input);
// Downsample the input PointCloud
pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
sor.setInputCloud (input);
// sor.setLeafSize (0.01, 0.01, 0.01); //play around with leafsize (more samples, better resolution?)
sor.setLeafSize (0.001, 0.001, 0.001);
sor.filter (downsampled2);
fromROSMsg(downsampled2,*downsampled);
pub_downsampled.publish (downsampled2);
// Segment the largest planar component from the downsampled cloud
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
pcl::ModelCoefficients::Ptr coeffs (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.0085);
seg.setInputCloud (downsampled);
seg.segment (*inliers, *coeffs);
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud (downsampled);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_planar);
// toROSMsg(*cloud_planar,planar2);
// pub_planar.publish (planar2);
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_objects);
// toROSMsg(*cloud_objects,objects2);
// pub_objects.publish (objects2);
// PassThrough Filter
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setInputCloud (cloud_objects);
pass.setFilterFieldName ("z"); //all duplos in pcd1
pass.setFilterLimits (0.8, 1.0);
pass.filter (*cloud_filtered);
toROSMsg(*cloud_filtered,filtered2);
pub_filtered.publish (filtered2);
//don't passthrough filter, does color filter work too? (cloud_red has many points in top right off the table)
// Segment filtered PointCloud by color (red, green, blue, yellow)
for (size_t i = 0 ; i < cloud_filtered->points.size () ; i++)
{
if ( (int(cloud_filtered->points[i].r) > 2*int(cloud_filtered->points[i].g)) && (cloud_filtered->points[i].r > cloud_filtered->points[i].b) )
cloud_red->points.push_back(cloud_filtered->points[i]);
if ( (cloud_filtered->points[i].g > cloud_filtered->points[i].r) && (cloud_filtered->points[i].g > cloud_filtered->points[i].b) )
cloud_green->points.push_back(cloud_filtered->points[i]);
if ( (cloud_filtered->points[i].b > cloud_filtered->points[i].r) && (cloud_filtered->points[i].b > cloud_filtered->points[i].g) )
cloud_blue->points.push_back(cloud_filtered->points[i]);
if ( (cloud_filtered->points[i].r > cloud_filtered->points[i].g) && (int(cloud_filtered->points[i].g) - int(cloud_filtered->points[i].b) > 30) )
cloud_yellow->points.push_back(cloud_filtered->points[i]);
}
cloud_red->header.frame_id = "base_link";
cloud_red->width = cloud_red->points.size ();
cloud_red->height = 1;
color_clouds.push_back(cloud_red);
toROSMsg(*cloud_red,red2);
pub_red.publish (red2);
cloud_green->header.frame_id = "base_link";
cloud_green->width = cloud_green->points.size ();
cloud_green->height = 1;
color_clouds.push_back(cloud_green);
toROSMsg(*cloud_green,green2);
pub_green.publish (green2);
cloud_blue->header.frame_id = "base_link";
cloud_blue->width = cloud_blue->points.size ();
cloud_blue->height = 1;
color_clouds.push_back(cloud_blue);
toROSMsg(*cloud_blue,blue2);
pub_blue.publish (blue2);
cloud_yellow->header.frame_id = "base_link";
cloud_yellow->width = cloud_yellow->points.size ();
cloud_yellow->height = 1;
color_clouds.push_back(cloud_yellow);
toROSMsg(*cloud_yellow,yellow2);
pub_yellow.publish (yellow2);
// Extract Euclidian clusters from color-segmented PointClouds
int j(0), num_red (0), num_green(0), num_blue(0), num_yellow(0);
for (size_t cit = 0 ; cit < color_clouds.size() ; cit++)
{
pcl::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
tree->setInputCloud (color_clouds[cit]);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
ec.setClusterTolerance (0.0075); // 0.01
// ec.setMinClusterSize (12);
// ec.setMaxClusterSize (75);
ec.setMinClusterSize (100);
ec.setMaxClusterSize (4000);
ec.setSearchMethod (tree);
ec.setInputCloud (color_clouds[cit]);
ec.extract (cluster_indices);
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
for (std::vector<int>::const_iterator pit = it->indices.begin () ; pit != it->indices.end () ; pit++)
cloud_cluster->points.push_back (color_clouds[cit]->points[*pit]);
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
cloud_cluster->header.frame_id = "base_link";
cluster_clouds.push_back(cloud_cluster);
labels.push_back(cloud_cluster->points[0]);
if (cit == 0) num_red++;
if (cit == 1) num_green++;
if (cit == 2) num_blue++;
if (cit == 3) num_yellow++;
// Create ConvexHull for cluster (keep points on perimeter of cluster)
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::ConvexHull<pcl::PointXYZRGB> chull;
chull.setInputCloud (cloud_cluster);
chull.reconstruct (*cloud_hull);
cloud_hull->width = cloud_hull->points.size ();
cloud_hull->height = 1;
cloud_hull->header.frame_id = "base_link";
hull_clouds.push_back(cloud_hull);
j++;
}
}
std::cout << "Number of RED clusters: " << num_red << std::endl;
std::cout << "Number of GREEN clusters: " << num_green << std::endl;
std::cout << "Number of BLUE clusters: " << num_blue << std::endl;
std::cout << "Number of YELLOW clusters: " << num_yellow << std::endl;
std::cout << "TOTAL number of clusters: " << j << std::endl;
// Concatenate PointCloud clusters and convex hulls
for (size_t k = 0 ; k < cluster_clouds.size() ; k++)
{
for (size_t l = 0 ; l < cluster_clouds[k]->size() ; l++)
{
cloud_concat_clusters->points.push_back(cluster_clouds[k]->points[l]);
cloud_concat_hulls->points.push_back(hull_clouds[k]->points[l]);
}
}
cloud_concat_clusters->header.frame_id = "base_link";
cloud_concat_clusters->width = cloud_concat_clusters->points.size ();
cloud_concat_clusters->height = 1;
toROSMsg(*cloud_concat_clusters,concat_clusters2);
pub_concat_clusters.publish (concat_clusters2);
cloud_concat_hulls->header.frame_id = "base_link";
cloud_concat_hulls->width = cloud_concat_hulls->points.size ();
cloud_concat_hulls->height = 1;
toROSMsg(*cloud_concat_hulls,concat_hulls2);
pub_concat_hulls.publish (concat_hulls2);
// Estimate the volume of each cluster
double height, width;
std::vector <double> heights, widths;
std::vector <int> height_ids, width_ids;
for (size_t k = 0 ; k < cluster_clouds.size() ; k++)
{
// Calculate cluster height
double tallest(0), shortest(1000), widest(0) ;
for (size_t l = 0 ; l < cluster_clouds[k]->size() ; l++)
{
double point_to_plane_dist;
point_to_plane_dist = coeffs->values[0] * cluster_clouds[k]->points[l].x +
coeffs->values[1] * cluster_clouds[k]->points[l].y +
coeffs->values[2] * cluster_clouds[k]->points[l].z + coeffs->values[3] /
sqrt( pow(coeffs->values[0], 2) + pow(coeffs->values[1], 2)+ pow(coeffs->values[2], 2) );
if (point_to_plane_dist < 0) point_to_plane_dist = 0;
if (point_to_plane_dist > tallest) tallest = point_to_plane_dist;
if (point_to_plane_dist < shortest) shortest = point_to_plane_dist;
}
// Calculate cluster width
for (size_t m = 0 ; m < hull_clouds[k]->size() ; m++)
{
double parallel_vector_dist;
for (size_t n = m ; n < hull_clouds[k]->size()-1 ; n++)
{
parallel_vector_dist = sqrt( pow(hull_clouds[k]->points[m].x - hull_clouds[k]->points[n+1].x,2) +
pow(hull_clouds[k]->points[m].y - hull_clouds[k]->points[n+1].y,2) +
pow(hull_clouds[k]->points[m].z - hull_clouds[k]->points[n+1].z,2) );
if (parallel_vector_dist > widest) widest = parallel_vector_dist;
}
}
// Classify block heights (error +/- .005m)
height = (shortest < .01) ? tallest : tallest - shortest; //check for stacked blocks
heights.push_back(height);
if (height > .020 && height < .032) height_ids.push_back(0); //0: standing flat
else if (height > .036 && height < .043) height_ids.push_back(1); //1: standing side
else if (height > .064) height_ids.push_back(2); //2: standing long
else height_ids.push_back(-1); //height not classified
// Classify block widths (error +/- .005m)
width = widest;
widths.push_back(widest);
if (width > .032-.005 && width < .0515+.005) width_ids.push_back(1); //1: short
else if (width > .065-.005 && width < .0763+.005) width_ids.push_back(2); //2: medium
else if (width > .1275-.005 && width < .1554+.005) width_ids.push_back(4); //4: long
else width_ids.push_back(-1); //width not classified
}
// Classify block size using width information
std::vector<int> block_ids, idx_1x1, idx_1x2, idx_1x4, idx_unclassified;
int num_1x1(0), num_1x2(0), num_1x4(0), num_unclassified(0);
for (size_t p = 0 ; p < width_ids.size() ; p++)
{
if (width_ids[p] == 1)
{
block_ids.push_back(1); //block is 1x1
idx_1x1.push_back(p);
num_1x1++;
}
else if (width_ids[p] == 2)
{
block_ids.push_back(2); //block is 1x2
idx_1x2.push_back(p);
num_1x2++;
}
else if (width_ids[p] == 4)
{
block_ids.push_back(4); //block is 1x4
idx_1x4.push_back(p);
num_1x4++;
}
else
{
block_ids.push_back(-1); //block not classified
idx_unclassified.push_back(p);
num_unclassified++;
}
}
// Determine Duplos of the same size
std::cout << "\nThere are " << num_1x1 << " blocks of size 1x1 ";
if (num_1x1>0) std::cout << "(cluster index: ";
for (size_t q = 0 ; q < idx_1x1.size() ; q++)
std::cout << idx_1x1[q] << ", ";
if (num_1x1>0) std::cout << ")";
std::cout << "\nThere are " << num_1x2 << " blocks of size 1x2 ";
if (num_1x2>0) std::cout << "(cluster index: ";
for (size_t q = 0 ; q < idx_1x2.size() ; q++)
std::cout << idx_1x2[q] << ", ";
if (num_1x2>0) std::cout << ")";
std::cout << "\nThere are " << num_1x4 << " blocks of size 1x4 ";
if (num_1x4>0) std::cout << "(cluster index: ";
for (size_t q = 0 ; q < idx_1x4.size() ; q++)
std::cout << idx_1x4[q] << ", ";
if (num_1x4>0) std::cout << ")";
std::cout << "\nThere are " << num_unclassified << " unclassified blocks ";
if (num_unclassified>0) std::cout << "(cluster index: ";
for (size_t q = 0 ; q < idx_unclassified.size() ; q++)
std::cout << idx_unclassified[q] << ", ";
if (num_unclassified>0) std::cout << ")";
std::cout << "\n\n\n";
return;
}
void SvmDetect::CloudClustersCallback(const lidar_tracker::CloudClusterArray::Ptr& in_cloud_cluster_array_ptr)
{
cloud_clusters_pub_.publish(*in_cloud_cluster_array_ptr);
}
bool robot(unit::for_double::Request &req, unit::for_double::Response &res)
{
ros::Time begin = ros::Time::now();
pos_robotx= req.positionx;
pos_roboty= req.positiony;
ang_robot = req.angle ;
check = true ;
msg.data = conv;
chatter_pub.publish(msg);
setCurrentOdeContext(0);
restoreOdeState(0);
const dReal *pos_first = odeBodyGetPosition(body1);
odeBodySetPosition(body1,pos_robotx,pos_roboty,pos_first[2]);
const dReal *pos_second = odeBodyGetPosition(body1);
printf(" Pos X %f Pos Y %f\n", pos_second[0], pos_second[1]);
saveOdeState(0);
//init all trajectories to the master state
//init all trajectories to the master state
for (int i=0; i<nSamples; i++)
{
//activate the context for this sample
setCurrentOdeContext(i+1);
//load the state from the master context
restoreOdeState(0);
const dReal *pos = odeBodyGetPosition(body1);
odeBodySetPosition(body1,pos_robotx,pos_roboty,pos[2]);
const dReal *pos1 = odeBodyGetPosition(body1);
saveOdeState(i+1,0);
}
for (int i=0; i<nSamples; i++)
{
setCurrentOdeContext(i);
const dReal *pos = odeBodyGetPosition(body1);
// printf(" position x :%f y:%f z:%f NUM: %d \n",pos[0],pos[1],pos[2],i);
}
//signal the start of new C-PBP iteration
setCurrentOdeContext(0);
restoreOdeState(0);
const dReal *pos2 = odeBodyGetPosition(body1);
odeBodySetPosition(body1,pos_robotx,pos_roboty,pos2[2]);
const dReal *pos = odeBodyGetPosition(body1);
float angle=odeJointGetHingeAngle(joint2);
//printf(" posX x :%f, aVel: %f \n",pos[0],aVel);
float stateVector[2]={pos[0],pos[1]};
pbp.startIteration(true,stateVector);
//simulate forward
for (int k=0; k<nTimeSteps; k++)
{
//signal the start of a planning step
pbp.startPlanningStep(k);
//NOTE: for multithreaded operation, one would typically run each iteration of the following loop in a separate thread.
//The getControl(), getPreviousSampleIdx(), and updateResults() methods of the optimizer are thread-safe.
//The physics simulation is also thread-safe as we have full copies of the simulated system for each thread/trajectory
for (int i=0; i<nSamples; i++)
{
//get control from C-PBP
float control, control1;
pbp.getControl(i,&control);
pbp.getControl(i,&control1);
//printf("k=%i",k);
//printf("i=%d \n",i);
//printf("control: %f control 1 %f\n",control,control1);
//get the mapping from this to previous state (affected by resampling operations)
int previousStateIdx=pbp.getPreviousSampleIdx(i);
//simulate to get next state.
setCurrentOdeContext(i+1);
//printf("previous state id: %d \n",previousStateIdx);
restoreOdeState(previousStateIdx+1); //continue the a trajectory (selected in the resampling operation inside ControlPBP)
pos = odeBodyGetPosition(body1);
angle=odeJointGetHingeAngle(joint2);
// printf("before Posx %1.3f,posz = %f avel %1.3f, \n",pos[0],pos[2],(aVel*180/PI));
// printf("control %f \n",control);
//dVector3 torque={0,control,0};
//odeBodyAddTorque(body,torque);
dReal MaxForce = dInfinity;
odeJointSetHingeParam(joint2,dParamFMax,dInfinity);
odeJointSetHingeParam(joint2,dParamVel,control );
odeJointSetHingeParam(joint1,dParamFMax,dInfinity);
odeJointSetHingeParam(joint1,dParamVel,control1 );
stepOde(1);
pos = odeBodyGetPosition(body1);
angle=odeJointGetHingeAngle(joint2);
// printf("AFTER Posx %1.3f,posz = %f avel %1.3f,\n",pos[0],pos[2],aVel);
const dReal *pos = odeBodyGetPosition(body1);
float angle=odeJointGetHingeAngle(joint2);
float angle1=odeJointGetHingeAngle(joint1);
float cost=squared((0.03-pos[0])*200.0f)+squared((0.01-pos[1])*200.0f) ;//+ squared(angle*10.1f)+squared(angle1*10.1f) ;//+ squared(control* 1.5f)+ squared(control1*1.5f);
/*
if ( pos[0]<0.06 && pos[0]>0.03 )
{
if( pos[1]>-0.03 && pos[0]<0.03 )
{
cost = cost + 10000;
}
}
*/
//store the state and cost to C-PBP. Note that in general, the stored state does not need to contain full simulation state as in this simple case.
//instead, one may use arbitrary state features
float stateVector[2]={pos[0],pos[1]};
float cont[2]= {control,control1};
//float stateVector[2]={angle,aVel};
pbp.updateResults(i,cont,stateVector,cost);
//printf("cost in the loop end: %f posz: %f \n",cost,pos[2]);
}
//save all states, will be used at next step (the restoreOdeState() call above)
for (int i=0; i<nSamples; i++)
{
saveOdeState(i+1,i+1);
}
//signal the end of the planning step. this normalizes the state costs etc. for the next step
pbp.endPlanningStep(k);
}
//signal the end of an iteration. this also executes the backwards smoothing pass
pbp.endIteration();
//deploy the best control found using the master context
float control[2];
pbp.getBestControl(0,control);
float cost=(float)pbp.getSquaredCost();
//printf("Cost %f \n",cost);
setCurrentOdeContext(0);
restoreOdeState(0);
//for(int j=0; j<20; j++)
//{
if (cnt >3)
{
dReal MaxForce = dInfinity;
odeJointSetHingeParam(joint2,dParamFMax,dInfinity);
odeJointSetHingeParam(joint2,dParamVel,control[0]);
odeJointSetHingeParam(joint1,dParamFMax,dInfinity);
odeJointSetHingeParam(joint1,dParamVel,control[1]);
}
else
{
odeJointSetHingeParam(joint2,dParamFMax,dInfinity);
odeJointSetHingeParam(joint2,dParamVel,0.0);
odeJointSetHingeParam(joint1,dParamFMax,dInfinity);
odeJointSetHingeParam(joint1,dParamVel,0.0);
}
stepOde(0);
saveOdeState(0);
//print output, both angle and aVel should converge to 0, with some sampling noise remaining
pos = odeBodyGetPosition(body1);
//angle=odeJointGetHingeAngle(hinge);
angle=odeJointGetHingeAngle(joint2);
res.commandx = control[0];
res.commandy = control[1];
conv = angle ;
last_angle = angle;
cnt = cnt +1 ;
last_position = pos[0] ;
printf("cnt %d\n",cnt );
ros::Time end = ros::Time::now();
double dt = (begin - end).toSec();
//ROS_INFO("dt %f", dt);
printf("FINAL Posx %1.3f,posy = %f angle %1.3f, control %f, control 1 %f \n",pos[0],pos[1],angle*180/3.1416,control[0],control[1]);
// printf("angle %1.3f, avel %1.3f, cost=%1.3f\n",angle,aVel,cost);
/* int i = 0;
loop : cin >> i;
if ( i != 1)
goto loop;
*/
}
void publishAidALM(const ublox_msgs::AidALM& m)
{
static ros::Publisher publisher = nh->advertise<ublox_msgs::AidALM>("aidalm", kROSQueueSize);
publisher.publish(m);
}
void Mapper::processCloud(unique_ptr<DP> newPointCloud, const std::string& scannerFrame, const ros::Time& stamp, uint32_t seq)
{
processingNewCloud = true;
BoolSetter stopProcessingSetter(processingNewCloud, false);
// if the future has completed, use the new map
processNewMapIfAvailable();
// IMPORTANT: We need to receive the point clouds in local coordinates (scanner or robot)
timer t;
// Convert point cloud
const size_t goodCount(newPointCloud->features.cols());
if (goodCount == 0)
{
ROS_ERROR("I found no good points in the cloud [at mapper.cpp]");
return;
}
// Dimension of the point cloud, important since we handle 2D and 3D
const int dimp1(newPointCloud->features.rows());
ROS_INFO("Processing new point cloud");
{
timer t; // Print how long take the algo
// Apply filters to incoming cloud, in scanner coordinates
inputFilters.apply(*newPointCloud);
ROS_INFO_STREAM("Input filters took " << t.elapsed() << " [s]");
}
string reason;
// Initialize the transformation to identity if empty
if(!icp.hasMap())
{
// we need to know the dimensionality of the point cloud to initialize properly
publishLock.lock();
TOdomToMap = PM::TransformationParameters::Identity(dimp1, dimp1);
publishLock.unlock();
}
// Fetch transformation from scanner to odom
// Note: we don't need to wait for transform. It is already called in transformListenerToEigenMatrix()
PM::TransformationParameters TOdomToScanner;
try
{
TOdomToScanner = PointMatcher_ros::eigenMatrixToDim<float>(
PointMatcher_ros::transformListenerToEigenMatrix<float>(
tfListener,
scannerFrame,
odomFrame,
stamp
), dimp1);
}
catch(tf::ExtrapolationException e)
{
ROS_ERROR_STREAM("Extrapolation Exception. stamp = "<< stamp << " now = " << ros::Time::now() << " delta = " << ros::Time::now() - stamp);
return;
}
//Correct TOdomToMap to fit dimp1 (dimension of input data)
//We need to do this because we could have called 'CorrectPose' which always returns a [4][4] matrix
TOdomToMap = PointMatcher_ros::eigenMatrixToDim<float>(TOdomToMap, dimp1);
ROS_DEBUG_STREAM("TOdomToScanner(" << odomFrame<< " to " << scannerFrame << "):\n" << TOdomToScanner);
ROS_DEBUG_STREAM("TOdomToMap(" << odomFrame<< " to " << mapFrame << "):\n" << TOdomToMap);
const PM::TransformationParameters TscannerToMap = transformation->correctParameters(TOdomToMap * TOdomToScanner.inverse());
ROS_DEBUG_STREAM("TscannerToMap (" << scannerFrame << " to " << mapFrame << "):\n" << TscannerToMap);
// Ensure a minimum amount of point after filtering
const int ptsCount = newPointCloud->features.cols();
if(ptsCount < minReadingPointCount)
{
ROS_ERROR_STREAM("Not enough points in newPointCloud: only " << ptsCount << " pts.");
return;
}
// Initialize the map if empty
if(!icp.hasMap())
{
ROS_INFO_STREAM("Creating an initial map");
mapCreationTime = stamp;
setMap(updateMap(newPointCloud.release(), TscannerToMap, false));
// we must not delete newPointCloud because we just stored it in the mapPointCloud
return;
}
// Check dimension
if (newPointCloud->features.rows() != icp.getInternalMap().features.rows())
{
ROS_ERROR_STREAM("Dimensionality missmatch: incoming cloud is " << newPointCloud->features.rows()-1 << " while map is " << icp.getInternalMap().features.rows()-1);
return;
}
// Call this function in order to verbose errors when matching the newPointCloud and the icp Internal Map
debugFeaturesDescriptors(*newPointCloud);
try
{
// Apply ICP
PM::TransformationParameters Ticp;
Ticp = icp(*newPointCloud, TscannerToMap);
ROS_DEBUG_STREAM("Ticp:\n" << Ticp);
// Ensure minimum overlap between scans
const double estimatedOverlap = icp.errorMinimizer->getOverlap();
ROS_INFO_STREAM("Overlap: " << estimatedOverlap);
if (estimatedOverlap < minOverlap)
{
ROS_ERROR_STREAM("Estimated overlap too small, ignoring ICP correction!");
return;
}
// Compute tf
publishStamp = stamp;
publishLock.lock();
TOdomToMap = Ticp * TOdomToScanner;
// Publish tf
tfBroadcaster.sendTransform(PointMatcher_ros::eigenMatrixToStampedTransform<float>(TOdomToMap, mapFrame, odomFrame, stamp));
publishLock.unlock();
processingNewCloud = false;
ROS_DEBUG_STREAM("TOdomToMap:\n" << TOdomToMap);
// Publish odometry
if (odomPub.getNumSubscribers())
odomPub.publish(PointMatcher_ros::eigenMatrixToOdomMsg<float>(Ticp, mapFrame, stamp));
// Publish error on odometry
if (odomErrorPub.getNumSubscribers())
odomErrorPub.publish(PointMatcher_ros::eigenMatrixToOdomMsg<float>(TOdomToMap, mapFrame, stamp));
// Publish outliers
if (outlierPub.getNumSubscribers())
{
//DP outliers = PM::extractOutliers(transformation->compute(*newPointCloud, Ticp), *mapPointCloud, 0.6);
//outlierPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(outliers, mapFrame, mapCreationTime));
}
// check if news points should be added to the map
if (
mapping &&
((estimatedOverlap < maxOverlapToMerge) || (icp.getInternalMap().features.cols() < minMapPointCount)) &&
#if BOOST_VERSION >= 104100
(!mapBuildingInProgress)
#else // BOOST_VERSION >= 104100
true
#endif // BOOST_VERSION >= 104100
)
{
// make sure we process the last available map
mapCreationTime = stamp;
#if BOOST_VERSION >= 104100
ROS_INFO("Adding new points to the map in background");
mapBuildingTask = MapBuildingTask(boost::bind(&Mapper::updateMap, this, newPointCloud.release(), Ticp, true));
mapBuildingFuture = mapBuildingTask.get_future();
mapBuildingThread = boost::thread(boost::move(boost::ref(mapBuildingTask)));
mapBuildingInProgress = true;
#else // BOOST_VERSION >= 104100
ROS_INFO("Adding new points to the map");
setMap(updateMap( newPointCloud.release(), Ticp, true));
#endif // BOOST_VERSION >= 104100
}
}
catch (PM::ConvergenceError error)
{
ROS_ERROR_STREAM("ICP failed to converge: " << error.what());
return;
}
//Statistics about time and real-time capability
int realTimeRatio = 100*t.elapsed() / (stamp.toSec()-lastPoinCloudTime.toSec());
ROS_INFO_STREAM("[TIME] Total ICP took: " << t.elapsed() << " [s]");
if(realTimeRatio < 80)
ROS_INFO_STREAM("[TIME] Real-time capability: " << realTimeRatio << "%");
else
ROS_WARN_STREAM("[TIME] Real-time capability: " << realTimeRatio << "%");
lastPoinCloudTime = stamp;
}
void publishAidEPH(const ublox_msgs::AidEPH& m)
{
static ros::Publisher publisher = nh->advertise<ublox_msgs::AidEPH>("aideph", kROSQueueSize);
publisher.publish(m);
}
bool setImageCropLimitsCB(apc_msgs::SetImageCrop::Request &req, apc_msgs::SetImageCrop::Response &res)
{
double focalLength_x, focalLength_y;
double bin_x_min, bin_x_max, bin_y_min, bin_y_max, bin_z_min, bin_z_max;
int pix_x_min, pix_x_max, pix_y_min, pix_y_max;
ROS_INFO("Set Crop Service Called! :D");
// Calculate Focal Lengths
focalLength_x = 640 / (2 * (tan((57/2) * (M_PI/180))));
focalLength_y = 480 / (2 * (tan((43/2) * (M_PI/180))));
// focalLength_x = 610;
// focalLength_y = 610;
ROS_INFO("Focal length: %d, %d", (int)focalLength_x, (int)focalLength_x);
// Get Bin Details
// double dy = 0.1;
// double dz = 0.1;
//
// double w2 = req.width / 2;
// double h2 = req.height / 2;
// bin_x_min = (req.center.x - w2);
// bin_y_min = (req.center.y + h2);
// bin_z_min = req.center.z + 0.2;
//
// bin_x_max = (req.center.x + w2);
// bin_y_max = (req.center.y - h2);
// bin_z_max = req.center.z + 0.5;
bin_x_min = (req.o.x);
bin_y_min = (req.o.y) - .26; //TODO figure out the reason for the bias in Y direction
bin_z_min = req.o.z + 0.2;
bin_x_max = (req.b.x);
bin_y_max = (req.b.y) - .26; //TODO figure out the reason for the bias in Y direction
bin_z_max = req.b.z + 0.5;
ROS_INFO("The Bin min coordinate is: %f, %f, %f and The Bin max coordinate is: %f, %f, %f", bin_x_min, bin_y_min, bin_z_min, bin_x_max, bin_y_max, bin_z_max);
// Pixel Calculation
pix_x_min = ((bin_x_min * focalLength_x)/bin_z_min) + 320 - 20;
pix_y_min = ((bin_y_min * focalLength_y)/bin_z_min) + 240 + 20;
pix_x_max = ((bin_x_max * focalLength_x)/bin_z_min) + 320 + 20;
pix_y_max = ((bin_y_max * focalLength_y)/bin_z_min) + 240 - 20;
ROS_INFO("The Pixels are: %d, %d, %d, %d", pix_x_min, pix_y_min, pix_x_max, pix_y_max);
// Publishing corner markers
markerArray.markers.resize(4);
publishBinCornerMarker(req.o, 0);
publishBinCornerMarker(req.a, 1);
publishBinCornerMarker(req.b, 2);
publishBinCornerMarker(req.c, 3);
pub_marker_bin_corner_.publish(markerArray);
// Sanity check
if((pix_x_min < 0) | (pix_y_min < 0) | ((pix_x_min + abs(pix_x_max - pix_x_min)) > 640) | ((pix_y_min + abs(pix_y_max - pix_y_min))>480))
{
return true;
}
// Update Filter
roi_x = pix_x_min;
roi_y = pix_y_min;
roi_width = abs(pix_x_max - pix_x_min);
roi_height = abs(pix_y_max - pix_y_min);
ROS_INFO("The Filter is: %d, %d, %d, %d", roi_x, roi_y, roi_width, roi_height);
// Enable Crop
//cropped = req.crop_image;
res.success = true;
return true;
}
void publish(const MessageT& m, const std::string& topic) {
static ros::Publisher publisher = nh->advertise<MessageT>(topic, kROSQueueSize);
publisher.publish(m);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "IMU_Node2");
ros::NodeHandle n;
imu_pub = n.advertise<sensor_msgs::Imu>("/imu/data",100);
// Change the next line according to your port name and baud rate
try
{
if(device.isOpen())
{
ROS_INFO("Port is opened");
}
}
catch(serial::SerialException& e)
{
ROS_FATAL("Failed to open the serial port!!!");
ROS_BREAK();
}
ros::Rate r(50);
while(ros::ok())
{
// Get the reply, the last value is the timeout in ms
try{
std::string reply;
// ros::Duration(0.005).sleep();
device.readline(reply);
Roll = strtod(reply.c_str(),&pRoll)/2;
Pitch = strtod(pRoll,&pRoll)/2;
Yaw = strtod(pRoll,&pRoll)/2;
GyroX = strtod(pRoll,&pRoll);
GyroY = strtod(pRoll,&pRoll);
GyroZ = strtod(pRoll,&pRoll);
AccX = strtod(pRoll,&pRoll);
AccY = strtod(pRoll,&pRoll);
AccZ = strtod(pRoll,&pRoll);
Mx = strtod(pRoll,&pRoll);
My = strtod(pRoll,&pRoll);
Mz = strtod(pRoll,NULL);
ROS_INFO("Euler %.2f %.2f %.2f",Roll,Pitch,Yaw);
sensor_msgs::Imu imu;
geometry_msgs::Quaternion Q;
Q = tf::createQuaternionMsgFromRollPitchYaw(Roll*-1,Pitch*-1,Yaw*-1);
imu.orientation.x = 0;
imu.orientation.y = 0;
imu.orientation.z = 0;
imu.orientation.w = 1;
LinearX = (AccX/256)*9.806;
LinearY = (AccY/256)*9.806;
LinearZ = (AccZ/256)*9.806;
imu.angular_velocity.x = GyroX*-1*0.07*0.01745329252;;
imu.angular_velocity.y = GyroY*0.07*0.01745329252;;
imu.angular_velocity.z = GyroZ*-1*0.07*0.01745329252;;
imu.linear_acceleration.x = LinearX*-1;
imu.linear_acceleration.y = LinearY;
imu.linear_acceleration.z = LinearZ*-1;
imu.header.stamp = ros::Time::now();
imu.header.frame_id = "imu";
imu_pub.publish(imu);
}
catch(serial::SerialException& e)
{
ROS_INFO("Error %s",e.what());
}
r.sleep();
}
}
/**
* This function takes the Hand data published by the RightHandRobotics hand
* and sends that information in joint format to an rviz visualizer
*/
void publish_to_rviz(const reflex_msgs::HandConstPtr& hand)
{
// Pressure sensor setup
bool contact_val;
float pressure_val;
visualization_msgs::MarkerArray marker_array;
// Finger joint assignment
joint_state.header.stamp = ros::Time::now();
joint_state.position[0] = hand->finger[0].proximal;
joint_state.position[1] = hand->finger[1].proximal;
joint_state.position[2] = hand->finger[2].proximal;
joint_state.position[3] = hand->palm.preshape;
joint_state.position[4] = -hand->palm.preshape;
int index = num_fixed_steps;
for (int finger = 0; finger<3; finger++)
{
for (int i=0; i<(num_flex_steps+1); i++)
{
joint_state.position[index] = hand->finger[finger].distal/((float) (num_flex_steps+1));
index++;
}
}
// Pressure sensor assignment
for (int finger=0; finger<3; finger++)
{
char prox_fid[20];
sprintf(prox_fid, "/proximal_%d_tactile", (finger+1));
char dist_fid[20];
sprintf(dist_fid, "/distal_%d_tactile", (finger+1));
for (int i=0; i<sensors_per_finger; i++) // Loop through tactile sensors in the fingers
{
contact_val = hand->finger[finger].contact[i];
pressure_val = hand->finger[finger].pressure[i];
visualization_msgs::Marker contact_marker = makeContactMarker(contact_val, i, 0.004, 0.005, true);
visualization_msgs::Marker pressure_marker = makePressureMarker(pressure_val, i, 0.008, 0.003, true);
if (i < 5) { // Proximal link
contact_marker.header.frame_id = prox_fid;
pressure_marker.header.frame_id = prox_fid;
} else { // Distal link
contact_marker.header.frame_id = dist_fid;
pressure_marker.header.frame_id = dist_fid;
}
contact_marker.id = i + (finger*sensors_per_finger);
pressure_marker.id = i + (finger*sensors_per_finger);
marker_array.markers.push_back(contact_marker);
marker_array.markers.push_back(pressure_marker);
}
}
// Loop through tactile sensors in the palm
for (int i=0; i<11; i++)
{
contact_val = hand->palm.contact[palm_map[i]];
pressure_val = hand->palm.pressure[palm_map[i]];
visualization_msgs::Marker contact_marker = makeContactMarker(contact_val, i, 0.004, 0.01, false);
visualization_msgs::Marker pressure_marker = makePressureMarker(pressure_val, i, 0.009, 0.008, false);
marker_array.markers.push_back(contact_marker);
marker_array.markers.push_back(pressure_marker);
}
// Publish joint data
joint_pub.publish(joint_state);
// Publish sensor markers
sensor_pub.publish(marker_array);
}
void publishNavSVINFO(const ublox_msgs::NavSVINFO& m)
{
static ros::Publisher publisher = nh->advertise<ublox_msgs::NavSVINFO>("navsvinfo", kROSQueueSize);
publisher.publish(m);
}
int main(int argc, char**argv){
//initialize ROS
ros::init(argc, argv, "main");
//create a nodehandle
ros::NodeHandle nh;
//use handle to subscribe to messages (name, buffer, callback pointer)
ros::Subscriber skeleton_sub = nh.subscribe("/skeletons", 1, skeletonCallback);
//if this doesn't work, send it to the Converter without Braitenburg
//gesture_pub = nh.advertise<MotorControlMsg::MotorCommand>("Motor_Command", 1000);
gesture_pub = nh.advertise<robot_msgs::MotorData>("motordata",200);
ros::Rate loop_rate(10);
while(ros::ok()){
//all spinning conditions met, send the command
if(sendSpin){
/*
This was the version that was to be sent to Braitenburg.
MotorControlMsg::MotorCommand GestureMsg;
GestureMsg.precedence = -1;
GestureMsg.isLeftRightVel = true;
GestureMsg.linear_velocity = 0.25;
GestureMsg.angular_velocity = -0.25;
gesture_pub.publish(GestureMsg);
sendSpin = false;
*/
//create message to send to Converter
robot_msgs::MotorData GestureMsg;
GestureMsg.motor_left_velocity = 25;
GestureMsg.motor_left_time = 100;
GestureMsg.motor_right_velocity = -25;
GestureMsg.motor_right_time = 100;
//publish message 5 times because of fix in Converter for Braitenburg
for(int i=0; i<100; i++){
gesture_pub.publish(GestureMsg);
}
sendSpin = false;
spinSent = true;
ROS_INFO("SUCCESS!!! Motor Command sent.");
}
else if(stopSpin){
robot_msgs::MotorData GestureMsg;
//send zeroes to stop the spinning
GestureMsg.motor_left_velocity = 0;
GestureMsg.motor_left_time = 100;
GestureMsg.motor_right_velocity = 0;
GestureMsg.motor_right_time = 100;
//publish message 5 times because of fix in Converter for Braitenburg
for(int i=0; i<10; i++){
gesture_pub.publish(GestureMsg);
}
stopSpin = false;
ROS_INFO("SUCCESS!!! Motor Command sent.");
}
//give callback time to respond
ros::spinOnce();
loop_rate.sleep();
ROS_INFO("End of loop reached.");
}
}
void publishNavCLK(const ublox_msgs::NavCLOCK& m)
{
static ros::Publisher publisher = nh->advertise<ublox_msgs::NavCLOCK>("navclock", kROSQueueSize);
publisher.publish(m);
}
void poseCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
xRobot=msg->pose.pose.position.x;
yRobot=msg->pose.pose.position.y;
// orientationRobot=msg->pose.pose.orientation.z;
tf::Pose pose;
tf::poseMsgToTF(msg->pose.pose, pose);
orientationRobot= tf::getYaw(pose.getRotation()); //get radiand rotation (0 front, 3.14 back, left positive, right negative)
// ROS_INFO("xRobot: %f", xRobot);
// ROS_INFO("yRobot: %f", yRobot);
// ROS_INFO("orientationRobot: %f", orientationRobot);
ROS_INFO("xRobot: %f", xRobot);
ROS_INFO("yRobot: %f", yRobot);
// ROS_INFO("orientationRobot: %f", orientationRobot);
ROS_INFO("BigLeft: %d", BigLeft);
ROS_INFO("SmallLeft: %d", SmallLeft);
ROS_INFO("WallLeft: %d", WallLeft);
ROS_INFO("BigRight: %d", BigRight);
ROS_INFO("SmallRight: %d", SmallRight);
ROS_INFO("WallRight: %d", WallRight);
ROS_INFO("laser_obstacle_flag: %d", laser_obstacle_flag);
// ROS_INFO("laser_angular_velocity: %f", laser_angular_velocity);
ROS_INFO("xLaser: %f", xLaserPerson);
ROS_INFO("yLaser: %f", yLaserPerson);
// ROS_INFO("AngleErrorLaser: %f", AngleErrorLaser);
// ROS_INFO("KinectLaserError: %f", error);
ROS_INFO("match: %d", kinectLaserMatch);
ROS_INFO("Confidence: %f", confidence);
ROS_INFO("Height: %f", height);
ROS_INFO("distanceKinect: %f", distanceKinect);
// ROS_INFO("age: %f", age);
// ROS_INFO("AngleErrorKinect: %f", AngleErrorKinect);
ROS_INFO("AngleErrorPan: %f", (AngleErrorPan*180)/PI);
ROS_INFO("xKinect: %f", xperson);
ROS_INFO("yKinect: %f", yperson);
// ROS_INFO("AngleErrorFollow: %f", AngleErrorFollow);
ROS_INFO("xPath: 0");
ROS_INFO("yPath: 0");
ROS_INFO("xFollow: 0");
ROS_INFO("yFollow: 0");
ROS_INFO("tempDistance: %f", tempDistance);
// ROS_INFO("xLast1: %f", xLast1);
// ROS_INFO("yLast1: %f", yLast1);
// ROS_INFO("yDirection: %f", yDirection);
ROS_INFO("kinectTrack: %d", kinectTrack);
ROS_INFO("laserTrack: %d", laserTrack);
//////////////////////////////////marker
for(int i=0;i<100000;i++){
visualization_msgs::Marker marker;
marker.header.frame_id = "odom";
marker.header.stamp = ros::Time();
marker.ns = "robotPose";
marker.id = i;
marker.type = visualization_msgs::Marker::SPHERE;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime=ros::Duration(100.0);
marker.pose.position.x = xRobot;
marker.pose.position.y = yRobot;
marker.pose.position.z = 0;
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
// marker.pose.orientation.w = orientationRobot;
marker.pose.orientation.w = 1.0;
marker.scale.x = 0.2;
marker.scale.y = 0.2;
marker.scale.z = 0.2;
marker.color.a = 1.0; // Don't forget to set the alpha!
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
vis_pub3.publish( marker );
}
////////////////////////
}
void publishRxmRAW(const ublox_msgs::RxmRAW& m)
{
static ros::Publisher publisher = nh->advertise<ublox_msgs::RxmRAW>("rxmraw", kROSQueueSize);
publisher.publish(m);
}
void personCallback(const opt_msgs::TrackArray::ConstPtr& msg)
{
validTrackKinect=false;
//Initialize the twist
cmd_vel.linear.x = 0.0;
cmd_vel.angular.z = 0.0;
//Get the number of tracks in the TrackArray
nbOfTracksKinect=msg->tracks.size();
//If at least 1 track, proceed
if (nbOfTracksKinect>0) {
//looping throught the TrackArray
for(int i=0;i<nbOfTracksKinect && !validTrackKinect;i++){
//oldest track which is older than the age threshold and above the confidence threshold
if ((msg->tracks[i].age>AgeThreshold) && (msg->tracks[i].confidence>ConfidenceTheshold) && (msg->tracks[i].height>HeightTheshold) && (msg->tracks[i].height<HeightMaxTheshold)){
distanceKinect=msg->tracks[i].distance;
xperson=((msg->tracks[i].distance)*cos(AngleSmallError+AngleErrorPan));
yperson=((msg->tracks[i].distance)*sin(AngleSmallError+AngleErrorPan));
AngleErrorKinect=atan2(yperson,xperson);
age=msg->tracks[i].age;
height=msg->tracks[i].height;
confidence=msg->tracks[i].confidence;
YpathPoints.insert(YpathPoints.begin(),yperson);
if (YpathPoints.size()>5){
yLast1=YpathPoints.at(4);
yLast2=YpathPoints.at(1);
xLast1=xperson;
tempDistanceKinect=distanceKinect;
yDirection=yLast2-yLast1;
YpathPoints.pop_back();
}
error= sqrt(pow(xperson-xLaserPerson,2)+pow(yperson-yLaserPerson,2)); //calculate the x and y error between the kinect and the laser
// ROS_INFO("KinectLaserError: %f", error);
if (error<0.2){
kinectLaserMatch=true;
// ROS_INFO("match: %d", kinectLaserMatch);
}else{kinectLaserMatch=false;}
//Calculate distance error
DistanceError=distanceKinect-DistanceTarget;
//print to the console
// ROS_INFO("Confidence: %f", msg->tracks[i].confidence);
// ROS_INFO("Height: %f", msg->tracks[i].height);
// ROS_INFO("distanceKinect: %f", distanceKinect);
// ROS_INFO("age: %f", msg->tracks[i].age);
// ROS_INFO("AngleErrorKinect: %f", AngleErrorKinect);
// ROS_INFO("AngleErrorPan: %f", (AngleErrorPan*180)/ PI);
// ROS_INFO("xperson: %f", xperson);
// ROS_INFO("yperson: %f", yperson);
//Set command Twist
// if(smallError==false ){ //to reduce vibration around 0 angle of the kinect view, and 0 robot angle // && abs(AngleErrorPan)<0.05
// cmd_vel.angular.z = (AngleErrorPan+followingAngle)*KpAngle;
if (!laser_obstacle_flag){
angular_command= AngleErrorKinect*KpAngle;;
if(abs(followingAngle)>0.1 && abs(AngleErrorKinect)<0.2){angular_command = (AngleErrorKinect+followingAngle)*KpAngleOcclusion;}
// else {angular_command = AngleErrorKinect*KpAngle;}
if(angular_command>MaxTurn){angular_command=MaxTurn;}
if(angular_command<-MaxTurn){angular_command=-MaxTurn;}
cmd_vel.angular.z = angular_command;
// if(abs(followingAngle)<0.1){cmd_vel.angular.z = (AngleErrorKinect+followingAngle)*KpAngle;}
// else {cmd_vel.angular.z = (AngleErrorKinect+followingAngle)*KpAngleOcclusion;}
// cmd_vel.angular.z = AngleError*KpAngle;
// }
//Avoid going backward
if (DistanceError>0.05){ //threshold for small distance error of 0.05 meter
double command_speed=DistanceError*KpDistance;
//Limit the speed
if (command_speed>MaxSpeed){command_speed=MaxSpeed;}
if (command_speed<0){command_speed=0;}
cmd_vel.linear.x = command_speed;
}
//Stop for loop
validTrackKinect=true;
cmd_vel_pub.publish(cmd_vel);
}
}
}
kinectTrack=true;
//////////////////////////////////marker
visualization_msgs::Marker marker;
marker.header.frame_id = "base_link";
marker.header.stamp = ros::Time();
marker.ns = "kinect";
marker.id = 0;
marker.type = visualization_msgs::Marker::SPHERE;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = xperson;
marker.pose.position.y = yperson;
marker.pose.position.z = 0;
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = AngleErrorKinect;
marker.scale.x = 0.1;
marker.scale.y = 0.1;
marker.scale.z = 0.1;
marker.color.a = 1.0; // Don't forget to set the alpha!
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 1.0;
vis_pub2.publish( marker );
////////////////////////
}
else if(!validTrackLaser){
kinectTrack=false;
// validTrackKinect=false;
xPath= xRobot+cos(orientationRobot+AngleErrorKinect)*tempDistanceKinect;
yPath= yRobot+sin(orientationRobot+AngleErrorKinect)*tempDistanceKinect;
AngleErrorFollow=PI-atan2(yPath-yRobot,(xPath-xRobot))-PI+orientationRobot;
if(abs(AngleErrorFollow)>PI){
if(AngleErrorFollow<0){AngleErrorFollow=AngleErrorFollow+2*PI;}
else{AngleErrorFollow=AngleErrorFollow-2*PI;}
}
tempDistance=sqrt(pow(xPath-xRobot,2)+pow(yPath-yRobot,2));
//Get the number of tracks in the TrackArray
nbOfTracksKinect=msg->tracks.size();
/* //If at least 1 track, proceed
if (nbOfTracksKinect>0) {
//looping throught the TrackArray
for(int i=0;i<nbOfTracksKinect && !validTrackKinect;i++){
//oldest track which is older than the age threshold and above the confidence threshold
if ((msg->tracks[i].age>AgeThreshold) && (msg->tracks[i].confidence>ConfidenceTheshold) && (msg->tracks[i].height>HeightTheshold) && (msg->tracks[i].height<HeightMaxTheshold)){
validTrackKinect=true;
}
}
}
*/
if (!laser_obstacle_flag){
ros::Time start= ros::Time::now();
while((ros::Time::now()-start<ros::Duration(round(tempDistance/0.3))) && (!validTrackKinect) && (!laser_obstacle_flag)){
if (!laser_obstacle_flag)
{cmd_vel.linear.x = 0.3;
cmd_vel.angular.z=0.0;}
else{cmd_vel.angular.z = laser_angular_velocity;
cmd_vel.linear.x = laser_linear_velocity;}
}
cmd_vel.linear.x = 0.0;
if(!validTrackKinect){
if(yDirection>0){cmd_vel.angular.z=0.2;}
else{cmd_vel.angular.z=-0.2;}
}
//Stop for loop
// validTrack=true;
cmd_vel_pub.publish(cmd_vel);
// ROS_INFO("xLast1: %f", xLast1);
// ROS_INFO("yLast1: %f", yLast1);
// ROS_INFO("yDirection: %f", yDirection);
}
}
/* else{
xPath= xRobot+cos(orientationRobot+AngleErrorKinect)*tempDistanceKinect;
yPath= yRobot+sin(orientationRobot+AngleErrorKinect)*tempDistanceKinect;
AngleErrorFollow=PI-atan2(yPath-yRobot,(xPath-xRobot))-PI+orientationRobot;
if(abs(AngleErrorFollow)>PI){
if(AngleErrorFollow<0){AngleErrorFollow=AngleErrorFollow+2*PI;}
else{AngleErrorFollow=AngleErrorFollow-2*PI;}
}
// ROS_INFO("AngleErrorFollow: %f", AngleErrorFollow);
tempDistance=sqrt(pow(xPath-xRobot,2)+pow(yPath-yRobot,2))-count*50*0.3/1000;
count++;
// ROS_INFO("tempDistance: %f", tempDistance);
if (!laser_obstacle_flag){
// tempAngular=atan2(yLast1,xLast1);
cmd_vel.angular.z =-AngleErrorFollow*KpAngle;
if(AngleErrorFollow<0.5){cmd_vel.angular.z =0;}
// if(AngleErrorFollow<0.05&&tempDistance>0.5){
double command_speed=0.3;
cmd_vel.linear.x = command_speed;
// cmd_vel_pub.publish(cmd_vel);
if(tempDistance<0.5){
cmd_vel.linear.x = 0;
if (yDirection>0){cmd_vel.angular.z=0.2;}
else{cmd_vel.angular.z=-0.2;}
}
// }
//Stop for loop
validTrack=true;
cmd_vel_pub.publish(cmd_vel);
// ROS_INFO("xLast1: %f", xLast1);
// ROS_INFO("yLast1: %f", yLast1);
// ROS_INFO("yDirection: %f", yDirection);
}
}
*/
}
void publishRxmSFRB(const ublox_msgs::RxmSFRB& m)
{
static ros::Publisher publisher = nh->advertise<ublox_msgs::RxmSFRB>("rxmsfrb", kROSQueueSize);
publisher.publish(m);
}
void TeleopAria::keyLoop()
{
char c;
bool dirty=false;
// get the console in raw mode
tcgetattr(kfd, &cooked);
memcpy(&raw, &cooked, sizeof(struct termios));
raw.c_lflag &=~ (ICANON | ECHO);
// Setting a new line, then end of file
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
tcsetattr(kfd, TCSANOW, &raw);
puts("Reading from keyboard");
puts("---------------------------");
puts("Use arrow keys to move the the Robot.");
for(;;)
{
// get the next event from the keyboard
if(read(kfd, &c, 1) < 0)
{
perror("read():");
exit(-1);
}
myTransRatio=0.0, myRotRatio=0.0;
ROS_DEBUG("value: 0x%02X\n", c);
switch(c)
{
case KEYCODE_L:
ROS_DEBUG("LEFT");
myRotRatio = 100;
dirty = true;
break;
case KEYCODE_R:
ROS_DEBUG("RIGHT");
myRotRatio = -100;
dirty = true;
break;
case KEYCODE_U:
ROS_DEBUG("UP");
myTransRatio = 100;
dirty = true;
break;
case KEYCODE_D:
ROS_DEBUG("DOWN");
myTransRatio = -100;
dirty = true;
break;
}
ariaClientDriver::AriaCommandData msg;
msg.TransRatio=myTransRatio;
msg.RotRatio=myRotRatio;
msg.LatRatio=myLatRatio;
msg.MaxVel=myMaxVel;
if(dirty ==true)
{
myCommandDataPublisher.publish(msg);
dirty=false;
}
}
return;
}
void publishStatusTimerEventHandler(const ros::TimerEvent&)
{
std_msgs::String msg;
msg.data = "UNM ALSA";
status_publisher.publish(msg);
}