int main(int argc, char** argv)
{
// ROS set-ups:
ros::init(argc, argv, "demoTfListener"); //node name
ros::NodeHandle nh; // create a node handle; need to pass this to the class constructor
tf::StampedTransform tf_sensor_frame_to_world_frame; //need objects of this type to hold tf's
tf::TransformListener tfListener; //create a TransformListener to listen for tf's and assemble them
//the tf listener needs to "warm up", in that it needs to collect a complete set of transforms
// to deduce arbitrary connectivities; use try/catch to let it fail until success
bool tferr = true;
ROS_INFO("waiting for tf between kinect_pc_frame and world...");
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
tfListener.lookupTransform("world", "kinect_pc_frame", ros::Time(0), tf_sensor_frame_to_world_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
ros::Rate sleep_timer(1.0); //a timer for desired rate, e.g. 1Hz
ROS_INFO:("starting main loop");
tf::Vector3 origin,row;
tf::Matrix3x3 R_orientation;
while (ros::ok()) {
tfListener.lookupTransform("world", "kinect_pc_frame", ros::Time(0), tf_sensor_frame_to_world_frame);
origin= tf_sensor_frame_to_world_frame.getOrigin();
ROS_INFO("origin: %f, %f, %f",origin[0],origin[1],origin[2]);
R_orientation = tf_sensor_frame_to_world_frame.getBasis();
ROS_INFO("orientation matrix: ");
row = R_orientation[0];
ROS_INFO(" %f %f %f ",row[0],row[1],row[2]);
row = R_orientation[1];
ROS_INFO(" %f %f %f ",row[0],row[1],row[2]);
row = R_orientation[02];
ROS_INFO(" %f %f %f ",row[0],row[1],row[2]);
ROS_INFO(" ");
ROS_INFO(" ");
//ros::spinOnce(); // tf_listener is its own thread--it does not require a spins from main
sleep_timer.sleep();
}
return 0;
}
std::error_code connect(asio::ip::udp::socket &s, const asio::ip::udp::endpoint &remote_ep, uint64_t timeout) {
std::error_code ec;
fibers::detail::fiber_ptr_t this_fiber(fibers::detail::fiber_object::current_fiber_->shared_from_this());
fibers::detail::timer_t sleep_timer(this_fiber->io_service_);
if(timeout>0) sleep_timer.expires_from_now(std::chrono::microseconds(timeout));
CHECK_CALLER(this_fiber);
if (this_fiber->caller_) {
bool timer_triggered=false;
bool async_op_triggered=false;
(*(this_fiber->caller_))([&, this_fiber](){
this_fiber->state_=fibers::detail::fiber_object::BLOCKED;
if(timeout>0) sleep_timer.async_wait(this_fiber->fiber_strand_.wrap([&, this_fiber](std::error_code ec){
/*if (ec!=asio::error::operation_aborted)*/ {
timer_triggered=true;
// Timeout, cancel socket op if it's still pending
if(async_op_triggered) {
// Both callback are called, resume fiber
this_fiber->state_=fibers::detail::fiber_object::RUNNING;
this_fiber->one_step();
} else {
s.cancel();
}
}
}));
s.async_connect(remote_ep, (this_fiber->fiber_strand_.wrap([&, this_fiber](std::error_code ec){
async_op_triggered=true;
// Operation completed, cancel timer
sleep_timer.cancel();
if(ec==asio::error::operation_aborted)
ec=asio::error::timed_out;
this_fiber->last_error_=ec;
if(timeout==0 || timer_triggered) {
// Both callback are called, resume fiber
// this_fiber->schedule();
this_fiber->state_=fibers::detail::fiber_object::RUNNING;
this_fiber->one_step();
}
})));
});
} else {
// TODO: Error
}
if (this_fiber->last_error_) {
ec=std::make_error_code(static_cast<std::errc>(this_fiber->last_error_.value()));
this_fiber->last_error_.clear();
}
return ec;
}
int main(int argc, char** argv)
{
// ROS set-ups:
ros::init(argc, argv, "demoTfListener"); //node name
ros::NodeHandle nh; // create a node handle; need to pass this to the class constructor
ROS_INFO("main: instantiating an object of type DemoTfListener");
DemoTfListener demoTfListener(&nh); //instantiate an ExampleRosClass object and pass in pointer to nodehandle for constructor to use
ros::Rate sleep_timer(UPDATE_RATE); //a timer for desired rate, e.g. 50Hz
ROS_INFO:("starting main loop");
while (ros::ok()) {
ros::spinOnce();
sleep_timer.sleep();
}
return 0;
}
int main(int argc, char** argv)
{
// ROS set-ups:
ros::init(argc, argv, "steeringController"); //node name
ros::NodeHandle nh; // create a node handle; need to pass this to the class constructor
ROS_INFO("main: instantiating an object of type SteeringController");
SteeringController steeringController(&nh); //instantiate an ExampleRosClass object and pass in pointer to nodehandle for constructor to use
ros::Rate sleep_timer(UPDATE_RATE); //a timer for desired rate, e.g. 50Hz
ROS_INFO:("starting steering algorithm");
while (ros::ok()) {
steeringController.lin_steering_algorithm(); // compute and publish twist commands and cmd_vel and cmd_vel_stamped
ros::spinOnce();
sleep_timer.sleep();
}
return 0;
}
int main(int argc, char** argv)
{
// ROS set-ups:
ros::init(argc, argv, "test_traj_sender"); //node name
ros::NodeHandle nh; // create a node handle; need to pass this to the class constructor
ros::Publisher pub = nh.advertise<trajectory_msgs::JointTrajectory>("joint_path_command", 1);
Eigen::Vector3d p;
Eigen::Vector3d n_des,t_des,b_des;
std::vector<Vectorq6x1> q6dof_solns;
Vectorq6x1 qvec;
double x_des,y_des,z_des;
z_des = 0.3;
x_des = 0.4;
y_des = 0.45;
b_des<<1,0,0;
t_des<<0,1,0;
n_des = t_des.cross(b_des);
Eigen::Matrix3d R_des;
R_des.col(0) = n_des;
R_des.col(1) = t_des;
R_des.col(2) = b_des;
Eigen::Affine3d a_tool_des; // expressed in DH frame
a_tool_des.linear() = R_des;
//ros::Rate sleep_timer(UPDATE_RATE); //a timer for desired rate to send new traj points as commands
trajectory_msgs::JointTrajectory new_trajectory; // an empty trajectory
trajectory_msgs::JointTrajectoryPoint trajectory_point1;
trajectory_msgs::JointTrajectoryPoint trajectory_point2;
// build an example trajectory:
trajectory_point1.positions.clear();
trajectory_point2.positions.clear();
new_trajectory.points.clear();
new_trajectory.joint_names.push_back("joint_1");
new_trajectory.joint_names.push_back("joint_2");
new_trajectory.joint_names.push_back("joint_3");
new_trajectory.joint_names.push_back("joint_4");
new_trajectory.joint_names.push_back("joint_5");
new_trajectory.joint_names.push_back("joint_6");
//specify two points: initially, all home angles
for (int ijnt=0;ijnt<6;ijnt++) {
trajectory_point1.positions.push_back(0.0); // stuff in position commands for 6 joints
//should also fill in trajectory_point.time_from_start
trajectory_point2.positions.push_back(0.0); // stuff in position commands for 6 joints
}
//ros::Duration t_from_start(0); //initialize duration to 0
double t=0.0;
double dt = 3;
trajectory_point1.time_from_start = ros::Duration(0);
trajectory_point2.time_from_start = ros::Duration(2);
new_trajectory.points.push_back(trajectory_point1); // add this single trajectory point to the trajectory vector
new_trajectory.points.push_back(trajectory_point2); // add this single trajectory point to the trajectory vector
new_trajectory.header.stamp = ros::Time::now();
ros::Rate sleep_timer(1.0); //1Hz update rate
Irb120_fwd_solver irb120_fwd_solver;
Irb120_IK_solver ik_solver;
Eigen::Affine3d A_fwd_DH;
ROS_INFO("going home");
//for (int i=0;i<5;i++)
//{
pub.publish(new_trajectory);
ros::spinOnce();
sleep_timer.sleep();
//}
new_trajectory.points.clear();
// start from home pose
new_trajectory.points.push_back(trajectory_point1); // add this single trajectory point to the trajectory vector
new_trajectory.header.stamp = ros::Time::now();
// now see about multiple solutions:
qvec<<0,0,0,0,0,0;
p[0] = x_des;
p[1]= y_des;
p[2] = z_des;
a_tool_des.translation()=p;
//is this point reachable?
int nsolns = ik_solver.ik_solve(a_tool_des);
ROS_INFO("there are %d solutions",nsolns);
ik_solver.get_solns(q6dof_solns);
// if so, try to go there...
for (int isoln = 0; isoln<nsolns;isoln++)
//if (nsolns>0)
{
qvec = q6dof_solns[isoln];
for (int ijnt=0;ijnt<6;ijnt++) {
trajectory_point2.positions[ijnt] = qvec[ijnt];
}
t += dt;
trajectory_point2.time_from_start = ros::Duration(t);
new_trajectory.points.push_back(trajectory_point2); // append another point
t += dt;
trajectory_point1.time_from_start = ros::Duration(t);
new_trajectory.points.push_back(trajectory_point1); // go home between pts
std::cout<<"qsoln = "<<qvec.transpose()<<std::endl;
A_fwd_DH = irb120_fwd_solver.fwd_kin_solve(qvec); //fwd_kin_solve
std::cout << "A rot: " << std::endl;
std::cout << A_fwd_DH.linear() << std::endl;
std::cout << "A origin: " << A_fwd_DH.translation().transpose() << std::endl;
}
int npts = new_trajectory.points.size();
int njnts = new_trajectory.points[0].positions.size();
ROS_INFO("sending a trajectory with %d poses, each with %d joints ",npts,njnts);
pub.publish(new_trajectory);
ros::spinOnce();
sleep_timer.sleep();
return 0;
}