int main(int argc, char **argv) {
ros::init (argc, argv, "move_arm_pose_goal_test");
ros::NodeHandle nh;
actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_SchunkArm",true);
move_arm.waitForServer();
ROS_INFO("Connected to server");
arm_navigation_msgs::MoveArmGoal goalA;
double x = 0.5,
y = 0.1,
z = 0.5,
R = -M_PI_2, // greifer 'gerade' drehen
P = M_PI_2, // M_PI_2 negativ: greifer zeigt nach oben. und umgekehrt
Y = 0;
// 0 0 0.6 -M_PI_2 M_PI_4 0
// 0.3 0 0.4 -M_PI_2 M_PI_2 0
// 0.5 0 0.3 -M_PI_2 M_PI_2 0
// 0.4 0.1 0.4 -M_PI_2 M_PI_2 -M_PI_2
if(argc >= 1+6) { // cmd name + args
x = parseArg(argv[1]);
y = parseArg(argv[2]);
z = parseArg(argv[3]);
R = parseArg(argv[4]);
P = parseArg(argv[5]);
Y = parseArg(argv[6]);
}
ROS_INFO_STREAM("x="<<x<<" y="<<y<<" z="<<z<<" R="<<R<<" P="<<P<<" Y="<<Y);
goalA.motion_plan_request.group_name = "SchunkArm";
goalA.motion_plan_request.num_planning_attempts = 2;
goalA.motion_plan_request.planner_id = std::string("");
goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
arm_navigation_msgs::SimplePoseConstraint desired_pose;
desired_pose.header.frame_id = "arm_base_link";
desired_pose.link_name = "gripper";
desired_pose.pose.position.x = x;
desired_pose.pose.position.y = y;
desired_pose.pose.position.z = z;
double yaw = tan(desired_pose.pose.position.y / desired_pose.pose.position.x);
ROS_INFO_STREAM("yaw="<<yaw);
if(argc >= 1+7 && !strcmp(argv[7], "calcyaw")) {
ROS_INFO("Using calculated yaw");
Y = yaw;
}
desired_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(R, P, Y);
ROS_INFO_STREAM(desired_pose.pose.orientation);
desired_pose.absolute_position_tolerance.x = 0.02;
desired_pose.absolute_position_tolerance.y = 0.02;
desired_pose.absolute_position_tolerance.z = 0.02;
desired_pose.absolute_roll_tolerance = 0.04;
desired_pose.absolute_pitch_tolerance = 0.04;
desired_pose.absolute_yaw_tolerance = 0.04;
arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA);
if (nh.ok())
{
bool finished_within_time = false;
move_arm.sendGoal(goalA);
finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
if (!finished_within_time)
{
move_arm.cancelGoal();
ROS_INFO("Timed out achieving goal A");
}
else
{
actionlib::SimpleClientGoalState state = move_arm.getState();
bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
if(success)
ROS_INFO("Action finished: %s",state.toString().c_str());
else
ROS_INFO("Action failed: %s",state.toString().c_str());
}
}
ros::shutdown();
}
void callback(const sensor_msgs::ImageConstPtr &img,
const sensor_msgs::CameraInfoConstPtr &info) {
boost::mutex::scoped_lock lock(mutex_);
ros::Time now = ros::Time::now();
static boost::circular_buffer<double> in_times(100);
static boost::circular_buffer<double> out_times(100);
static boost::circular_buffer<double> in_bytes(100);
static boost::circular_buffer<double> out_bytes(100);
ROS_DEBUG("resize: callback");
if ( !publish_once_ || cp_.getNumSubscribers () == 0 ) {
ROS_DEBUG("resize: number of subscribers is 0, ignoring image");
return;
}
in_times.push_front((now - last_subscribe_time_).toSec());
in_bytes.push_front(img->data.size());
//
try {
int width = dst_width_ ? dst_width_ : (resize_x_ * info->width);
int height = dst_height_ ? dst_height_ : (resize_y_ * info->height);
double scale_x = dst_width_ ? ((double)dst_width_)/info->width : resize_x_;
double scale_y = dst_height_ ? ((double)dst_height_)/info->height : resize_y_;
cv_bridge::CvImagePtr cv_img = cv_bridge::toCvCopy(img);
cv::Mat tmpmat(height, width, cv_img->image.type());
cv::resize(cv_img->image, tmpmat, cv::Size(width, height));
cv_img->image = tmpmat;
sensor_msgs::CameraInfo tinfo = *info;
tinfo.height = height;
tinfo.width = width;
tinfo.K[0] = tinfo.K[0] * scale_x; // fx
tinfo.K[2] = tinfo.K[2] * scale_x; // cx
tinfo.K[4] = tinfo.K[4] * scale_y; // fy
tinfo.K[5] = tinfo.K[5] * scale_y; // cy
tinfo.P[0] = tinfo.P[0] * scale_x; // fx
tinfo.P[2] = tinfo.P[2] * scale_x; // cx
tinfo.P[3] = tinfo.P[3] * scale_x; // T
tinfo.P[5] = tinfo.P[5] * scale_y; // fy
tinfo.P[6] = tinfo.P[6] * scale_y; // cy
if ( !use_messages_ || now - last_publish_time_ > period_ ) {
cp_.publish(cv_img->toImageMsg(),
boost::make_shared<sensor_msgs::CameraInfo> (tinfo));
out_times.push_front((now - last_publish_time_).toSec());
out_bytes.push_front(cv_img->image.total()*cv_img->image.elemSize());
last_publish_time_ = now;
}
} catch( cv::Exception& e ) {
ROS_ERROR("%s", e.what());
}
float duration = (now - last_rosinfo_time_).toSec();
if ( duration > 2 ) {
int in_time_n = in_times.size();
int out_time_n = out_times.size();
double in_time_mean = 0, in_time_rate = 1.0, in_time_std_dev = 0.0, in_time_max_delta, in_time_min_delta;
double out_time_mean = 0, out_time_rate = 1.0, out_time_std_dev = 0.0, out_time_max_delta, out_time_min_delta;
std::for_each( in_times.begin(), in_times.end(), (in_time_mean += boost::lambda::_1) );
in_time_mean /= in_time_n;
in_time_rate /= in_time_mean;
std::for_each( in_times.begin(), in_times.end(), (in_time_std_dev += (boost::lambda::_1 - in_time_mean)*(boost::lambda::_1 - in_time_mean) ) );
in_time_std_dev = sqrt(in_time_std_dev/in_time_n);
if ( in_time_n > 1 ) {
in_time_min_delta = *std::min_element(in_times.begin(), in_times.end());
in_time_max_delta = *std::max_element(in_times.begin(), in_times.end());
}
std::for_each( out_times.begin(), out_times.end(), (out_time_mean += boost::lambda::_1) );
out_time_mean /= out_time_n;
out_time_rate /= out_time_mean;
std::for_each( out_times.begin(), out_times.end(), (out_time_std_dev += (boost::lambda::_1 - out_time_mean)*(boost::lambda::_1 - out_time_mean) ) );
out_time_std_dev = sqrt(out_time_std_dev/out_time_n);
if ( out_time_n > 1 ) {
out_time_min_delta = *std::min_element(out_times.begin(), out_times.end());
out_time_max_delta = *std::max_element(out_times.begin(), out_times.end());
}
double in_byte_mean = 0, out_byte_mean = 0;
std::for_each( in_bytes.begin(), in_bytes.end(), (in_byte_mean += boost::lambda::_1) );
std::for_each( out_bytes.begin(), out_bytes.end(), (out_byte_mean += boost::lambda::_1) );
in_byte_mean /= duration;
out_byte_mean /= duration;
ROS_INFO_STREAM(" in bandwidth: " << std::fixed << std::setw(11) << std::setprecision(3) << in_byte_mean/1000*8
<< " Kbps rate:" << std::fixed << std::setw(7) << std::setprecision(3) << in_time_rate
<< " hz min:" << std::fixed << std::setw(7) << std::setprecision(3) << in_time_min_delta
<< " s max: " << std::fixed << std::setw(7) << std::setprecision(3) << in_time_max_delta
<< " s std_dev: "<< std::fixed << std::setw(7) << std::setprecision(3) << in_time_std_dev << "s n: " << in_time_n);
ROS_INFO_STREAM(" out bandwidth: " << std::fixed << std::setw(11) << std::setprecision(3) << out_byte_mean/1000*8
<< " kbps rate:" << std::fixed << std::setw(7) << std::setprecision(3) << out_time_rate
<< " hz min:" << std::fixed << std::setw(7) << std::setprecision(3) << out_time_min_delta
<< " s max: " << std::fixed << std::setw(7) << std::setprecision(3) << out_time_max_delta
<< " s std_dev: "<< std::fixed << std::setw(7) << std::setprecision(3) << out_time_std_dev << "s n: " << out_time_n);
in_times.clear();
in_bytes.clear();
out_times.clear();
out_bytes.clear();
last_rosinfo_time_ = now;
}
last_subscribe_time_ = now;
if(use_snapshot_) {
publish_once_ = false;
}
}
int main(int argc, char **argv)
{
if (argc < 2)
{
std::cout << "This program need one input parameter.\n"<<
"The first input parameter is the number of the UAV." << std::endl;
return -1;
}
// The UAV ID is stored in a global variable
std::string uavID="aal_";
uavID.append(std::string(argv[1]));
signal(SIGINT, quit);
string node_name="arm_joystick";
node_name.append(std::string(argv[1]));
ros::init(argc, argv, node_name.c_str());
ros::NodeHandle n(uavID);
string topicname = "/cmd_vel";
//Publisher and Subscribers
ros::Publisher arm_control_ref_pub = n.advertise<arcas_msgs::ArmControlReferencesStamped>("/aal_1/arm_control_references",0);
ros::Subscriber state_estimation_subscriber = n.subscribe("/aal_1/arm_state_estimation",
1000, &armStateEstimationCallback);
int fd;
struct wwvi_js_event wjse;
wjse.stick2_x = 0; // Roll
wjse.stick2_y = 0; // Yaw
wjse.stick_x = 0; // Pitch
wjse.stick_y = 0; // Thrust
wjse.button[0]=0; // Take-Off, Button 1 on Joystick
wjse.button[1]=0; // Land, Button 2 on Joystick
CJoystick joystick;
fd = joystick.open_joystick("/dev/input/js1");
// Initialize ActionClient
AALExtensionActionWrapper aalExtensionActionWrapper;
if(!fd)
{
cerr << "Can not open joystick, is it still connected?\r\n" << endl;
return(-1);
}
ros::AsyncSpinner spinner_(0);
spinner_.start();
while(ros::ok())
{
// Calculate the quad_control_references
if(wjse.stick_x!=0)
{
arm_control_references.arm_control_references.position_ref[2] = arm_state_estimation.arm_state_estimation.position[2] + wjse.stick_x/6000.0;
}
if(wjse.stick2_x!=0)
{
arm_control_references.arm_control_references.position_ref[3] = arm_state_estimation.arm_state_estimation.position[3] + wjse.stick2_x/6000.0;
}
if(wjse.stick_y!=0)
{
arm_control_references.arm_control_references.position_ref[1] = arm_state_estimation.arm_state_estimation.position[1] + wjse.stick_y/6000.0;
}
if( wjse.stick2_y!=0)
{
arm_control_references.arm_control_references.position_ref[0] = arm_state_estimation.arm_state_estimation.position[0] + wjse.stick2_y/6000.0;
}
if(wjse.stick3_x!=0)
{
arm_control_references.arm_control_references.position_ref[4] = arm_state_estimation.arm_state_estimation.position[4] + wjse.stick3_x/12000.0;
}
if(wjse.stick3_y!=0)
{
arm_control_references.arm_control_references.position_ref[5] = arm_state_estimation.arm_state_estimation.position[5] + wjse.stick3_y/12000.0;
}
/*
if(joystick.get_joystick_status(&wjse)!=-1)
{
command_to_send.linear.x = wjse.stick_x/2500.0;
//std::cout << "Pitch control: " << command_to_send.linear.x << std::endl;
command_to_send.linear.y = wjse.stick2_x/2500.0;
//std::cout << "Roll control: " << command_to_send.linear.y << std::endl;
command_to_send.linear.z = wjse.stick_y/4000.0;
//std::cout << "Thrust control: " << command_to_send.linear.z << std::endl;
command_to_send.angular.z = wjse.stick2_y/4000.0;
//std::cout << "Yaw control: " << command_to_send.angular.z << std::endl;
}
else
{
ROS_ERROR_STREAM("Problems with joystick!");
}
*/
// Implement take-off
if((joystick.get_joystick_status(&wjse)!=-1) && (wjse.button[0] == 1))
{
ROS_INFO_STREAM("Extracting Sent");
aalExtensionActionWrapper.extendArm();
usleep(100000); // In order to not send it twice accidentally
}
// Implement landing
if((joystick.get_joystick_status(&wjse)!=-1) && (wjse.button[1] == 1))
{
ROS_INFO_STREAM("Contracting Sent");
aalExtensionActionWrapper.contractArm();
usleep(100000); // In order to not send it twice accidentally
}
if((joystick.get_joystick_status(&wjse)!=-1) && (wjse.button[2] == 1))
{
arm_control_references.arm_control_references.position_ref[6] = 3.14159/5;
usleep(100000); // In order to not send it twice accidentally
}
if((joystick.get_joystick_status(&wjse)!=-1) && (wjse.button[3] == 1))
{
arm_control_references.arm_control_references.position_ref[6] =0.0;
usleep(100000); // In order to not send it twice accidentally
}
//joystick_publisher.publish(command_to_send);
arm_control_ref_pub.publish(arm_control_references);
//~20Hz
usleep(50000);
}
return 0;
}
/*
* Get the list of controller names.
*/
virtual void getControllersList(std::vector<std::string> &names) {
for (std::map<std::string, ActionBasedControllerHandleBasePtr>::const_iterator it = controllers_.begin() ; it != controllers_.end() ; ++it)
names.push_back(it->first);
ROS_INFO_STREAM("Returned " << names.size() << " controllers in list");
}
int main( int argc, char **argv )
{
ros::init( argc, argv, "example2" );
ros::NodeHandle n;
const double val = 3.14;
// Basic messages:
ROS_INFO( "My INFO message." );
ROS_INFO( "My INFO message with argument: %f", val );
ROS_INFO_STREAM(
"My INFO stream message with argument: " << val
);
// Named messages:
ROS_INFO_STREAM_NAMED(
"named_msg",
"My named INFO stream message; val = " << val
);
// Conditional messages:
ROS_INFO_STREAM_COND(
val < 0.,
"My conditional INFO stream message; val (" << val << ") < 0"
);
ROS_INFO_STREAM_COND(
val >= 0.,
"My conditional INFO stream message; val (" << val << ") >= 0"
);
// Conditional Named messages:
ROS_INFO_STREAM_COND_NAMED(
val < 0., "cond_named_msg",
"My conditional INFO stream message; val (" << val << ") < 0"
);
ROS_INFO_STREAM_COND_NAMED(
val >= 0., "cond_named_msg",
"My conditional INFO stream message; val (" << val << ") >= 0"
);
// Filtered messages:
struct MyLowerFilter : public ros::console::FilterBase {
MyLowerFilter( const double& val ) : value( val ) {}
inline virtual bool isEnabled()
{
return value < 0.;
}
double value;
};
struct MyGreaterEqualFilter : public ros::console::FilterBase {
MyGreaterEqualFilter( const double& val ) : value( val ) {}
inline virtual bool isEnabled()
{
return value >= 0.;
}
double value;
};
MyLowerFilter filter_lower( val );
MyGreaterEqualFilter filter_greater_equal( val );
ROS_INFO_STREAM_FILTER(
&filter_lower,
"My filter INFO stream message; val (" << val << ") < 0"
);
ROS_INFO_STREAM_FILTER(
&filter_greater_equal,
"My filter INFO stream message; val (" << val << ") >= 0"
);
// Once messages:
for( int i = 0; i < 10; ++i ) {
ROS_INFO_STREAM_ONCE(
"My once INFO stream message; i = " << i
);
}
// Throttle messages:
for( int i = 0; i < 10; ++i ) {
ROS_INFO_STREAM_THROTTLE(
2,
"My throttle INFO stream message; i = " << i
);
ros::Duration( 1 ).sleep();
}
ros::spinOnce();
return EXIT_SUCCESS;
}
int main(int argc, char ** argv)
{
ros::init(argc, argv, "mpu_calculate");
ros::NodeHandle nh;
//ros::Subscriber imu_data = nh.subscribe();
ros::Publisher imuPub = nh.advertise<sensor_msgs::Imu>("imudata",1);
//串口读取数据
// serial::Serial serport;
// try
// {
// //打开串口0
// serport.setPort("/dev/ttyUSB0");
// serport.setBaudrate(115200);
// serial::Timeout to = serial::Timeout::simpleTimeout(0);
// serport.setTimeout(to);
// serport.open();
// }
// catch(serial::IOException & e)
// {
// ROS_ERROR_STREAM("Unable to open serial port");
// return -1;
// }
// //判断串口是否打开
// if(serport.isOpen())
// {
// ROS_INFO_STREAM("Serial Port initialized");
// }
// else
// {
// return -1;
// }
// serport.flushInput();
//uint8 dataPacket[PKGSIZE];
m_serial.init();
sensor_msgs::Imu imu_msg;
//判断是否已经标定过
bool calibrated = false;
double zeroshift[3];
int times = 0;
ROS_INFO_STREAM("Reading from serial port");
std::cout << "start calibrate gyro ";
ros::Rate loop_rate(200);
double starttime, endtime;
while(ros::ok())
{
starttime = ros::Time::now().toSec();
ros::spinOnce();
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin( tf::Vector3(0.0, 0.0, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, 0);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", "imu"));
// int datasize = serport.read(dataPacket, packetLen);
m_serial.Read();
if (m_serial.isReadOK == 1)
{
m_serial.isReadOK = 0;
if(parseimumsg(m_serial.readbuff, &imu_msg) == true)
{
//解析imu的datapacket
if(!calibrated)
{
ROS_INFO_STREAM("Calibrating");
if(times == 200)
{
std::cout << std::endl;
calibrated = true;
zeroshift[0] /= 200;
zeroshift[1] /= 200;
zeroshift[2] /= 200;
Global_Initialise(roll_init/200.0,pitch_init/200.0,yaw_init/200.0);
}
else
{
zeroshift[0] += imu_msg.angular_velocity.x;
zeroshift[1] += imu_msg.angular_velocity.y;
zeroshift[2] += imu_msg.angular_velocity.z;
GetInitAng(&imu_msg);
std::cout << ".";
}
times += 1;
}
else
{
imu_msg.angular_velocity.x -= zeroshift[0];
imu_msg.angular_velocity.y -= zeroshift[1];
imu_msg.angular_velocity.z -= zeroshift[2];
// std::cout << "--------------------------------" << std::endl;
// std::cout << imu_msg.angular_velocity.x <<" "<< imu_msg.angular_velocity.y << " " << imu_msg.angular_velocity.z << std::endl;
// std::cout << imu_msg.linear_acceleration.x <<" "<< imu_msg.linear_acceleration.y <<" "<< imu_msg.linear_acceleration.z << std::endl;
imuPub.publish(imu_msg);
pixhawk_ekf(imu_msg.angular_velocity.x,imu_msg.angular_velocity.y,imu_msg.angular_velocity.z,imu_msg.linear_acceleration.x,imu_msg.linear_acceleration.y,imu_msg.linear_acceleration.z);
//ROS_INFO_STREAM("OK!");
}
}
}
//发送数据
//endtime = ros::Time::now().toSec();
//std::cout << "time" << endtime - starttime << std::endl;
loop_rate.sleep();
}
}
void GripperAction::goalCB(GoalHandle gh)
{
// Cancels the currently active goal.
if (has_active_goal_)
{
// Marks the current goal as canceled.
active_goal_.setCanceled();
has_active_goal_ = false;
std::cout << "canceled current active goal" << std::endl;
}
gh.setAccepted();
active_goal_ = gh;
has_active_goal_ = true;
goal_received_ = ros::Time::now();
min_error_seen_ = 1e10;
ROS_INFO_STREAM("Gripper target position: " << active_goal_.getGoal()->command.position << "effort: " << active_goal_.getGoal()->command.max_effort);
ROS_INFO("setFingersValues");
std::vector<double> fingerPositionsRadian = jaco_->getFingersJointAngle();
std::cout << "current fingerpositions: " << fingerPositionsRadian[0] << " " << fingerPositionsRadian[1] << " " << fingerPositionsRadian[2] << std::endl;
target_position = active_goal_.getGoal()->command.position;
target_effort = active_goal_.getGoal()->command.max_effort;
//determine if the gripper is supposed to be opened or closed
double current_position = jaco_->getFingersJointAngle()[0];
if(current_position > target_position){
opening = true;
std::cout << "Opening gripper" << std::endl;
}else{
opening = false;
std::cout << "Closing gripper" << std::endl;
}
fingerPositionsRadian[0] = target_position;
fingerPositionsRadian[1] = target_position;
fingerPositionsRadian[2] = target_position;
std::cout << "Gripper target position: " << active_goal_.getGoal()->command.position << "effort: " << active_goal_.getGoal()->command.max_effort << std::endl;
double fingerPositionsDegree[3] = {radToDeg(fingerPositionsRadian[0]),radToDeg(fingerPositionsRadian[1]),radToDeg(fingerPositionsRadian[2])};
if(!jaco_->setFingersValues(fingerPositionsDegree))
{
active_goal_.setCanceled();
ROS_ERROR("Cancelling goal: moveJoint didn't work.");
std::cout << "Cancelling goal because setFingers didn't work" << std::endl;
}
last_movement_time_ = ros::Time::now();
}
int getCamCalibration(string dir_root, string camera_name, double* K,std::vector<double> & D,double *R,double* P){
/*
* from: http://kitti.is.tue.mpg.de/kitti/devkit_raw_data.zip
* calib_cam_to_cam.txt: Camera-to-camera calibration
* --------------------------------------------------
*
* - S_xx: 1x2 size of image xx before rectification
* - K_xx: 3x3 calibration matrix of camera xx before rectification
* - D_xx: 1x5 distortion vector of camera xx before rectification
* - R_xx: 3x3 rotation matrix of camera xx (extrinsic)
* - T_xx: 3x1 translation vector of camera xx (extrinsic)
* - S_rect_xx: 1x2 size of image xx after rectification
* - R_rect_xx: 3x3 rectifying rotation to make image planes co-planar
* - P_rect_xx: 3x4 projection matrix after rectification
*/
// double K[9]; // Calibration Matrix
// double D[5]; // Distortion Coefficients
// double R[9]; // Rectification Matrix
// double P[12]; // Projection Matrix Rectified (u,v,w) = P * R * (x,y,z,q)
string calib_cam_to_cam=(fs::path(dir_root) / fs::path("calib_cam_to_cam.txt")).string();
ifstream file_c2c(calib_cam_to_cam.c_str());
if (!file_c2c.is_open())
return false;
ROS_INFO_STREAM("Reading camera" << camera_name << " calibration from " << calib_cam_to_cam);
typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
boost::char_separator<char> sep{" "};
string line="";
char index=0;
tokenizer::iterator token_iterator;
while (getline(file_c2c,line))
{
// Parse string phase 1, tokenize it using Boost.
tokenizer tok(line,sep);
// Move the iterator at the beginning of the tokenize vector and check for K/D/R/P matrices.
token_iterator=tok.begin();
if (strcmp((*token_iterator).c_str(),((string)(string("K_")+camera_name+string(":"))).c_str())==0) //Calibration Matrix
{
index=0; //should be 9 at the end
ROS_DEBUG_STREAM("K_" << camera_name);
for (token_iterator++; token_iterator != tok.end(); token_iterator++)
{
//std::cout << *token_iterator << '\n';
K[index++]=boost::lexical_cast<double>(*token_iterator);
}
}
// EXPERIMENTAL: use with unrectified images
// token_iterator=tok.begin();
// if (strcmp((*token_iterator).c_str(),((string)(string("D_")+camera_name+string(":"))).c_str())==0) //Distortion Coefficients
// {
// index=0; //should be 5 at the end
// ROS_DEBUG_STREAM("D_" << camera_name);
// for (token_iterator++; token_iterator != tok.end(); token_iterator++)
// {
//// std::cout << *token_iterator << '\n';
// D[index++]=boost::lexical_cast<double>(*token_iterator);
// }
// }
token_iterator=tok.begin();
if (strcmp((*token_iterator).c_str(),((string)(string("R_")+camera_name+string(":"))).c_str())==0) //Rectification Matrix
{
index=0; //should be 12 at the end
ROS_DEBUG_STREAM("R_" << camera_name);
for (token_iterator++; token_iterator != tok.end(); token_iterator++)
{
//std::cout << *token_iterator << '\n';
R[index++]=boost::lexical_cast<double>(*token_iterator);
}
}
token_iterator=tok.begin();
if (strcmp((*token_iterator).c_str(),((string)(string("P_rect_")+camera_name+string(":"))).c_str())==0) //Projection Matrix Rectified
{
index=0; //should be 12 at the end
ROS_DEBUG_STREAM("P_rect_" << camera_name);
for (token_iterator++; token_iterator != tok.end(); token_iterator++)
{
//std::cout << *token_iterator << '\n';
P[index++]=boost::lexical_cast<double>(*token_iterator);
}
}
}
ROS_INFO_STREAM("... ok");
return true;
}
void GSPipe::publish_stream() {
ROS_INFO_STREAM("Publishing stream...");
// Pre-roll camera if needed
if (preroll_) {
ROS_DEBUG("Performing preroll...");
//The PAUSE, PLAY, PAUSE, PLAY cycle is to ensure proper pre-roll
//I am told this is needed and am erring on the side of caution.
gst_element_set_state(pipeline_, GST_STATE_PLAYING);
if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
ROS_ERROR("Failed to PLAY during preroll.");
return;
} else {
ROS_DEBUG("Stream is PLAYING in preroll.");
}
gst_element_set_state(pipeline_, GST_STATE_PAUSED);
if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
ROS_ERROR("Failed to PAUSE.");
return;
} else {
ROS_INFO("Stream is PAUSED in preroll.");
}
}
if(gst_element_set_state(pipeline_, GST_STATE_PLAYING) == GST_STATE_CHANGE_FAILURE) {
ROS_ERROR("Could not start stream!");
return;
}
ROS_INFO("Started stream.");
// Poll the data as fast a spossible
while(ros::ok()) {
// This should block until a new frame is awake, this way, we'll run at the
// actual capture framerate of the device.
ROS_DEBUG("Getting data...");
GstBuffer* buf = gst_app_sink_pull_buffer(GST_APP_SINK(sink_));
GstFormat fmt = GST_FORMAT_TIME;
gint64 current = -1;
// Query the current position of the stream
//if (gst_element_query_position(pipeline_, &fmt, ¤t)) {
//ROS_INFO_STREAM("Position "<<current);
//}
// Stop on end of stream
if (!buf) {
ROS_INFO("Stream ended.");
break;
}
ROS_DEBUG("Got data.");
// Get the image width and height
GstPad* pad = gst_element_get_static_pad(sink_, "sink");
const GstCaps *caps = gst_pad_get_negotiated_caps(pad);
GstStructure *structure = gst_caps_get_structure(caps,0);
gst_structure_get_int(structure,"width",&width_);
gst_structure_get_int(structure,"height",&height_);
// Complain if the returned buffer is smaller than we expect
const unsigned int expected_frame_size =
image_encoding_ == sensor_msgs::image_encodings::RGB8
? width_ * height_ * 3
: width_ * height_;
if (buf->size < expected_frame_size) {
ROS_WARN_STREAM( "GStreamer image buffer underflow: Expected frame to be "
<< expected_frame_size << " bytes but got only "
<< (buf->size) << " bytes. (make sure frames are correctly encoded)");
}
// Construct Image message
sensor_msgs::ImagePtr img(new sensor_msgs::Image());
sensor_msgs::CameraInfoPtr cinfo;
// Update header information
cinfo.reset(new sensor_msgs::CameraInfo(camera_info_manager_.getCameraInfo()));
cinfo->header.stamp = ros::Time::now();
cinfo->header.frame_id = frame_id_;
img->header = cinfo->header;
// Image data and metadata
img->width = width_;
img->height = height_;
img->encoding = image_encoding_;
img->is_bigendian = false;
img->data.resize(expected_frame_size);
ROS_WARN("Image encoding: %s Width: %i Height %i \n", image_encoding_.c_str(), width_, height_);
// Copy only the data we received
// Since we're publishing shared pointers, we need to copy the image so
// we can free the buffer allocated by gstreamer
if (image_encoding_ == sensor_msgs::image_encodings::RGB8) {
img->step = width_ * 3;
} else {
img->step = width_;
}
std::copy(
buf->data,
(buf->data)+(buf->size),
img->data.begin());
// Publish the image/info
camera_pub_.publish(img, cinfo);
// Release the buffer
gst_buffer_unref(buf);
ros::spinOnce();
}
}
// Initialization process
void EkfNode::init() {
/**************************************************************************
* Initialize firstRun, time, and frame names
**************************************************************************/
// Set first run to true for encoders. Once a message is received, this will
// be set to false.
firstRunEnc_ = true;
firstRunSys_ = true;
// Set Times
ros::Time currTime = ros::Time::now();
lastEncTime_ = currTime;
lastSysTime_ = currTime;
// Use the ROS parameter server to initilize parameters
if(!private_nh_.getParam("base_frame", base_frame_))
base_frame_ = "base_link";
if(!private_nh_.getParam("odom_frame", base_frame_))
odom_frame_ = "odom";
if(!private_nh_.getParam("map_frame", map_frame_))
map_frame_ = "map";
/**************************************************************************
* Initialize state for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize state
double state_temp[] = {0, 0, 0, 0, 0, 0};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> state (state_temp, state_temp + sizeof(state_temp) / sizeof(double));
// Check the parameter server and initialize state
if(!private_nh_.getParam("state", state)) {
ROS_WARN_STREAM("No state found. Using default.");
}
// Check to see if the size is not equal to 6
if (state.size() != 6) {
ROS_WARN_STREAM("state isn't 6 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 6, 1> Vector6d;
Vector6d stateMat(state.data());
std::cout << "state_map = " << std::endl;
std::cout << stateMat << std::endl;
ekf_map_.initState(stateMat);
// Odom is always initialized at all zeros.
stateMat << 0, 0, 0, 0, 0, 0;
ekf_odom_.initState(stateMat);
std::cout << "state_odom = " << std::endl;
std::cout << stateMat << std::endl;
/**************************************************************************
* Initialize covariance for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize covariance
double cov_temp[] = {0.01, 0, 0, 0, 0, 0,
0, 0.01, 0, 0, 0, 0,
0, 0, 0.01, 0, 0, 0,
0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> cov (cov_temp, cov_temp + sizeof(cov_temp) / sizeof(double));
// Check the parameter server and initialize cov
if(!private_nh_.getParam("covariance", cov)) {
ROS_WARN_STREAM("No covariance found. Using default.");
}
// Check to see if the size is not equal to 36
if (cov.size() != 36) {
ROS_WARN_STREAM("cov isn't 36 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 6, 6, RowMajor> Matrix66;
Matrix66 covMat(cov.data());
std::cout << "covariance = " << std::endl;
std::cout << covMat << std::endl;
ekf_map_.initCov(covMat);
// Initialize odom covariance the same as the map covariance (this isn't
// correct. But, since it is all an estimate anyway, it should be fine.
ekf_odom_.initCov(covMat);
/**************************************************************************
* Initialize Q for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize Q
double Q_temp[] = {0.01, 0, 0, 0, 0, 0,
0, 0.01, 0, 0, 0, 0,
0, 0, 0.01, 0, 0, 0,
0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> Q (Q_temp, Q_temp + sizeof(Q_temp) / sizeof(double));
// Check the parameter server and initialize Q
if(!private_nh_.getParam("Q", Q)) {
ROS_WARN_STREAM("No Q found. Using default.");
}
// Check to see if the size is not equal to 36
if (Q.size() != 36) {
ROS_WARN_STREAM("Q isn't 36 elements long!");
}
// And initialize the Matrix
Matrix66 QMat(Q.data());
std::cout << "Q = " << std::endl;
std::cout << QMat << std::endl;
ekf_map_.initSystem(QMat);
ekf_odom_.initSystem(QMat);
/**************************************************************************
* Initialize Decawave for ekf_map_ (not used in ekf_odom_)
**************************************************************************/
// Create temp array to initialize R for DecaWave
double RDW_temp[] = {0.01, 0, 0, 0,
0, 0.01, 0, 0,
0, 0, 0.01, 0,
0, 0, 0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> RDW (RDW_temp, RDW_temp + sizeof(RDW_temp) / sizeof(double));
// Check the parameter server and initialize RDW
if(!private_nh_.getParam("DW_R", RDW)) {
ROS_WARN_STREAM("No DW_R found. Using default.");
}
// Check to see if the size is not equal to 16
if (RDW.size() != 16) {
ROS_WARN_STREAM("DW_R isn't 16 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 4, 4, RowMajor> Matrix44;
Matrix44 RDWMat(RDW.data());
std::cout << "RDecaWave = " << std::endl;
std::cout << RDWMat << std::endl;
// Create temp array to initialize beacon locations for DecaWave
double DWBL_temp[] = {0, 0,
5, 0,
5, 15,
0, 15};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> DWBL (DWBL_temp, DWBL_temp + sizeof(DWBL_temp) / sizeof(double));
// Check the parameter server and initialize DWBL
if(!private_nh_.getParam("DW_Beacon_Loc", DWBL)) {
ROS_WARN_STREAM("No DW_Beacon_Loc found. Using default.");
}
// Check to see if the size is not equal to 8
if (DWBL.size() != 8) {
ROS_WARN_STREAM("DW_Beacon_Loc isn't 8 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 4, 2, RowMajor> Matrix42;
Matrix42 DWBLMat(DWBL.data());
std::cout << "DW_Beacon_Loc = " << std::endl;
std::cout << DWBLMat << std::endl;
MatrixXd DecaWaveBeaconLoc(4,2);
MatrixXd DecaWaveOffset(2,1);
double DW_offset_x;
double DW_offset_y;
if(!private_nh_.getParam("DW_offset_x", DW_offset_x))
DW_offset_x = 0.0;
if(!private_nh_.getParam("DW_offset_y", DW_offset_y))
DW_offset_y = 0.0;
DecaWaveOffset << DW_offset_x, DW_offset_y;
std::cout << "DecaWaveOffset = " << std::endl;
std::cout << DecaWaveOffset << std::endl;
ekf_map_.initDecaWave(RDWMat, DWBLMat, DecaWaveOffset);
// Decawave is not used in odom, so no need to initialize
/**************************************************************************
* Initialize Encoders for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize R for DecaWave
double REnc_temp[] = {0.01, 0,
0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> REnc (REnc_temp, REnc_temp + sizeof(REnc_temp) / sizeof(double));
// Check the parameter server and initialize RDW
if(!private_nh_.getParam("Enc_R", REnc)) {
ROS_WARN_STREAM("No Enc_R found. Using default.");
}
// Check to see if the size is not equal to 4
if (REnc.size() != 4) {
ROS_WARN_STREAM("Enc_R isn't 4 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 2, 2, RowMajor> Matrix22;
Matrix22 REncMat(REnc.data());
std::cout << "R_Enc = " << std::endl;
std::cout << REncMat << std::endl;
double b, tpmRight, tpmLeft;
if(!private_nh_.getParam("track_width", b))
b = 1;
if(!private_nh_.getParam("tpm_right", tpmRight))
tpmRight = 1;
if(!private_nh_.getParam("tpm_left", tpmLeft))
tpmLeft = 1;
ekf_map_.initEnc(REncMat, b, tpmRight, tpmLeft);
ekf_odom_.initEnc(REncMat, b, tpmRight, tpmLeft);
std::cout << "track_width = " << b << std::endl;
std::cout << "tpm_right = " << tpmRight << std::endl;
std::cout << "tpm_left = " << tpmLeft << std::endl;
/**************************************************************************
* Initialize IMU for ekf_odom_ and ekf_map_
**************************************************************************/
double RIMU;
if(!private_nh_.getParam("R_IMU", RIMU))
RIMU = 0.01;
ekf_map_.initIMU(RIMU);
ekf_odom_.initIMU(RIMU);
std::cout << "R_IMU = " << RIMU << std::endl;
// Publish the initialized state and exit initialization.
publishState(currTime);
ROS_INFO_STREAM("Finished with initialization.");
}
int main(int argc, char** argv) {
ros::init(argc, argv, "example_cart_move_action_client"); // name this node
ros::NodeHandle nh; //standard ros node handle
ArmMotionCommander arm_motion_commander(&nh);
CwruPclUtils cwru_pcl_utils(&nh);
while (!cwru_pcl_utils.got_kinect_cloud()) {
ROS_INFO("did not receive pointcloud");
ros::spinOnce();
ros::Duration(1.0).sleep();
}
ROS_INFO("got a pointcloud");
tf::StampedTransform tf_sensor_frame_to_torso_frame; //use this to transform sensor frame to torso frame
tf::TransformListener tf_listener; //start a transform listener
//let's warm up the tf_listener, to make sure it get's all the transforms it needs to avoid crashing:
bool tferr = true;
ROS_INFO("waiting for tf between kinect_pc_frame and torso...");
while (tferr) {
tferr = false;
try {
//The direction of the transform returned will be from the target_frame to the source_frame.
//Which if applied to data, will transform data in the source_frame into the target_frame. See tf/CoordinateFrameConventions#Transform_Direction
tf_listener.lookupTransform("torso", "kinect_pc_frame", ros::Time(0), tf_sensor_frame_to_torso_frame);
} catch (tf::TransformException &exception) {
ROS_ERROR("%s", exception.what());
tferr = true;
ros::Duration(0.5).sleep(); // sleep for half a second
ros::spinOnce();
}
}
ROS_INFO("tf is good"); //tf-listener found a complete chain from sensor to world; ready to roll
//convert the tf to an Eigen::Affine:
Eigen::Affine3f A_sensor_wrt_torso;
Eigen::Affine3d Affine_des_gripper;
Eigen::Vector3d xvec_des,yvec_des,zvec_des,origin_des;
geometry_msgs::PoseStamped rt_tool_pose;
A_sensor_wrt_torso = cwru_pcl_utils.transformTFToEigen(tf_sensor_frame_to_torso_frame);
Eigen::Vector3f plane_normal, major_axis, centroid;
Eigen::Matrix3d Rmat;
int rtn_val;
double plane_dist;
//send a command to plan a joint-space move to pre-defined pose:
rtn_val=arm_motion_commander.plan_move_to_pre_pose();
//send command to execute planned motion
rtn_val=arm_motion_commander.rt_arm_execute_planned_path();
while (ros::ok()) {
if (cwru_pcl_utils.got_selected_points()) {
ROS_INFO("transforming selected points");
cwru_pcl_utils.transform_selected_points_cloud(A_sensor_wrt_torso);
//fit a plane to these selected points:
cwru_pcl_utils.fit_xformed_selected_pts_to_plane(plane_normal, plane_dist);
ROS_INFO_STREAM(" normal: " << plane_normal.transpose() << "; dist = " << plane_dist);
major_axis= cwru_pcl_utils.get_major_axis();
centroid = cwru_pcl_utils.get_centroid();
cwru_pcl_utils.reset_got_selected_points(); // reset the selected-points trigger
//construct a goal affine pose:
for (int i=0;i<3;i++) {
origin_des[i] = centroid[i]; // convert to double precision
zvec_des[i] = -plane_normal[i]; //want tool z pointing OPPOSITE surface normal
xvec_des[i] = major_axis[i];
}
origin_des[2]+=0.02; //raise up 2cm
yvec_des = zvec_des.cross(xvec_des); //construct consistent right-hand triad
Rmat.col(0)= xvec_des;
Rmat.col(1)= yvec_des;
Rmat.col(2)= zvec_des;
Affine_des_gripper.linear()=Rmat;
Affine_des_gripper.translation()=origin_des;
cout<<"des origin: "<<Affine_des_gripper.translation().transpose()<<endl;
cout<<"orientation: "<<endl;
cout<<Rmat<<endl;
//convert des pose from Eigen::Affine to geometry_msgs::PoseStamped
rt_tool_pose.pose = arm_motion_commander.transformEigenAffine3dToPose(Affine_des_gripper);
//could fill out the header as well...
// send move plan request:
rtn_val=arm_motion_commander.rt_arm_plan_path_current_to_goal_pose(rt_tool_pose);
if (rtn_val == cwru_action::cwru_baxter_cart_moveResult::SUCCESS) {
//send command to execute planned motion
rtn_val=arm_motion_commander.rt_arm_execute_planned_path();
}
else {
ROS_WARN("Cartesian path to desired pose not achievable");
}
}
ros::Duration(0.5).sleep(); // sleep for half a second
ros::spinOnce();
}
return 0;
}
// Publish the state as an odom message on the topic odom_ekf. Alos well broadcast a transform.
void EkfNode::publishState(ros::Time now){
// Create an Odometry message to publish
nav_msgs::Odometry state_msg;
/**************************************************************************
* Populate Odometry message for ekf_map_
**************************************************************************/
// Populate timestamp, position frame, and velocity frame
state_msg.header.stamp = now;
state_msg.header.frame_id = map_frame_;
state_msg.child_frame_id = base_frame_;
// Populate the position and orientation
state_msg.pose.pose.position.x = ekf_map_.state_(0); // x
state_msg.pose.pose.position.y = ekf_map_.state_(1); // y
state_msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(ekf_map_.state_(2)); // theta
boost::array<double,36> temp;
// Populate the covariance matrix
state_msg.pose.covariance = temp;
// Populate the linear and angular velocities
state_msg.twist.twist.linear.x = ekf_map_.state_(3); // v
state_msg.twist.twist.angular.z = ekf_map_.state_(4); // omega
// Populate the covariance matrix
state_msg.twist.covariance = temp;
// Publish the message!
stateMapPub_.publish(state_msg);
// Print for debugging purposes
ROS_INFO_STREAM("\nstate = \n" << ekf_map_.state_);
ROS_INFO_STREAM("\ncovariance = \n" << ekf_map_.cov_);
/**************************************************************************
* Populate Odometry message for ekf_odom_
**************************************************************************/
// Populate timestamp, position frame, and velocity frame
state_msg.header.stamp = now;
state_msg.header.frame_id = odom_frame_;
state_msg.child_frame_id = base_frame_;
// Populate the position and orientation
state_msg.pose.pose.position.x = ekf_odom_.state_(0); // x
state_msg.pose.pose.position.y = ekf_odom_.state_(1); // y
state_msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(ekf_odom_.state_(2)); // theta
// Populate the covariance matrix
state_msg.pose.covariance = temp;
// Populate the linear and angular velocities
state_msg.twist.twist.linear.x = ekf_odom_.state_(3); // v
state_msg.twist.twist.angular.z = ekf_odom_.state_(4); // omega
// Populate the covariance matrix
state_msg.twist.covariance = temp;
// Publish the message!
stateOdomPub_.publish(state_msg);
/**************************************************************************
* Create and broadcast transforms for map->odom and odom->base_link
**************************************************************************/
static tf::TransformBroadcaster br;
// First create and broadcast odom->base_link since we already have that.
tf::Transform odom2base;
odom2base.setOrigin(tf::Vector3(ekf_odom_.state_(0), ekf_odom_.state_(1), 0.0));
tf::Quaternion q;
q.setRPY(0.0, 0.0, ekf_odom_.state_(2));
odom2base.setRotation(q);
br.sendTransform(tf::StampedTransform(odom2base, now, odom_frame_, base_frame_));
// Now create and broadcast map->odom. But, first we must calculate this from
// map->base_link and odom->base_link.
tf::Transform map2odom;
double x2 = ekf_odom_.state_(0);
double y2 = ekf_odom_.state_(1);
double theta2 = ekf_odom_.state_(2);
double x3 = ekf_map_.state_(0);
double y3 = ekf_map_.state_(1);
double theta3 = ekf_map_.state_(2);
double x1 = cos(theta3)*(-x2*cos(theta2)-y2*sin(theta2))
-sin(theta3)*( x2*sin(theta2)-y2*cos(theta2))
+x3;
double y1 = sin(theta3)*(-x2*cos(theta2)-y2*sin(theta2))
+cos(theta3)*( x2*sin(theta2)-y2*cos(theta2))
+y3;
double theta1 = theta3 - theta2;
map2odom.setOrigin(tf::Vector3(x1, y1, 0.0));
q.setRPY(0.0, 0.0, theta1);
map2odom.setRotation(q);
br.sendTransform(tf::StampedTransform(map2odom, now, map_frame_, odom_frame_));
}
// Processes the point cloud with OpenCV using the PCL cluster indices
void processClusters( const std::vector<pcl::PointIndices> cluster_indices,
// const sensor_msgs::PointCloud2ConstPtr& pointcloud_msg,
const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud_transformed,
const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud_filtered,
const pcl::PointCloud<pcl::PointXYZRGB>& cloud )
{
// -------------------------------------------------------------------------------------------------------
// Convert image
ROS_INFO_STREAM_NAMED("perception","Converting image to OpenCV format");
try
{
sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image);
// pcl::toROSMsg (*pointcloud_msg, *image_msg);
pcl::toROSMsg (*cloud_transformed, *image_msg);
cv_bridge::CvImagePtr input_bridge = cv_bridge::toCvCopy(image_msg, "rgb8");
full_input_image = input_bridge->image;
}
catch (cv_bridge::Exception& ex)
{
ROS_ERROR_STREAM_NAMED("perception","[calibrate] Failed to convert image");
return;
}
// -------------------------------------------------------------------------------------------------------
// Process Image
// Convert image to gray
cv::cvtColor( full_input_image, full_input_image_gray, CV_BGR2GRAY );
//cv::adaptiveThreshold( full_input_image, full_input_image_gray, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY,5,10);
// Blur image - reduce noise with a 3x3 kernel
cv::blur( full_input_image_gray, full_input_image_gray, cv::Size(3,3) );
ROS_INFO_STREAM_NAMED("perception","Finished coverting");
// -------------------------------------------------------------------------------------------------------
// Check OpenCV and PCL image height for errors
int image_width = cloud.width;
int image_height = cloud.height;
ROS_DEBUG_STREAM( "PCL Image height " << image_height << " -- width " << image_width << "\n");
int image_width_cv = full_input_image.size.p[1];
int image_height_cv = full_input_image.size.p[0];
ROS_DEBUG_STREAM( "OpenCV Image height " << image_height_cv << " -- width " << image_width_cv << "\n");
if( image_width != image_width_cv || image_height != image_height_cv )
{
ROS_ERROR_STREAM_NAMED("perception","PCL and OpenCV image heights/widths do not match!");
return;
}
// -------------------------------------------------------------------------------------------------------
// GUI Stuff
// First window
const char* opencv_window = "Source";
/*
cv::namedWindow( opencv_window, CV_WINDOW_AUTOSIZE );
cv::imshow( opencv_window, full_input_image_gray );
*/
// while(true) // use this when we want to tweak the image
{
output_image = full_input_image.clone();
// -------------------------------------------------------------------------------------------------------
// Start processing clusters
ROS_INFO_STREAM_NAMED("perception","Finding min/max in x/y axis");
int top_image_overlay_x = 0; // tracks were to copyTo the mini images
// for each cluster, see if it is a block
for(size_t c = 0; c < cluster_indices.size(); ++c)
{
ROS_INFO_STREAM_NAMED("perception","\n\n");
ROS_INFO_STREAM_NAMED("perception","On cluster " << c);
// find the outer dimensions of the cluster
double xmin = 0; double xmax = 0;
double ymin = 0; double ymax = 0;
// also remember each min & max's correponding other coordinate (not needed for z)
double xminy = 0; double xmaxy = 0;
double yminx = 0; double ymaxx = 0;
// also remember their corresponding indice
int xmini = 0; int xmaxi = 0;
int ymini = 0; int ymaxi = 0;
// loop through and find all min/max of x/y
for(size_t i = 0; i < cluster_indices[c].indices.size(); i++)
{
int j = cluster_indices[c].indices[i];
// Get RGB from point cloud
pcl::PointXYZRGB p = cloud_transformed->points[j];
double x = p.x;
double y = p.y;
if(i == 0) // initial values
{
xmin = xmax = x;
ymin = ymax = y;
xminy = xmaxy = y;
yminx = ymaxx = x;
xmini = xmaxi = ymini = ymaxi = j; // record the indice corresponding to the min/max
}
else
{
if( x < xmin )
{
xmin = x;
xminy = y;
xmini = j;
}
if( x > xmax )
{
xmax = x;
xmaxy = y;
xmaxi = j;
}
if( y < ymin )
{
ymin = y;
yminx = x;
ymini = j;
}
if( y > ymax )
{
ymax = y;
ymaxx = x;
ymaxi = j;
}
}
}
ROS_DEBUG_STREAM_NAMED("perception","Cluster size - xmin: " << xmin << " xmax: " << xmax << " ymin: " << ymin << " ymax: " << ymax);
ROS_DEBUG_STREAM_NAMED("perception","Cluster size - xmini: " << xmini << " xmaxi: " << xmaxi << " ymini: " << ymini << " ymaxi: " << ymaxi);
// ---------------------------------------------------------------------------------------------
// Check if these dimensions make sense for the block size specified
double xside = xmax-xmin;
double yside = ymax-ymin;
const double tol = 0.01; // 1 cm error tolerance
// In order to be part of the block, xside and yside must be between
// blocksize and blocksize*sqrt(2)
if(xside > block_size-tol &&
xside < block_size*sqrt(2)+tol &&
yside > block_size-tol &&
yside < block_size*sqrt(2)+tol )
{
// -------------------------------------------------------------------------------------------------------
// Get the four farthest corners of the block - use OpenCV only on the region identified by PCL
// Get the pixel coordinates of the xmax and ymax indicies
int px_xmax = 0; int py_xmax = 0;
int px_ymax = 0; int py_ymax = 0;
getXYCoordinates( xmaxi, image_height, image_width, px_xmax, py_xmax);
getXYCoordinates( ymaxi, image_height, image_width, px_ymax, py_ymax);
// Get the pixel coordinates of the xmin and ymin indicies
int px_xmin = 0; int py_xmin = 0;
int px_ymin = 0; int py_ymin = 0;
getXYCoordinates( xmini, image_height, image_width, px_xmin, py_xmin);
getXYCoordinates( ymini, image_height, image_width, px_ymin, py_ymin);
ROS_DEBUG_STREAM_NAMED("perception","px_xmin " << px_xmin << " px_xmax: " << px_xmax << " py_ymin: " << py_ymin << " py_ymax: " << py_ymax );
// -------------------------------------------------------------------------------------------------------
// Change the frame of reference from the robot to the camera
// Create an array of all the x value options
const int x_values_a[] = {px_xmax, px_ymax, px_xmin, px_ymin};
const int y_values_a[] = {py_xmax, py_ymax, py_xmin, py_ymin};
// Turn it into a vector
std::vector<int> x_values (x_values_a, x_values_a + sizeof(x_values_a) / sizeof(x_values_a[0]));
std::vector<int> y_values (y_values_a, y_values_a + sizeof(y_values_a) / sizeof(y_values_a[0]));
// Find the min
int x1 = *std::min_element(x_values.begin(), x_values.end());
int y1 = *std::min_element(y_values.begin(), y_values.end());
// Find the max
int x2 = *std::max_element(x_values.begin(), x_values.end());
int y2 = *std::max_element(y_values.begin(), y_values.end());
ROS_DEBUG_STREAM_NAMED("perception","x1: " << x1 << " y1: " << y1 << " x2: " << x2 << " y2: " << y2);
// -------------------------------------------------------------------------------------------------------
// Expand the ROI by a fudge factor, if possible
const int FUDGE_FACTOR = 5; // pixels
if( x1 > FUDGE_FACTOR)
x1 -= FUDGE_FACTOR;
if( y1 > FUDGE_FACTOR )
y1 -= FUDGE_FACTOR;
if( x2 < image_width - FUDGE_FACTOR )
x2 += FUDGE_FACTOR;
if( y2 < image_height - FUDGE_FACTOR )
y2 += FUDGE_FACTOR;
ROS_DEBUG_STREAM_NAMED("perception","After Fudge Factor - x1: " << x1 << " y1: " << y1 << " x2: " << x2 << " y2: " << y2);
// -------------------------------------------------------------------------------------------------------
// Create ROI parameters
// (x1,y1)----------------------
// | |
// | ROI |
// | |
// |_____________________(x2,y2)|
// Create Region of Interest
int roi_width = x2 - x1;
int roi_height = y2 - y1;
cv::Rect region_of_interest = cv::Rect( x1, y1, roi_width, roi_height );
ROS_DEBUG_STREAM_NAMED("perception","ROI: x " << x1 << " -- y " << y1 << " -- height " << roi_height << " -- width " << roi_width );
// -------------------------------------------------------------------------------------------------------
// Find paramters of the block in pixel coordiantes
int block_center_x = x1 + 0.5*roi_width;
int block_center_y = y1 + 0.5*roi_height;
int block_center_z = block_size / 2; // TODO: make this better
const cv::Point block_center = cv::Point( block_center_x, block_center_y );
// -------------------------------------------------------------------------------------------------------
// Create a sub image of just the block
cv::Point a1 = cv::Point(x1, y1);
cv::Point a2 = cv::Point(x2, y2);
cv::rectangle( output_image, a1, a2, cv::Scalar(0, 255, 255), 1, 8);
// Crop image (doesn't actually copy the data)
cropped_image = full_input_image_gray(region_of_interest);
// -------------------------------------------------------------------------------------------------------
// Detect edges using canny
ROS_INFO_STREAM_NAMED("perception","Detecting edges using canny");
// Find edges
cv::Mat canny_output;
cv::Canny( cropped_image, canny_output, canny_threshold, canny_threshold*2, 3 );
// Get mini window stats
const int mini_width = canny_output.size.p[1];
const int mini_height = canny_output.size.p[0];
const cv::Size mini_size = canny_output.size();
const cv::Point mini_center = cv::Point( mini_width/2, mini_height/2 );
// Find contours
vector<vector<cv::Point> > contours;
vector<cv::Vec4i> hierarchy;
cv::findContours( canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
ROS_INFO_STREAM_NAMED("perception","Contours");
// Draw contours
cv::Mat drawing = cv::Mat::zeros( mini_size, CV_8UC3 );
ROS_INFO_STREAM_NAMED("perception","Drawing contours");
// Find the largest contour for getting the angle
double max_contour_length = 0;
int max_contour_length_i;
for( size_t i = 0; i< contours.size(); i++ )
{
double contour_length = cv::arcLength( contours[i], false );
if( contour_length > max_contour_length )
{
max_contour_length = contour_length;
max_contour_length_i = i;
}
//ROS_DEBUG_STREAM_NAMED("perception","Contour length = " << contour_length << " of index " << max_contour_length_i);
cv::Scalar color = cv::Scalar( (30 + i*10) % 255, (30 + i*10) % 255, (30 + i*10) % 255);
cv::drawContours( drawing, contours, (int)i, color, 1, 8, hierarchy, 0, cv::Point() );
//drawContours( image, contours, contourIdx, color, thickness, lineType, hierarchy, maxLevel, offset )
}
// -------------------------------------------------------------------------------------------------------
// Copy largest contour to main image
cv::Scalar color = cv::Scalar( 0, 255, 0 );
cv::drawContours( output_image, contours, (int)max_contour_length_i, color, 1, 8, hierarchy, 0, a1 );
//drawContours( image, contours, contourIdx, color, thickness, lineType, hierarchy, maxLevel, offset )
// -------------------------------------------------------------------------------------------------------
// Copy largest contour to seperate image
cv::Mat hough_input = cv::Mat::zeros( mini_size, CV_8UC1 );
cv::Mat hough_input_color;
cv::Scalar hough_color = cv::Scalar( 200 );
cv::drawContours( hough_input, contours, (int)max_contour_length_i, hough_color, 1, 8, hierarchy, 0 );
cv::cvtColor(hough_input, hough_input_color, CV_GRAY2BGR);
// -------------------------------------------------------------------------------------------------------
// Hough Transform
cv::Mat hough_drawing = cv::Mat::zeros( mini_size, CV_8UC3 );
std::vector<cv::Vec4i> lines;
ROS_DEBUG_STREAM_NAMED("perception","hough_rho " << hough_rho << " hough_theta " << hough_theta <<
" theta_converted " << (1/hough_theta)*CV_PI/180 << " hough_threshold " <<
hough_threshold << " hough_minLineLength " << hough_minLineLength <<
" hough_maxLineGap " << hough_maxLineGap );
cv::HoughLinesP(hough_input, lines, hough_rho, (1/hough_theta)*CV_PI/180, hough_threshold, hough_minLineLength, hough_maxLineGap);
ROS_WARN_STREAM_NAMED("perception","Found " << lines.size() << " lines");
std::vector<double> line_angles;
// Copy detected lines to the drawing image
for( size_t i = 0; i < lines.size(); i++ )
{
cv::Vec4i line = lines[i];
cv::line( hough_drawing, cv::Point(line[0], line[1]), cv::Point(line[2], line[3]),
cv::Scalar(255,255,255), 1, CV_AA);
// Error check
if(line[3] - line[1] == 0 && line[2] - line[0] == 0)
{
ROS_ERROR_STREAM_NAMED("perception","Line is actually two points at the origin, unable to calculate. TODO: handle better?");
continue;
}
// Find angle
double line_angle = atan2(line[3] - line[1], line[2] - line[0]); //in radian, degrees: * 180.0 / CV_PI;
// Reverse angle direction if negative
if( line_angle < 0 )
{
line_angle += CV_PI;
}
line_angles.push_back(line_angle);
ROS_DEBUG_STREAM_NAMED("perception","Hough Line angle: " << line_angle * 180.0 / CV_PI;);
}
double block_angle = 0; // the overall result of the block's angle
// Everything is based on the first angle
if( line_angles.size() == 0 ) // make sure we have at least 1 angle
{
ROS_ERROR_STREAM_NAMED("perception","No lines were found for this cluster, unable to calculate block angle");
}
else
{
calculateBlockAngle( line_angles, block_angle );
}
// -------------------------------------------------------------------------------------------------------
// Draw chosen angle
ROS_INFO_STREAM_NAMED("perception","Using block angle " << block_angle*180.0/CV_PI);
// Draw chosen angle on mini image
cv::Mat angle_drawing = cv::Mat::zeros( mini_size, CV_8UC3 );
int line_length = 0.5*double(mini_width); // have the line go 1/4 across the screen
int new_x = mini_center.x + line_length*cos( block_angle );
int new_y = mini_center.y + line_length*sin( block_angle );
ROS_INFO_STREAM("Origin (" << mini_center.x << "," << mini_center.y << ") New (" << new_x << "," << new_y <<
") length " << line_length << " angle " << block_angle <<
" mini width " << mini_width << " mini height " << mini_height);
cv::Point angle_point = cv::Point(new_x, new_y);
cv::line( angle_drawing, mini_center, angle_point, cv::Scalar(255,255,255), 1, CV_AA);
// Draw chosen angle on contours image
cv::line( hough_drawing, mini_center, angle_point, cv::Scalar(255,0, 255), 1, CV_AA);
// Draw chosen angle on main image
line_length = 0.75 * double(mini_width); // have the line go 1/2 across the box
new_x = block_center_x + line_length*cos( block_angle );
new_y = block_center_y + line_length*sin( block_angle );
ROS_INFO_STREAM_NAMED("perception",block_center_x << ", " << block_center_y << ", " << new_x << ", " << new_y);
angle_point = cv::Point(new_x, new_y);
cv::line( output_image, block_center, angle_point, cv::Scalar(255,0,255), 2, CV_AA);
// -------------------------------------------------------------------------------------------------------
// Get world coordinates
// Find the block's center point
double world_x1 = xmin+(xside)/2.0;
double world_y1 = ymin+(yside)/2.0;
double world_z1 = table_height + block_size / 2;
// Convert pixel coordiantes back to world coordinates
double world_x2 = cloud_transformed->at(new_x, new_y).x;
double world_y2 = cloud_transformed->at(new_x, new_y).y;
double world_z2 = world_z1; // is this even necessary?
// Get angle from two world coordinates...
double world_theta = abs( atan2(world_y2 - world_y1, world_x2 - world_x1) );
// Attempt to make all angles point in same direction
makeAnglesUniform( world_theta );
// -------------------------------------------------------------------------------------------------------
// GUI Stuff
// Copy the cluster image to the main image in the top left corner
if( top_image_overlay_x + mini_width < image_width )
{
const int common_height = 42;
cv::Rect small_roi_row0 = cv::Rect(top_image_overlay_x, common_height*0, mini_width, mini_height);
cv::Rect small_roi_row1 = cv::Rect(top_image_overlay_x, common_height*1, mini_width, mini_height);
cv::Rect small_roi_row2 = cv::Rect(top_image_overlay_x, common_height*2, mini_width, mini_height);
cv::Rect small_roi_row3 = cv::Rect(top_image_overlay_x, common_height*3, mini_width, mini_height);
drawing.copyTo( output_image(small_roi_row0) );
hough_input_color.copyTo( output_image(small_roi_row1) );
hough_drawing.copyTo( output_image(small_roi_row2) );
angle_drawing.copyTo( output_image(small_roi_row3) );
top_image_overlay_x += mini_width;
}
// figure out the position and the orientation of the block
//double angle = atan(block_size/((xside+yside)/2));
//double angle = atan( (xmaxy - xminy) / (xmax - xmin ) );
// Then add it to our set
//addBlock( xmin+(xside)/2.0, ymin+(yside)/2.0, zmax - block_size/2.0, angle);
//ROS_INFO_STREAM_NAMED("perception","FOUND -> xside: " << xside << " yside: " << yside << " angle: " << block_angle);
addBlock( world_x1, world_y1, world_z1, world_theta );
//addBlock( block_center_x, block_center_y, block_center_z, block_angle);
}
else
{
void OPTCalibration::perform()
{
const ViewMap & view_map = view_map_vec_.back();
if (initialization_)
{
if (not tree_initialized_ and not view_map.empty()) // Set /world
{
ViewMap::const_iterator it = view_map.begin();
while (it != view_map.end() and it->first->type() == TreeNode::DEPTH)
++it;
if (it == view_map.end())
{
view_map_vec_.resize(view_map_vec_.size() - 1);
return;
}
TreeNode::Ptr tree_node = it->first;
tree_node->sensor()->setParent(world_);
tree_node->setLevel(0);
ROS_INFO_STREAM(tree_node->sensor()->frameId() << " added to the tree.");
tree_initialized_ = true;
}
if (view_map.empty())
{
view_map_vec_.resize(view_map_vec_.size() - 1); // Remove data
}
else if (view_map.size() >= 2) // At least 2 cameras
{
int min_level = TreeNode::MAX_LEVEL;
TreeNode::Ptr min_sensor_node;
for (ViewMap::const_iterator it = view_map.begin(); it != view_map.end(); ++it)
{
TreeNode::Ptr tree_node = it->first;
if (tree_node->level() < min_level)
{
min_level = tree_node->level();
min_sensor_node = tree_node;
}
}
if (min_level < TreeNode::MAX_LEVEL) // At least one already in tree
{
for (ViewMap::const_iterator it = view_map.begin(); it != view_map.end(); ++it)
{
TreeNode::Ptr tree_node = it->first;
if (tree_node != min_sensor_node)
{
const CheckerboardView::Ptr & view = it->second;
cb::PlanarObject::Ptr planar_object = view->object;
cb::Point3 center = view->center;
if (tree_node->type() == TreeNode::INTENSITY)
{
cb::Scalar error = std::abs(planar_object->plane().normal().dot(cb::Vector3::UnitZ())) * center.squaredNorm();
if (tree_node->level() > min_level and error < tree_node->minError())
{
CheckerboardView::Ptr min_checkerboard_view = view_map.at(min_sensor_node);
cb::PlanarObject::Ptr min_planar_object = min_checkerboard_view->object;
if (tree_node->level() == TreeNode::MAX_LEVEL)
ROS_INFO_STREAM(tree_node->sensor()->frameId() << " added to the tree.");
tree_node->setMinError(error);
tree_node->setLevel(min_level + 1);
tree_node->sensor()->setParent(min_sensor_node->sensor());
tree_node->sensor()->setPose(min_planar_object->pose() * planar_object->pose().inverse());
}
}
}
}
}
initialization_ = (view_map_vec_.size() < OPTIMIZATION_COUNT);
for (int i = 0; not initialization_ and i < node_vec_.size(); ++i)
{
TreeNode::Ptr & tree_node = node_vec_[i];
if (tree_node->type() == TreeNode::INTENSITY and tree_node->level() == TreeNode::MAX_LEVEL)
initialization_ = true;
}
if (not initialization_)
ROS_INFO("All cameras added to the tree. Now calibrate the global reference frame and save!");
}
}
else
{
if (view_map.empty())
{
view_map_vec_.resize(view_map_vec_.size() - 1); // Remove data
}
else if (view_map.size() >= 2) // At least 2 cameras
{
if (last_optimization_ == 0)
{
optimize();
last_optimization_ = OPTIMIZATION_COUNT;
}
else
last_optimization_--;
}
}
}
void GlobalPlanner::Display()
{
m_lastDisplay = ros::Time::now();
ROS_INFO_STREAM("Robot Status:");
for (std::map<int, Robot_Ptr>::iterator it = m_robots.begin(); it != m_robots.end(); ++it)
{
ROS_INFO_STREAM(it->second->ToString());
}
geometry_msgs::PoseArray availPoseArray;
geometry_msgs::PoseArray finPoseArray;
availPoseArray.header.stamp = ros::Time::now();
availPoseArray.header.frame_id = "/map";
finPoseArray.header = availPoseArray.header;
std::map<int, Waypoint_Ptr> wps = m_tm.GetWaypoints();
if (wps.size() > 0)
{
ROS_INFO_STREAM("Waypoint Status:");
}
for (std::map<int, Waypoint_Ptr>::iterator it = wps.begin(); it != wps.end(); ++it)
{
if (m_tm.IsWaypoint(it->first))
{
ROS_INFO_STREAM(it->second->ToString());
if (it->second->GetStatus() == TaskResult::SUCCESS)
finPoseArray.poses.push_back(it->second->GetPose());
else
availPoseArray.poses.push_back(it->second->GetPose());
}
}
m_waypointPoseArrayAvailPub.publish(availPoseArray);
m_waypointPoseArrayFinPub.publish(finPoseArray);
finPoseArray.poses.clear();
availPoseArray.poses.clear();
std::map<int, Waypoint_Ptr> goals = m_tm.GetGoals();
if (goals.size() > 0)
{
ROS_INFO_STREAM("Goal Status");
}
for (std::map<int, Waypoint_Ptr>::iterator it = goals.begin(); it != goals.end(); ++it)
{
ROS_INFO_STREAM(it->second->ToString());
if (it->second->GetStatus() == TaskResult::SUCCESS)
finPoseArray.poses.push_back(it->second->GetPose());
else
availPoseArray.poses.push_back(it->second->GetPose());
}
m_goalPoseArrayAvailPub.publish(availPoseArray);
m_goalPoseArrayFinPub.publish(finPoseArray);
finPoseArray.poses.clear();
availPoseArray.poses.clear();
std::map<int, Dump_Ptr> dumps = m_tm.GetDumps();
if (dumps.size() > 0)
{
ROS_INFO_STREAM("Dump Status");
}
for (std::map<int, Dump_Ptr>::iterator it = dumps.begin(); it != dumps.end(); ++it)
{
ROS_INFO_STREAM(it->second->ToString());
if (it->second->GetStatus() == TaskResult::SUCCESS)
{
finPoseArray.poses.push_back(it->second->GetPose1());
finPoseArray.poses.push_back(it->second->GetPose2());
}
else
{
availPoseArray.poses.push_back(it->second->GetPose1());
availPoseArray.poses.push_back(it->second->GetPose2());
}
}
m_dumpPoseArrayAvailPub.publish(availPoseArray);
m_dumpPoseArrayFinPub.publish(finPoseArray);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "mtconnect_state_machine");
ros::NodeHandle ph("~");
ros::NodeHandle nh;
std::string task_desc;
std::map<std::string, trajectory_msgs::JointTrajectoryPtr> joint_paths;
JointTractoryClientPtr joint_traj_client_ptr;
control_msgs::FollowJointTrajectoryGoal joint_traj_goal;
ros::ServiceClient trajectory_filter_client;
arm_navigation_msgs::FilterJointTrajectoryWithConstraints trajectory_filter_;
// Load parameters
ROS_INFO_STREAM("Loading parameters");
if (!ph.getParam(PARAM_TASK_DESCRIPTION, task_desc))
{
ROS_ERROR("Failed to load task description parameter");
return false;
}
std::map<std::string, boost::shared_ptr<mtconnect::JointPoint> > temp_pts;
if (!mtconnect_state_machine::parseTaskXml(task_desc, joint_paths, temp_pts))
{
ROS_ERROR("Failed to parse xml");
return false;
}
ROS_INFO_STREAM("Loaded " << joint_paths.size() << " paths");
// Load actions/clients
joint_traj_client_ptr = JointTractoryClientPtr(new JointTractoryClient(DEFAULT_JOINT_TRAJ_ACTION, true));
trajectory_filter_client = nh.serviceClient<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>(
DEFAULT_TRAJECTORY_FILTER_SERVICE);
ROS_INFO_STREAM("Waiting for joint trajectory filter and action server");
joint_traj_client_ptr->waitForServer(ros::Duration(0));
trajectory_filter_client.waitForExistence();
ROS_INFO_STREAM("Starting loop through trajectory");
while (ros::ok())
{
for(JTPMapItr itr = joint_paths.begin(); itr != joint_paths.end(); itr++)
{
ROS_INFO_STREAM("Loading " << itr->first << " path");
joint_traj_goal.trajectory = *itr->second;
ROS_INFO_STREAM("Filtering a joint trajectory with " << joint_traj_goal.trajectory.points.size() << " points");
trajectory_filter_.request.trajectory = joint_traj_goal.trajectory;
if (trajectory_filter_client.call(trajectory_filter_))
{
if (trajectory_filter_.response.error_code.val == trajectory_filter_.response.error_code.SUCCESS)
{
ROS_INFO_STREAM("======================== MOVING ROBOT ========================");
ROS_INFO("Trajectory successfully filtered...sending goal");
joint_traj_goal.trajectory = trajectory_filter_.response.trajectory;
ROS_INFO_STREAM("Sending a joint trajectory with " << joint_traj_goal.trajectory.points.size() << " points");
//joint_traj_client_ptr->sendGoal(joint_traj_goal);
//joint_traj_client_ptr->waitForResult(ros::Duration(120));
joint_traj_client_ptr->sendGoalAndWait(joint_traj_goal, ros::Duration(120), ros::Duration(120));
}
}
else
{
ROS_ERROR("Failed to call filter trajectory");
}
}
}
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "igv_TestNode");
ros::start();
ros::Rate loop_rate(10);
uint8_t getTxLeft[12] = {0xa5, 0x3e, 0x02, 0x07, 0x00, 0x01, 0x19, 0xb6,
0xff, 0x00, 0x03, 0xff};
uint8_t enableBridgeLeft[12] = {0xa5, 0x3e, 0x02, 0x01, 0x00, 0x01, 0xab, 0x16,
0x00, 0x00, 0x00, 0x00};
uint8_t disableBridgeLeft[12] = {0xa5, 0x3e, 0x02, 0x01, 0x00, 0x01, 0xab, 0x16,
0x01, 0x00, 0x33, 0x31};
uint8_t setVelocityLeft[14] = {0xa5, 0x3e, 0x02, 0x45, 0x00, 0x02, 0x5a, 0x18,
0xb5, 0x01, 0x00, 0x00, 0x7a, 0xa4};
//========================================================================
uint8_t getTxRight[12] = {0xa5, 0x3f, 0x02, 0x07, 0x00, 0x01, 0xb3, 0xe7,
0xff, 0x00, 0x03, 0xff};
uint8_t enableBridgeRight[12] = {0xa5, 0x3f, 0x02, 0x01, 0x00, 0x01, 0x01, 0x47,
0x00, 0x00, 0x00, 0x00};
uint8_t disableBridgeRight[12] = {0xa5, 0x3f, 0x02, 0x01, 0x00, 0x01, 0x01, 0x47,
0x01, 0x00, 0x33, 0x31};
uint8_t setVelocityRight[14] = {0xa5, 0x3f, 0x02, 0x45, 0x00, 0x02, 0x96, 0x2b,
0xb5, 0x01, 0x00, 0x00, 0x7a, 0xa4};
//TODO Implement setting timeout to main program.
serial::Serial LeftPort;
LeftPort.setPort(LEFT_PORT);
LeftPort.setBaudrate(115200);
serial::Timeout to = serial::Timeout::simpleTimeout(1000);
LeftPort.setTimeout(to);
LeftPort.open();
ROS_INFO_STREAM("Left Port is opened.");
// serial::Serial RightPort;
// RightPort.setPort(RIGHT_PORT);
// RightPort.setBaudrate(115200);
// serial::Timeout to = serial::Timeout::simpleTimeout(1000);
// RightPort.setTimeout(to);
// RightPort.open();
// ROS_INFO_STREAM("Right Port is opened.");
if(ros::ok())
{
ros::spinOnce();
LeftPort.write(getTxLeft, 12);
ROS_INFO_STREAM("Tx Access Got.");
LeftPort.write(setVelocityLeft, 14);
ROS_INFO_STREAM("Velocity Set.");
LeftPort.write(enableBridgeLeft, 12);
ROS_INFO_STREAM("Bridge Enabled.");
ros::Duration(2).sleep();
LeftPort.write(disableBridgeLeft, 12);
ROS_INFO_STREAM("Bridge Disabled.");
// RightPort.write(getTxRight, 12);
// ROS_INFO_STREAM("Tx Access Got.");
// RightPort.write(setVelocityRight, 14);
// ROS_INFO_STREAM("Velocity Set.");
// RightPort.write(enableBridgeRight, 12);
// ROS_INFO_STREAM("Bridge Enabled.");
// ros::Duration(2).sleep();
// RightPort.write(disableBridgeRight, 12);
// ROS_INFO_STREAM("Bridge Disabled.");
loop_rate.sleep();
}
ros::shutdown();
return 0;
}
bool PlaceObject::IncrementalMoveTCP(const double threshold, const double eef_step, const double max_distance, const double timeout)
{
ROS_INFO("PlaceObject: Start incremental move in TCP frame.");
move_group_->setMaxVelocityScalingFactor(0.001);
ros::Time start = ros::Time::now();
bool collision = false;
geometry_msgs::PoseStamped current_pose = move_group_->getCurrentPose();
geometry_msgs::PoseStamped goal_pose;
goal_pose.header = current_pose.header;
goal_pose.pose = target_pose_;
goal_pose.pose.position.z -= 0.003;
std::vector<geometry_msgs::Pose> waypoints;
waypoints.push_back(current_pose.pose);
waypoints.push_back(goal_pose.pose);
robot_state::RobotState rs = *move_group_->getCurrentState();
move_group_->setStartState(rs);
// Plan trajectory.
moveit_msgs::RobotTrajectory trajectory;
moveit_msgs::MoveItErrorCodes error_code;
double path_fraction = move_group_->computeCartesianPath(waypoints, eef_step, 0.0, trajectory, false, &error_code);
robot_trajectory::RobotTrajectory robot_trajectory(rs.getRobotModel(), move_group_->getName());
robot_trajectory.setRobotTrajectoryMsg(rs, trajectory);
trajectory_processing::IterativeParabolicTimeParameterization iptp;
iptp.computeTimeStamps(robot_trajectory, 1.0);
robot_trajectory.getRobotTrajectoryMsg(trajectory);
moveit::planning_interface::MoveGroup::Plan my_plan;
moveit_msgs::RobotState robot_state_msg;
robot_state::robotStateToRobotStateMsg(*move_group_->getCurrentState(), robot_state_msg);
my_plan.trajectory_ = trajectory;
my_plan.start_state_ = robot_state_msg;
// removing points with velocity zero
my_plan.trajectory_.joint_trajectory.points.erase(
std::remove_if(my_plan.trajectory_.joint_trajectory.points.begin() + 1, my_plan.trajectory_.joint_trajectory.points.end(), [](const trajectory_msgs::JointTrajectoryPoint & p)
{ return p.time_from_start.toSec() == 0;}), my_plan.trajectory_.joint_trajectory.points.end());
// start sampling
if (!gripper_interface_->ClientHandCommand("sampling")) {
ROS_WARN("Execute Grasping: sampling failed");
return false;
}
int8_t collision_check = gripper_interface_->GetHandState().collision;
printf("pre collision result: %d \n", collision_check);
// if(collision_check) {
// ROS_WARN("collision is already detected!");
// return true;
// }
// ros::Duration(0.5).sleep();
// removing points with velocity zero
my_plan.trajectory_.joint_trajectory.points.erase(
std::remove_if(my_plan.trajectory_.joint_trajectory.points.begin() + 1, my_plan.trajectory_.joint_trajectory.points.end(), [](const trajectory_msgs::JointTrajectoryPoint & p)
{ return p.time_from_start.toSec() == 0;}),
my_plan.trajectory_.joint_trajectory.points.end());
move_group_->execute(my_plan);
ROS_INFO_STREAM("robot state: effort: " << move_group_->getCurrentState()->hasVelocities());
while(move_group_->getCurrentState()->hasVelocities() ){
ROS_INFO_STREAM("robot state: effort: " << move_group_->getCurrentState()->hasEffort());
collision_check = gripper_interface_->GetHandState().collision;
printf("%d \n", collision_check);
if ((ros::Time::now() - start).toSec() > timeout) {
ROS_WARN("Incremental movement: Timeout.");
// move_group_->stop();
gripper_interface_->ClientHandCommand("c-open");
move_group_->stop();
return true;
}
ros::spinOnce();
ros::Duration(0.1).sleep();
}
gripper_interface_->ClientHandCommand("c-open");
move_group_->stop();
ROS_WARN("collision was detected!");
return true;
}
bool parseimumsg(uint8 * dataPacket, sensor_msgs::Imu * imu_msg)
{
float m1=0.0,m2=0.0,m3=0.0;
//
if(dataPacket[0] == 0x55 && dataPacket[1] == 0xaa)
{
//检验校验和
unsigned int sum = 0;
for(int i = 0; i < PKGSIZE - 1; i++)
{
sum += dataPacket[i];
}
//std::cout<< sum << " " << imumsg[14] << std::endl;
//
if( sum % 256 == dataPacket[PKGSIZE - 1])
{
//解算加速度X,Y,Z
int16 AccX = Byte16(int16, dataPacket[2], dataPacket[3]);
int16 AccY = Byte16(int16, dataPacket[4], dataPacket[5]);
int16 AccZ = Byte16(int16, dataPacket[6], dataPacket[7]);
float Accscalar = MPU9250A_4g * GRAVITY_MSS;
float AccTrueX = AccX * Accscalar;
float AccTrueY = AccY * Accscalar;
float AccTrueZ = AccZ * Accscalar;
//解算角速度X,Y,Z
int16 GyrX = Byte16(int16, dataPacket[8], dataPacket[9]);
int16 GyrY = Byte16(int16, dataPacket[10], dataPacket[11]);
int16 GyrZ = Byte16(int16, dataPacket[12], dataPacket[13]);
float Gyrscalar = UserGyrResolution / 57.3;
float GyrTrueX = GyrX * Gyrscalar;
float GyrTrueY = GyrY * Gyrscalar;
float GyrTrueZ = GyrZ * Gyrscalar;
//解算Mag X,Y,Z
int16 MagX = Byte16(int16, dataPacket[15], dataPacket[14]);
int16 MagY = Byte16(int16, dataPacket[17], dataPacket[16]);
int16 MagZ = Byte16(int16, dataPacket[19], dataPacket[18]);
float Magscalar = MPU9250M_4800uT*Tesla2Gauss;
float magx=MagX*Magscalar;
float magy=MagY*Magscalar;
float magz=MagZ*Magscalar;
m1=magx+mag_offset[0]; m2=magy+mag_offset[1]; m3=magz+mag_offset[2];
MagTrueY=mag_sca[0]*m1+mag_sca[1]*m2+mag_sca[2]*m3;
MagTrueX=mag_sca[3]*m1+mag_sca[4]*m2+mag_sca[5]*m3;
MagTrueZ=-1.0*(mag_sca[6]*m1+mag_sca[7]*m2+mag_sca[8]*m3);
newDataMag =(bool) dataPacket[20];
//发送Imu数据
imu_msg->header.stamp = ros::Time::now();
imu_msg->header.frame_id = "imu";
imu_msg->angular_velocity.x = GyrTrueX;
imu_msg->angular_velocity.y = GyrTrueY;
imu_msg->angular_velocity.z = GyrTrueZ;
imu_msg->linear_acceleration.x = AccTrueX;
imu_msg->linear_acceleration.y = AccTrueY;
imu_msg->linear_acceleration.z = AccTrueZ;
return true;
}
else
{
ROS_INFO_STREAM("Checksum Error");
return false;
}
}
else
{
ROS_INFO_STREAM("Header Error");
return false;
}
}
void Action::setTolerance(const double value)
{
move_group_->setGoalTolerance(value);
if (verbose_)
ROS_INFO_STREAM("The GoalPositionTolerance = " << move_group_->getGoalPositionTolerance());
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "collision_proximity_test");
ros::NodeHandle nh;
std::string robot_description_name = nh.resolveName("robot_description", true);
srand ( time(NULL) ); // initialize random seed
ros::service::waitForService(ARM_QUERY_NAME);
ros::ServiceClient query_client = nh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>(ARM_QUERY_NAME);
kinematics_msgs::GetKinematicSolverInfo::Request request;
kinematics_msgs::GetKinematicSolverInfo::Response response;
query_client.call(request, response);
int num_joints;
std::vector<double> min_limits, max_limits;
num_joints = (int) response.kinematic_solver_info.joint_names.size();
std::vector<std::string> joint_state_names = response.kinematic_solver_info.joint_names;
std::map<std::string, double> joint_state_map;
for(int i=0; i< num_joints; i++)
{
joint_state_map[joint_state_names[i]] = 0.0;
min_limits.push_back(response.kinematic_solver_info.limits[i].min_position);
max_limits.push_back(response.kinematic_solver_info.limits[i].max_position);
}
std::vector<std::vector<double> > valid_joint_states;
valid_joint_states.resize(TEST_NUM);
for(unsigned int i = 0; i < TEST_NUM; i++) {
std::vector<double> jv(7,0.0);
for(unsigned int j=0; j < min_limits.size(); j++)
{
jv[j] = gen_rand(std::max(min_limits[j],-M_PI),std::min(max_limits[j],M_PI));
}
valid_joint_states[i] = jv;
}
collision_proximity::CollisionProximitySpace* cps = new collision_proximity::CollisionProximitySpace(robot_description_name);
ros::ServiceClient planning_scene_client = nh.serviceClient<arm_navigation_msgs::GetPlanningScene>(planning_scene_name, true);
ros::service::waitForService(planning_scene_name);
arm_navigation_msgs::GetPlanningScene::Request ps_req;
arm_navigation_msgs::GetPlanningScene::Response ps_res;
arm_navigation_msgs::CollisionObject obj1;
obj1.header.stamp = ros::Time::now();
obj1.header.frame_id = "odom_combined";
obj1.id = "obj1";
obj1.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
obj1.shapes.resize(1);
obj1.shapes[0].type = arm_navigation_msgs::Shape::BOX;
obj1.shapes[0].dimensions.resize(3);
obj1.shapes[0].dimensions[0] = .1;
obj1.shapes[0].dimensions[1] = .1;
obj1.shapes[0].dimensions[2] = .75;
obj1.poses.resize(1);
obj1.poses[0].position.x = .6;
obj1.poses[0].position.y = -.6;
obj1.poses[0].position.z = .375;
obj1.poses[0].orientation.w = 1.0;
arm_navigation_msgs::AttachedCollisionObject att_obj;
att_obj.object = obj1;
att_obj.object.header.stamp = ros::Time::now();
att_obj.object.header.frame_id = "r_gripper_palm_link";
att_obj.link_name = "r_gripper_palm_link";
att_obj.touch_links.push_back("r_gripper_palm_link");
att_obj.touch_links.push_back("r_gripper_r_finger_link");
att_obj.touch_links.push_back("r_gripper_l_finger_link");
att_obj.touch_links.push_back("r_gripper_r_finger_tip_link");
att_obj.touch_links.push_back("r_gripper_l_finger_tip_link");
att_obj.touch_links.push_back("r_wrist_roll_link");
att_obj.touch_links.push_back("r_wrist_flex_link");
att_obj.touch_links.push_back("r_forearm_link");
att_obj.touch_links.push_back("r_gripper_motor_accelerometer_link");
att_obj.object.id = "obj2";
att_obj.object.shapes[0].type = arm_navigation_msgs::Shape::CYLINDER;
att_obj.object.shapes[0].dimensions.resize(2);
att_obj.object.shapes[0].dimensions[0] = .025;
att_obj.object.shapes[0].dimensions[1] = .5;
att_obj.object.poses.resize(1);
att_obj.object.poses[0].position.x = .12;
att_obj.object.poses[0].position.y = 0.0;
att_obj.object.poses[0].position.z = 0.0;
att_obj.object.poses[0].orientation.w = 1.0;
ps_req.collision_object_diffs.push_back(obj1);
ps_req.attached_collision_object_diffs.push_back(att_obj);
arm_navigation_msgs::GetRobotState::Request rob_state_req;
arm_navigation_msgs::GetRobotState::Response rob_state_res;
ros::Rate slow_wait(1.0);
while(1) {
if(!nh.ok()) {
ros::shutdown();
exit(0);
}
planning_scene_client.call(ps_req,ps_res);
ROS_INFO_STREAM("Num coll " << ps_res.all_collision_objects.size() << " att " << ps_res.all_attached_collision_objects.size());
if(ps_res.all_collision_objects.size() > 0
&& ps_res.all_attached_collision_objects.size() > 0) break;
slow_wait.sleep();
}
ROS_INFO("After test");
ros::ServiceClient robot_state_service = nh.serviceClient<arm_navigation_msgs::GetRobotState>(robot_state_name, true);
ros::service::waitForService(robot_state_name);
planning_models::KinematicState* state = cps->setupForGroupQueries("right_arm_and_end_effector",
ps_res.complete_robot_state,
ps_res.allowed_collision_matrix,
ps_res.transformed_allowed_contacts,
ps_res.all_link_padding,
ps_res.all_collision_objects,
ps_res.all_attached_collision_objects,
ps_res.unmasked_collision_map);
ros::WallDuration tot_ode, tot_prox;
ros::WallDuration min_ode(1000.0,1000.0);
ros::WallDuration min_prox(1000.0, 1000.0);
ros::WallDuration max_ode;
ros::WallDuration max_prox;
std::vector<double> link_distances;
std::vector<std::vector<double> > distances;
std::vector<std::vector<tf::Vector3> > gradients;
std::vector<std::string> link_names;
std::vector<std::string> attached_body_names;
std::vector<collision_proximity::CollisionType> collisions;
unsigned int prox_num_in_collision = 0;
unsigned int ode_num_in_collision = 0;
ros::WallTime n1, n2;
for(unsigned int i = 0; i < TEST_NUM; i++) {
for(unsigned int j = 0; j < joint_state_names.size(); j++) {
joint_state_map[joint_state_names[j]] = valid_joint_states[i][j];
}
state->setKinematicState(joint_state_map);
n1 = ros::WallTime::now();
cps->setCurrentGroupState(*state);
bool in_prox_collision = cps->isStateInCollision();
n2 = ros::WallTime::now();
if(in_prox_collision) {
prox_num_in_collision++;
}
ros::WallDuration prox_dur(n2-n1);
if(prox_dur > max_prox) {
max_prox = prox_dur;
} else if (prox_dur < min_prox) {
min_prox = prox_dur;
}
tot_prox += prox_dur;
n1 = ros::WallTime::now();
bool ode_in_collision = cps->getCollisionModels()->isKinematicStateInCollision(*state);
n2 = ros::WallTime::now();
if(ode_in_collision) {
ode_num_in_collision++;
}
if(0){//in_prox_collision && !ode_in_collision) {
ros::Rate r(1.0);
while(nh.ok()) {
cps->getStateCollisions(link_names, attached_body_names, in_prox_collision, collisions);
ROS_INFO("Prox not ode");
cps->visualizeDistanceField();
cps->visualizeCollisions(link_names, attached_body_names, collisions);
cps->visualizeConvexMeshes(cps->getCollisionModels()->getGroupLinkUnion());
std::vector<std::string> objs = link_names;
objs.insert(objs.end(), attached_body_names.begin(), attached_body_names.end());
cps->visualizeObjectSpheres(objs);
//cps->visualizeVoxelizedLinks(collision_models->getGroupLinkUnion());
r.sleep();
}
exit(0);
}
if(0 && !in_prox_collision && ode_in_collision) {
ros::Rate r(1.0);
while(nh.ok()) {
ROS_INFO("Ode not prox");
cps->visualizeDistanceField();
cps->getStateCollisions(link_names, attached_body_names, in_prox_collision, collisions);
cps->visualizeCollisions(link_names, attached_body_names, collisions);
cps->visualizeConvexMeshes(cps->getCollisionModels()->getGroupLinkUnion());
std::vector<std::string> objs = link_names;
objs.insert(objs.end(), attached_body_names.begin(), attached_body_names.end());
cps->visualizeObjectSpheres(objs);
r.sleep();
}
//cps->visualizeVoxelizedLinks(collision_models->getGroupLinkUnion());
std::vector<collision_space::EnvironmentModel::AllowedContact> allowed_contacts;
std::vector<collision_space::EnvironmentModel::Contact> contact;
cps->getCollisionModels()->getCollisionSpace()->getCollisionContacts(allowed_contacts, contact, 10);
for(unsigned int i = 0; i < contact.size(); i++) {
std::string name1;
std::string name2;
if(contact[i].link1 != NULL) {
if(contact[i].link1_attached_body_index != 0) {
name1 = contact[i].link1->getAttachedBodyModels()[contact[i].link1_attached_body_index-1]->getName();
} else {
name1 = contact[i].link1->getName();
}
}
if(contact[i].link2 != NULL) {
if(contact[i].link2_attached_body_index != 0) {
name2 = contact[i].link2->getAttachedBodyModels()[contact[i].link2_attached_body_index-1]->getName();
} else {
name2 = contact[i].link2->getName();
}
} else if (!contact[i].object_name.empty()) {
name2 = contact[i].object_name;
}
ROS_INFO_STREAM("Contact " << i << " between " << name1 << " and " << name2);
}
exit(0);
if(0) {
std::vector<double> prox_link_distances;
std::vector<std::vector<double> > prox_distances;
std::vector<std::vector<tf::Vector3> > prox_gradients;
std::vector<std::string> prox_link_names;
std::vector<std::string> prox_attached_body_names;
cps->getStateGradients(prox_link_names, prox_attached_body_names, prox_link_distances, prox_distances, prox_gradients);
ROS_INFO_STREAM("Link size " << prox_link_names.size());
for(unsigned int i = 0; i < prox_link_names.size(); i++) {
ROS_INFO_STREAM("Link " << prox_link_names[i] << " closest distance " << prox_link_distances[i]);
}
for(unsigned int i = 0; i < prox_attached_body_names.size(); i++) {
ROS_INFO_STREAM("Attached body names " << prox_attached_body_names[i] << " closest distance " << prox_link_distances[i+prox_link_names.size()]);
}
exit(0);
}
}
ros::WallDuration ode_dur(n2-n1);
if(ode_dur > max_ode) {
max_ode = ode_dur;
} else if (ode_dur < min_ode) {
min_ode = ode_dur;
}
tot_ode += ode_dur;
}
//ROS_INFO_STREAM("Setup prox " << tot_prox_setup.toSec()/(TEST_NUM*1.0) << " ode " << tot_ode_setup.toSec()/(TEST_NUM*1.0));
ROS_INFO_STREAM("Av prox time " << (tot_prox.toSec()/(TEST_NUM*1.0)) << " min " << min_prox.toSec() << " max " << max_prox.toSec()
<< " percent in coll " << (prox_num_in_collision*1.0)/(TEST_NUM*1.0));
ROS_INFO_STREAM("Av ode time " << (tot_ode.toSec()/(TEST_NUM*1.0)) << " min " << min_ode.toSec() << " max " << max_ode.toSec()
<< " percent in coll " << (ode_num_in_collision*1.0)/(TEST_NUM*1.0));
ros::shutdown();
delete cps;
}
bool Action::placeAction(MetaBlock *block, const std::string surface_name)
{
if (verbose_)
ROS_INFO_STREAM("Placing " << block->name << " at pose " << block->goal_pose);
// Prevent collision with table
if (!surface_name.empty())
move_group_->setSupportSurfaceName(surface_name);
std::vector<moveit_msgs::PlaceLocation> place_locations;
// Re-usable datastruct
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.frame_id = grasp_data_.base_link_;
pose_stamped.header.stamp = ros::Time::now();
// Create 360 degrees of place location rotated around a center
//for (double angle = 0; angle < 2*M_PI; angle += M_PI/2)
//{
pose_stamped.pose = block->goal_pose;
// Create new place location
moveit_msgs::PlaceLocation place_loc;
place_loc.place_pose = pose_stamped;
// Approach
moveit_msgs::GripperTranslation pre_place_approach;
pre_place_approach.direction.header.stamp = ros::Time::now();
pre_place_approach.desired_distance = grasp_data_.approach_retreat_desired_dist_; // The distance the origin of a robot link needs to travel
pre_place_approach.min_distance = grasp_data_.approach_retreat_min_dist_; // half of the desired? Untested.
pre_place_approach.direction.header.frame_id = grasp_data_.base_link_;
pre_place_approach.direction.vector.x = 0;
pre_place_approach.direction.vector.y = 0;
pre_place_approach.direction.vector.z = 0.1; //-1 // Approach direction (negative z axis) // TODO: document this assumption
place_loc.pre_place_approach = pre_place_approach;
// Retreat
/*moveit_msgs::GripperTranslation post_place_retreat(pre_place_approach);
post_place_retreat.direction.vector.x = 0;
post_place_retreat.direction.vector.y = 0;
post_place_retreat.direction.vector.z = -1; //1 // Retreat direction (pos z axis)
place_loc.post_place_retreat = post_place_retreat;*/
// Post place posture - use same as pre-grasp posture (the OPEN command)
//ROS_INFO_STREAM("place_loc.post_place_retreat" << place_loc.post_place_retreat);
place_loc.post_place_posture = grasp_data_.pre_grasp_posture_; //grasp_data_.grasp_posture_;
place_locations.push_back(place_loc);
//}
move_group_->setPlannerId("RRTConnectkConfigDefault");
bool success = move_group_->place(block->name, place_locations);
if (verbose_)
{
if (success)
ROS_INFO_STREAM("Place success! \n\n");
else
ROS_ERROR_STREAM_NAMED("simple_actions:","Place failed.");
}
return success;
}
MoveItSimpleControllerManager() : node_handle_("~") {
if (!node_handle_.hasParam("controller_list")) {
ROS_ERROR_STREAM("MoveitSimpleControllerManager: No controller_list specified.");
return;
}
XmlRpc::XmlRpcValue controller_list;
node_handle_.getParam("controller_list", controller_list);
if (controller_list.getType() != XmlRpc::XmlRpcValue::TypeArray) {
ROS_ERROR("MoveitSimpleControllerManager: controller_list should be specified as an array");
return;
}
/* actually create each controller */
for (int i = 0 ; i < controller_list.size() ; ++i) {
if (!controller_list[i].hasMember("name") || !controller_list[i].hasMember("joints")) {
ROS_ERROR("MoveitSimpleControllerManager: Name and joints must be specifed for each controller");
continue;
}
try {
std::string name = std::string(controller_list[i]["name"]);
std::string action_ns;
if (controller_list[i].hasMember("ns")) {
/* TODO: this used to be called "ns", renaming to "action_ns" and will remove in the future */
action_ns = std::string(controller_list[i]["ns"]);
ROS_WARN("MoveitSimpleControllerManager: use of 'ns' is deprecated, use 'action_ns' instead.");
}
else if (controller_list[i].hasMember("action_ns")) action_ns = std::string(controller_list[i]["action_ns"]);
else ROS_WARN("MoveitSimpleControllerManager: please note that 'action_ns' no longer has a default value.");
if (controller_list[i]["joints"].getType() != XmlRpc::XmlRpcValue::TypeArray) {
ROS_ERROR_STREAM("MoveitSimpleControllerManager: The list of joints for controller " << name << " is not specified as an array");
continue;
}
if (!controller_list[i].hasMember("type")) {
ROS_ERROR_STREAM("MoveitSimpleControllerManager: No type specified for controller " << name);
continue;
}
std::string type = std::string(controller_list[i]["type"]);
ActionBasedControllerHandleBasePtr new_handle;
if ( type == "GripperCommand" ) {
new_handle.reset(new GripperControllerHandle(name, action_ns));
if (static_cast<GripperControllerHandle*>(new_handle.get())->isConnected()) {
if (controller_list[i].hasMember("parallel")) {
if (controller_list[i]["joints"].size() != 2) {
ROS_ERROR_STREAM("MoveItSimpleControllerManager: Parallel Gripper requires exactly two joints");
continue;
}
static_cast<GripperControllerHandle*>(new_handle.get())->setParallelJawGripper(controller_list[i]["joints"][0], controller_list[i]["joints"][1]);
} else {
if (controller_list[i].hasMember("command_joint"))
static_cast<GripperControllerHandle*>(new_handle.get())->setCommandJoint(controller_list[i]["command_joint"]);
else
static_cast<GripperControllerHandle*>(new_handle.get())->setCommandJoint(controller_list[i]["joints"][0]);
}
if (controller_list[i].hasMember("allow_failure")) static_cast<GripperControllerHandle*>(new_handle.get())->allowFailure(true);
ROS_INFO_STREAM("MoveitSimpleControllerManager: Added GripperCommand controller for " << name );
controllers_[name] = new_handle;
}
} else if ( type == "FollowJointTrajectory" ) {
new_handle.reset(new FollowJointTrajectoryControllerHandle(name, action_ns));
if (static_cast<FollowJointTrajectoryControllerHandle*>(new_handle.get())->isConnected()) {
ROS_INFO_STREAM("MoveitSimpleControllerManager: Added FollowJointTrajectory controller for " << name );
controllers_[name] = new_handle;
}
} else {
ROS_ERROR("Unknown controller type: '%s'", type.c_str());
continue;
}
if (!controllers_[name]) { controllers_.erase(name); continue; }
/* add list of joints, used by controller manager and moveit */
for (int j = 0 ; j < controller_list[i]["joints"].size() ; ++j) controllers_[name]->addJoint(std::string(controller_list[i]["joints"][j]));
}
catch (...) { ROS_ERROR("MoveitSimpleControllerManager: Unable to parse controller information"); }
}
}
void BasicWorker::MsgCB(const std_msgs::Int32ConstPtr &msg) {
ROS_INFO("Got callback");
ROS_INFO_STREAM("Heard: " << msg->data);
}
int main(int argc, char** argv)
{
bool desired_mode_full=false; //if false go in safe_mode
bool ir_warning_;
bool bumper_warning_;
ros::init(argc, argv, "roomba560_node");
ROS_INFO("Roomba for ROS %.2f", NODE_VERSION);
double last_x, last_y, last_yaw;
double vel_x, vel_y, vel_yaw;
double dt;
float last_charge = 0.0;
int time_remaining = -1;
ros::NodeHandle n;
ros::NodeHandle pn("~");
std::string base_name;
int id;
pn.param<std::string>("base_name_", base_name, "iRobot_");
pn.param<int>("id_", id, 0);
std_msgs::String my_namespace;
my_namespace.data = base_name + std::to_string(id);
bool publish_name;
ros::Publisher listpub;
pn.param<bool>("publish_name_", publish_name, false);
if(publish_name)
{
listpub = n.advertise<std_msgs::String>("/agent_list", 50);
}
int name_count=0;
pn.param<std::string>("port_", port, "/dev/ttyUSB0");
std::string base_frame_id;
std::string odom_frame_id;
pn.param<std::string>("base_frame_id", base_frame_id, my_namespace.data + "/base_link");
pn.param<std::string>("odom_frame_id", odom_frame_id, my_namespace.data + "/odom");
bool publishTf;
pn.param<bool>("publishTf", publishTf, true);
Eigen::MatrixXd poseCovariance(6,6);
Eigen::MatrixXd twistCovariance(6,6);
bool pose_cov_mat = false;
bool twist_cov_mat = false;
std::vector<double> pose_covariance_matrix;
std::vector<double> twist_covariance_matrix;
XmlRpc::XmlRpcValue poseCovarConfig;
XmlRpc::XmlRpcValue twistCovarConfig;
if (pn.hasParam("poseCovariance"))
{
try
{
pn.getParam("poseCovariance", poseCovarConfig);
ROS_ASSERT(poseCovarConfig.getType() == XmlRpc::XmlRpcValue::TypeArray);
int matSize = poseCovariance.rows();
if (poseCovarConfig.size() != matSize * matSize)
{
ROS_WARN_STREAM("Pose_covariance matrix should have " << matSize * matSize << " values.");
}
else
{
for (int i = 0; i < matSize; i++)
{
for (int j = 0; j < matSize; j++)
{
std::ostringstream ostr;
ostr << poseCovarConfig[matSize * i + j];
std::istringstream istr(ostr.str());
istr >> poseCovariance(i, j);
pose_covariance_matrix.push_back(poseCovariance(i,j));
}
}
pose_cov_mat = true;
}
}
catch (XmlRpc::XmlRpcException &e)
{
ROS_ERROR_STREAM("ERROR reading sensor config: " << e.getMessage() << " for pose_covariance (type: " << poseCovarConfig.getType() << ")");
}
}
if (pn.hasParam("twistCovariance"))
{
try
{
pn.getParam("twistCovariance", twistCovarConfig);
ROS_ASSERT(twistCovarConfig.getType() == XmlRpc::XmlRpcValue::TypeArray);
int matSize = twistCovariance.rows();
if (twistCovarConfig.size() != matSize * matSize)
{
ROS_WARN_STREAM("Twist_covariance matrix should have " << matSize * matSize << " values.");
}
else
{
for (int i = 0; i < matSize; i++)
{
for (int j = 0; j < matSize; j++)
{
std::ostringstream ostr;
ostr << twistCovarConfig[matSize * i + j];
std::istringstream istr(ostr.str());
istr >> twistCovariance(i, j);
twist_covariance_matrix.push_back(twistCovariance(i,j));
}
}
twist_cov_mat = true;
}
}
catch (XmlRpc::XmlRpcException &e)
{
ROS_ERROR_STREAM("ERROR reading sensor config: " << e.getMessage() << " for twist_covariance (type: " << poseCovarConfig.getType() << ")");
}
}
roomba = new irobot::OpenInterface(port.c_str());
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
ros::Publisher battery_pub = n.advertise<irobotcreate2::Battery>("battery", 50);
ros::Publisher bumper_pub = n.advertise<irobotcreate2::Bumper>("bumper", 50);
ros::Publisher buttons_pub = n.advertise<irobotcreate2::Buttons>("buttons", 50);
ros::Publisher cliff_pub = n.advertise<irobotcreate2::RoombaIR>("cliff", 50);
ros::Publisher irbumper_pub = n.advertise<irobotcreate2::RoombaIR>("ir_bumper", 50);
ros::Publisher irchar_pub = n.advertise<irobotcreate2::IRCharacter>("ir_character", 50);
ros::Publisher wheeldrop_pub = n.advertise<irobotcreate2::WheelDrop>("wheel_drop", 50);
tf::TransformBroadcaster tf_broadcaster;
ros::Subscriber cmd_vel_sub = n.subscribe<geometry_msgs::Twist>("cmd_vel", 1, cmdVelReceived);
ros::Subscriber leds_sub = n.subscribe<irobotcreate2::Leds>("leds", 1, ledsReceived);
ros::Subscriber digitleds_sub = n.subscribe<irobotcreate2::DigitLeds>("digit_leds", 1, digitLedsReceived);
ros::Subscriber song_sub = n.subscribe<irobotcreate2::Song>("song", 1, songReceived);
ros::Subscriber playsong_sub = n.subscribe<irobotcreate2::PlaySong>("play_song", 1, playSongReceived);
ros::Subscriber mode_sub = n.subscribe<std_msgs::String>("mode", 1, cmdModeReceived);
ros::Subscriber special_sub = n.subscribe<std_msgs::String>("special", 1, cmdSpecialReceived);
irobot::OI_Packet_ID sensor_packets[1] = {irobot::OI_PACKET_GROUP_100};
roomba->setSensorPackets(sensor_packets, 1, OI_PACKET_GROUP_100_SIZE);
if( roomba->openSerialPort(true) == 0) ROS_INFO("Connected to Roomba.");
else
{
ROS_FATAL("Could not connect to Roomba.");
ROS_BREAK();
}
ros::Time current_time, last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();
bool first_loop=true;
while(roomba->getSensorPackets(100) == -1 && ros::ok())
{
usleep(100);
ROS_INFO_STREAM("Waiting for roomba sensors");
}
roomba->calculateOdometry();
roomba->resetOdometry();
ros::Rate r(50.0);
while(n.ok())
{
if(publish_name)
{
name_count++;
if(name_count >= 10)
{
listpub.publish(my_namespace);
name_count=0;
}
}
ir_warning_ = false;
bumper_warning_ = false;
current_time = ros::Time::now();
last_x = roomba->odometry_x_;
last_y = roomba->odometry_y_;
last_yaw = roomba->odometry_yaw_;
if( roomba->getSensorPackets(100) == -1) ROS_ERROR("Could not retrieve sensor packets.");
else roomba->calculateOdometry();
/* OLD CODE
* dt = (current_time - last_time).toSec();
vel_x = (roomba->odometry_x_ - last_x)/dt;
vel_y = (roomba->odometry_y_ - last_y)/dt;
vel_yaw = (roomba->odometry_yaw_ - last_yaw)/dt;*/
vel_x = roomba->new_odometry_.getLinear();
vel_y = 0.0;
vel_yaw = roomba->new_odometry_.getAngular();
// ******************************************************************************************
//first, we'll publish the transforms over tf
if(publishTf)
{
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = odom_frame_id;
odom_trans.child_frame_id = base_frame_id;
odom_trans.transform.translation.x = roomba->odometry_x_;
odom_trans.transform.translation.y = roomba->odometry_y_;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(roomba->odometry_yaw_);
tf_broadcaster.sendTransform(odom_trans);
}
//TODO: Finish brodcasting the tf for all the ir sensors on the Roomba
/*geometry_msgs::TransformStamped cliff_left_trans;
cliff_left_trans.header.stamp = current_time;
cliff_left_trans.header.frame_id = "base_link";
cliff_left_trans.child_frame_id = "base_cliff_left";
cliff_left_trans.transform.translation.x = 0.0;
cliff_left_trans.transform.translation.y = 0.0;
cliff_left_trans.transform.translation.z = 0.0;
cliff_left_trans.transform.rotation = ;
tf_broadcaster.sendTransform(cliff_left_trans); */
// ******************************************************************************************
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = odom_frame_id;
//set the position
odom.pose.pose.position.x = roomba->odometry_x_;
odom.pose.pose.position.y = roomba->odometry_y_;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(roomba->odometry_yaw_);
if(pose_cov_mat)
for(int i = 0; i < pose_covariance_matrix.size(); i++)
odom.pose.covariance[i] = pose_covariance_matrix[i];
//set the velocity
odom.child_frame_id = base_frame_id;
odom.twist.twist.linear.x = vel_x;
odom.twist.twist.linear.y = vel_y;
odom.twist.twist.angular.z = vel_yaw;
if(twist_cov_mat)
for(int i = 0; i < pose_covariance_matrix.size(); i++)
odom.twist.covariance[i] = twist_covariance_matrix[i];
//publish the message
odom_pub.publish(odom);
// ******************************************************************************************
//publish battery
irobotcreate2::Battery battery;
battery.header.stamp = current_time;
battery.power_cord = roomba->power_cord_;
battery.dock = roomba->dock_;
battery.level = 100.0*(roomba->charge_/roomba->capacity_);
if(last_charge > roomba->charge_) time_remaining = (int)(battery.level/((last_charge-roomba->charge_)/roomba->capacity_)/dt)/60;
last_charge = roomba->charge_;
battery.time_remaining = time_remaining;
battery_pub.publish(battery);
// ******************************************************************************************
//publish bumpers
irobotcreate2::Bumper bumper;
bumper.left.header.stamp = current_time;
bumper.left.state = roomba->bumper_[LEFT];
bumper.right.header.stamp = current_time;
bumper.right.state = roomba->bumper_[RIGHT];
bumper_pub.publish(bumper);
bumper_warning_ = bumper.left.state || bumper.right.state;
bumper_warning.store(bumper_warning_);
// ******************************************************************************************
//publish buttons
irobotcreate2::Buttons buttons;
buttons.header.stamp = current_time;
buttons.clean = roomba->buttons_[BUTTON_CLEAN];
buttons.spot = roomba->buttons_[BUTTON_SPOT];
buttons.dock = roomba->buttons_[BUTTON_DOCK];
buttons.day = roomba->buttons_[BUTTON_DAY];
buttons.hour = roomba->buttons_[BUTTON_HOUR];
buttons.minute = roomba->buttons_[BUTTON_MINUTE];
buttons.schedule = roomba->buttons_[BUTTON_SCHEDULE];
buttons.clock = roomba->buttons_[BUTTON_CLOCK];
buttons_pub.publish(buttons);
// ******************************************************************************************
//publish cliff
irobotcreate2::RoombaIR cliff;
cliff.header.stamp = current_time;
cliff.header.frame_id = "base_cliff_left";
cliff.state = roomba->cliff_[LEFT];
cliff.signal = roomba->cliff_signal_[LEFT];
cliff_pub.publish(cliff);
cliff.header.frame_id = "base_cliff_front_left";
cliff.state = roomba->cliff_[FRONT_LEFT];
cliff.signal = roomba->cliff_signal_[FRONT_LEFT];
cliff_pub.publish(cliff);
cliff.header.frame_id = "base_cliff_front_right";
cliff.state = roomba->cliff_[FRONT_RIGHT];
cliff.signal = roomba->cliff_signal_[FRONT_RIGHT];
cliff_pub.publish(cliff);
cliff.header.frame_id = "base_cliff_right";
cliff.state = roomba->cliff_[RIGHT];
cliff.signal = roomba->cliff_signal_[RIGHT];
cliff_pub.publish(cliff);
// ******************************************************************************************
//publish irbumper
irobotcreate2::RoombaIR irbumper;
irbumper.header.stamp = current_time;
irbumper.header.frame_id = "base_irbumper_left";
irbumper.state = roomba->ir_bumper_[LEFT];
irbumper.signal = roomba->ir_bumper_signal_[LEFT];
irbumper_pub.publish(irbumper);
ir_warning_ = ir_warning_ || irbumper.state;
irbumper.header.frame_id = "base_irbumper_front_left";
irbumper.state = roomba->ir_bumper_[FRONT_LEFT];
irbumper.signal = roomba->ir_bumper_signal_[FRONT_LEFT];
irbumper_pub.publish(irbumper);
ir_warning_ = ir_warning_ || irbumper.state;
irbumper.header.frame_id = "base_irbumper_center_left";
irbumper.state = roomba->ir_bumper_[CENTER_LEFT];
irbumper.signal = roomba->ir_bumper_signal_[CENTER_LEFT];
irbumper_pub.publish(irbumper);
ir_warning_ = ir_warning_ || irbumper.state;
irbumper.header.frame_id = "base_irbumper_center_right";
irbumper.state = roomba->ir_bumper_[CENTER_RIGHT];
irbumper.signal = roomba->ir_bumper_signal_[CENTER_RIGHT];
irbumper_pub.publish(irbumper);
ir_warning_ = ir_warning_ || irbumper.state;
irbumper.header.frame_id = "base_irbumper_front_right";
irbumper.state = roomba->ir_bumper_[FRONT_RIGHT];
irbumper.signal = roomba->ir_bumper_signal_[FRONT_RIGHT];
irbumper_pub.publish(irbumper);
ir_warning_ = ir_warning_ || irbumper.state;
irbumper.header.frame_id = "base_irbumper_right";
irbumper.state = roomba->ir_bumper_[RIGHT];
irbumper.signal = roomba->ir_bumper_signal_[RIGHT];
irbumper_pub.publish(irbumper);
ir_warning_ = ir_warning_ || irbumper.state;
ir_warning.store(ir_warning_);
// ******************************************************************************************
//publish irchar
irobotcreate2::IRCharacter irchar;
irchar.header.stamp = current_time;
irchar.omni = roomba->ir_char_[OMNI];
irchar.left = roomba->ir_char_[LEFT];
irchar.right = roomba->ir_char_[RIGHT];
irchar_pub.publish(irchar);
// ******************************************************************************************
//publish wheeldrop
irobotcreate2::WheelDrop wheeldrop;
wheeldrop.left.header.stamp = current_time;
wheeldrop.left.state = roomba->wheel_drop_[LEFT];
wheeldrop.right.header.stamp = current_time;
wheeldrop.right.state = roomba->wheel_drop_[RIGHT];
wheeldrop_pub.publish(wheeldrop);
ros::spinOnce();
r.sleep();
if(first_loop)
{
roomba->startOI(true);
if(!desired_mode_full) roomba->Safe();
first_loop=false;
}
}
roomba->powerDown();
roomba->closeSerialPort();
}
// Call setup functions
bool GlobalPlanner::Init(ros::NodeHandle* nh)
{
ROS_INFO_STREAM("Initializing global planner class");
m_nh = nh;
SetupCallbacks();
ROS_INFO_STREAM("Initializing TaskMaster");
std::string waypointFile("testList1.points");
if (m_nh->getParam("/global_planner/waypoints_file", waypointFile))
{
ROS_INFO_STREAM("Read from file: "<<waypointFile);
}
// Planner types :
// naive : original, for each waypoint in order of file, choose first available robot
// closest_robot : for each waypoint in order of file, choose closest available robot
// closest_waypoint : for each available robot in order of id, choose closest available waypoint
std::string planner("naive");
m_planner = PLANNER_NAIVE;
if (m_nh->getParam("/global_planner/planner", planner))
{
ROS_INFO_STREAM("Planner set from file to " << planner);
if (planner.compare("closest_robot") == 0) {
m_planner = PLANNER_CLOSEST_ROBOT;
}
else if (planner.compare("closest_waypoint") == 0) {
m_planner = PLANNER_CLOSEST_WAYPOINT;
}
}
// Load list of possible dumpsites
// std::string dumpsiteFile("cubicle_dump_meets.points");
std::string dumpsiteFile("springdemo_dump_meets.points");
m_nh->getParam("/global_planner/dumpsite_file", dumpsiteFile);
ROS_INFO_STREAM("Loading dumpsite file: " << dumpsiteFile);
loadDumpSites(dumpsiteFile);
// Need to wait for m_robots to get populated by callback
// Some better ways todo: call globalplanner with param saying # of robots, then wait till # robots is populated
// for now just pausing for 10 seconds
// Unsustainable. Must hack harder.
// ROS_INFO_STREAM("Waiting till 2 robot controllers are loaded and update global planner robot map...");
// int robot_count = 0;
// while (robot_count < 2)
// {
// robot_count = 0;
// for (std::map<int, Robot_Ptr>::iterator it = m_robots.begin(); it != m_robots.end(); ++it)
// {
// robot_count++;
// }
// ROS_INFO_STREAM(robot_count << " robots loaded so far.");
// ros::spinOnce();
// sleep(1);
// }
// ROS_INFO("Done waiting, initializing task master.");
m_tm.Init(nh, m_robots, waypointFile);
ros::spinOnce();
return true;
}
int main(int argc, char **argv)
{
std::string nodeName = "node";
std::string hostName = "localhost";
std::string configFile = "";
for(int i = 0; i < argc; i++)
{
if(!strcmp(argv[i], "-nodename"))
nodeName = argv[i+1];
if(!strcmp(argv[i], "-hostname"))
hostName = argv[i+1];
if(!strcmp(argv[i], "-config"))
configFile = argv[i+1];
}
std::vector<boost::thread*> compThreads;
XMLParser nodeParser;
std::string configFileName = nodeName + ".xml";
if (configFile.length() > 0)
configFileName = configFile;
if (nodeParser.Parse(configFileName))
{
for (int i=0;i<nodeParser.libList.size();i++)
{
void *hndl = dlopen(nodeParser.libList[i].c_str(), RTLD_LAZY | RTLD_GLOBAL);
if (hndl == NULL)
{
cerr << dlerror() << endl;
exit(-1);
}
else
ROS_INFO_STREAM("Opened " << nodeParser.libList[i]);
}
nodeName = nodeParser.nodeName;
ros::init(argc, argv, nodeName.c_str());
// Create Node Handle
ros::NodeHandle n;
ROS_INFO_STREAM(nodeName << " thread id = " << boost::this_thread::get_id());
for (int i=0;i<nodeParser.compConfigList.size();i++)
{
std::string libraryLocation = nodeParser.compConfigList[i].libraryLocation;
void *hndl = dlopen(libraryLocation.c_str(), RTLD_NOW);
if(hndl == NULL)
{
cerr << dlerror() << endl;
exit(-1);
}
void *mkr = dlsym(hndl, "maker");
Component *comp_inst = ((Component *(*)(ComponentConfig &, int , char **))(mkr))
(nodeParser.compConfigList[i], argc, argv);
// Create Component Threads
boost::thread *comp_thread = new boost::thread(componentThreadFunc, comp_inst);
compThreads.push_back(comp_thread);
ROS_INFO_STREAM(nodeName << " has started " << nodeParser.compConfigList[i].compName);
}
for (int i=0;i<compThreads.size();i++)
{
compThreads[i]->join();
}
return 0;
}
else
{
printf("ERROR::Unable to parse XML file\n");
return -1;
}
}
// Executive function
void GlobalPlanner::Execute()
{
ros::spinOnce();
if ((ros::Time::now() - m_lastDisplay) > ros::Duration(5))
{
Display();
}
// Get Robot Status...
QueryRobots();
// FOR each pair of robots that're just chillin in a dump stage and are stopped near the handoff location
// Pick best bot to meet with?
// Check if robot1 & robot2 are within handoff threshold
// Add new dump to m_dumpMap
// Send handoff message to each robot giving them their new capacities
//
// IF robot in wait state AND is full AND there are goals
// Find the best dump location for the robots to meet
// Update Dump List
// Send Dump Message
//
// IF there are goals available
// pick best robot to do job (return an ID)
// OPTIONAL: IF need to cancel robot's current goal -> Send Cancel Message First AND update the task it was assigned to
// Send goal message
// ELSE IF robots available AND waypoints available
// pick best robot to get to waypoints (return an ID)
// OPTIONAL: IF need to cancel robot's current goal -> Send Cancel Message First AND update the task it was assigned to
// Send Waypoint Message
// Check if a robot is full & find the best binbot for it
std::vector<Robot_Ptr> availableRobots = GetAvailableRobots(0); // Get any available robots even with no storage space left
for (std::vector<Robot_Ptr>::iterator it = availableRobots.begin(); it != availableRobots.end(); ++it)
{
Robot_Ptr robot = *it;
// If robot has no space (TODO: Check if collector bot or bin bot for where to dump)
if (robot->GetStorageAvailable() <= 0 &&
robot->GetType() == RobotState::COLLECTOR_BOT)
{
ROS_INFO_STREAM_THROTTLE(5, "Robot " << robot->GetName() <<
"(" << robot->GetID() << ")" <<
" full, searching for closest available bin bot...");
int collectorBot = robot->GetID();
// Get closest bin bot to collector bot if it exists
int bestBinBot = GetBestBinBot( collectorBot );
if (bestBinBot != NO_ROBOT_FOUND) {
ROS_INFO_STREAM("Bin Bot " << robot->GetName() <<
"(" << robot->GetID() << ") found.");
// Create new dump site
Dump_Ptr dp(new DumpWrapper(dumps_count++));
m_tm.AddDump(dp);
// Assign collector robot to dump, Assign bin bot to dump
if (!AssignRobotsDump(collectorBot, bestBinBot, dp->GetID()))
{
ROS_ERROR_STREAM("Error Assigning Robots " << m_robots[collectorBot]->GetName() <<
"(" << collectorBot << ") & " << m_robots[bestBinBot]->GetName() <<
"(" << bestBinBot << ")" << " to Dump(" << dp->GetID() << ")");
}
}
}
}
// Check each dump, see if each robot in the dump finished state
std::map<int, Dump_Ptr > dumps = m_tm.GetDumps();
for (std::map<int, Dump_Ptr>::iterator it = dumps.begin(); it != dumps.end(); ++it)
{
Dump_Ptr dump = it->second;
int collectorBot = dump->GetRobot1();
int binBot = dump->GetRobot2();
// if (dump->GetReadyToTransfer()) { // This requires services to work
// Avoid services by querying robot states
if ( dump->GetInProgress() && (m_robots[collectorBot]->GetState() == RobotState::WAITING || m_robots[binBot]->GetState() == RobotState::WAITING) )
{
// Resend dump so waiting robot
m_tm.SendDump(dump->GetID());
}
if (dump->GetInProgress() && m_robots[collectorBot]->GetState() == RobotState::DUMPING_FINISHED && m_robots[binBot]->GetState() == RobotState::DUMPING_FINISHED)
{
ROS_INFO_STREAM("Dump(" << dump->GetID() << ") transferring trash.");
// Move trash over
int collectorBot = dump->GetRobot1();
int binBot = dump->GetRobot2();
int trash_to_transfer = m_robots[collectorBot]->GetStorageUsed();
// Send trash updates to robot controllers
global_planner::SetTrashSrv s;
s.request.storage = 0;
if (m_setTrashServices[collectorBot].call(s) && s.response.result == 0) { ROS_INFO_STREAM("Set Trash for " << m_robots[collectorBot]->GetName()); }
else { ROS_ERROR_STREAM("Did not receive trash response from robot: " << collectorBot); }
s.request.storage = m_robots[binBot]->GetStorageUsed() + trash_to_transfer;
if (m_setTrashServices[binBot].call(s) && s.response.result == 0) { ROS_INFO_STREAM("Set Trash for " << m_robots[binBot]->GetName()); }
else { ROS_ERROR_STREAM("Did not receive trash response from robot: " << binBot); }
// In global planner, update storage values
// Add trash to bin bot
m_robots[binBot]->SetStorageUsed( m_robots[binBot]->GetStorageUsed() + trash_to_transfer );
// Remove trash from collector bot
m_robots[collectorBot]->SetStorageUsed(0);
// Set states to waiting in global planner but not robot controllers
// the robot controllers are in DUMP_FINISHED state, but they're ready to transition to waypoints etc.
// So set them to waiting in global planner and hopefully go to planning before a callback update resets them.
m_robots[collectorBot]->SetState(RobotState::WAITING);
m_robots[binBot]->SetState(RobotState::WAITING);
// Transition dump state to success
dump->SetStatus(TaskResult::SUCCESS);
}
}
// ProcessGoals();
/*
// If no available waypoints, do nothing
if (m_tm.GetAvailableWaypoints().size() == 0) {
ROS_WARN_STREAM_THROTTLE(10, "No Available Waypoints! ("
<< m_tm.GetAvailableWaypoints().size() << ") "
<< (isFinished() ? "FIN" : "GP Waiting") );
// Print out status of all waypoints
std::map<int, Waypoint_Ptr> allWaypoints = m_tm.GetWaypoints();
std::stringstream ss;
for (std::map<int, Waypoint_Ptr>::iterator it = allWaypoints.begin(); it != allWaypoints.end(); ++it)
{
ss << it->second->GetID() << "-" << it->second->GetStatusMessage() << " ";
}
ROS_INFO_STREAM_THROTTLE(10, ss.str() );
return;
}
*/
switch (m_planner)
{
case PLANNER_CLOSEST_ROBOT:
PlanNNRobot();
break;
case PLANNER_CLOSEST_WAYPOINT:
PlanNNWaypoint();
break;
case PLANNER_NAIVE:
default:
PlanNaive();
break;
}
}
void TonavRos::printHelp() {
ROS_INFO_STREAM(options_description_);
}
void NavSatTransform::computeTransform()
{
// Only do this if:
// 1. We haven't computed the odom_frame->utm_frame transform before
// 2. We've received the data we need
if (!transformGood_ &&
hasTransformOdom_ &&
hasTransformGps_ &&
hasTransformImu_)
{
// Get the IMU's current RPY values. Need the raw values (for yaw, anyway).
tf2::Matrix3x3 mat(transformOrientation_);
// Convert to RPY
double imuRoll;
double imuPitch;
double imuYaw;
mat.getRPY(imuRoll, imuPitch, imuYaw);
/* The IMU's heading was likely originally reported w.r.t. magnetic north.
* However, all the nodes in robot_localization assume that orientation data,
* including that reported by IMUs, is reported in an ENU frame, with a 0 yaw
* value being reported when facing east and increasing counter-clockwise (i.e.,
* towards north). Conveniently, this aligns with the UTM grid, where X is east
* and Y is north. However, we have two additional considerations:
* 1. The IMU may have its non-ENU frame data transformed to ENU, but there's
* a possibility that its data has not been corrected for magnetic
* declination. We need to account for this. A positive magnetic
* declination is counter-clockwise in an ENU frame. Therefore, if
* we have a magnetic declination of N radians, then when the sensor
* is facing a heading of N, it reports 0. Therefore, we need to add
* the declination angle.
* 2. To account for any other offsets that may not be accounted for by the
* IMU driver or any interim processing node, we expose a yaw offset that
* lets users work with navsat_transform_node.
*/
imuYaw += (magneticDeclination_ + yawOffset_);
ROS_INFO_STREAM("Corrected for magnetic declination of " << std::fixed << magneticDeclination_ <<
" and user-specified offset of " << yawOffset_ << ". Transform heading factor is now " << imuYaw);
// Convert to tf-friendly structures
tf2::Quaternion imuQuat;
imuQuat.setRPY(0.0, 0.0, imuYaw);
// The transform order will be orig_odom_pos * orig_utm_pos_inverse * cur_utm_pos.
// Doing it this way will allow us to cope with having non-zero odometry position
// when we get our first GPS message.
tf2::Transform utmPoseWithOrientation;
utmPoseWithOrientation.setOrigin(transformUtmPose_.getOrigin());
utmPoseWithOrientation.setRotation(imuQuat);
utmWorldTransform_.mult(transformWorldPose_, utmPoseWithOrientation.inverse());
utmWorldTransInverse_ = utmWorldTransform_.inverse();
double roll = 0;
double pitch = 0;
double yaw = 0;
mat.setRotation(latestWorldPose_.getRotation());
mat.getRPY(roll, pitch, yaw);
ROS_INFO_STREAM("Transform world frame pose is: " << std::fixed <<
"\nPosition: (" << transformWorldPose_.getOrigin().getX() << ", " <<
transformWorldPose_.getOrigin().getY() << ", " <<
transformWorldPose_.getOrigin().getZ() << ")" <<
"\nOrientation: (" << roll << ", " <<
pitch << ", " <<
yaw << ")");
mat.setRotation(utmWorldTransform_.getRotation());
mat.getRPY(roll, pitch, yaw);
ROS_INFO_STREAM("World frame->utm transform is " << std::fixed <<
"\nPosition: (" << utmWorldTransform_.getOrigin().getX() << ", " <<
utmWorldTransform_.getOrigin().getY() << ", " <<
utmWorldTransform_.getOrigin().getZ() << ")" <<
"\nOrientation: (" << roll << ", " <<
pitch << ", " <<
yaw << ")");
transformGood_ = true;
// Send out the (static) UTM transform in case anyone else would like to use it.
if (broadcastUtmTransform_)
{
geometry_msgs::TransformStamped utmTransformStamped;
utmTransformStamped.header.stamp = ros::Time::now();
utmTransformStamped.header.frame_id = worldFrameId_;
utmTransformStamped.child_frame_id = "utm";
utmTransformStamped.transform = tf2::toMsg(utmWorldTransform_);
utmBroadcaster_.sendTransform(utmTransformStamped);
}
}
}
