MyLittleTurtle(ros::NodeHandle& nodeHandle): m_nodeHandle(nodeHandle)
{
ROS_INFO("Publishing topic...");
m_turtlesimPublisher = m_nodeHandle.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
ROS_INFO("Subscribing to turtlesim topic...");
m_turtlesimSubscriber = m_nodeHandle.subscribe("/turtle1/pose", 10, &MyLittleTurtle::updateTurtleStatus, this);
ROS_INFO("Waiting for turtlesim...");
ros::Rate loopRate(10);
while (ros::ok() && (m_turtlesimPublisher.getNumSubscribers() <= 0 || m_turtlesimSubscriber.getNumPublishers() <= 0))
loopRate.sleep();
checkRosOk_v();
ROS_INFO("Retrieving initial status...");
ros::spinOnce();
ROS_INFO("Initial status: x=%.3f, y=%.3f, z=%.3f", m_status.x, m_status.y, m_status.z);
ROS_INFO("Everything's ready.");
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "aicroboxi_example_move");
std::string topic = "/cmd_vel";
ros::NodeHandle node;
cmdVelPub = node.advertise<geometry_msgs::Twist>(topic, 1);
ros::Rate loopRate(10);
signal(SIGINT, shutdown);
ROS_INFO("aicrobo_example_move cpp start...");
geometry_msgs::Twist speed; // 控制信号载体 Twist message
while (ros::ok())
{
speed.linear.x = 0.1; // 设置线速度为0.1m/s，正为前进，负为后退
speed.angular.z = 0.4; // 设置角速度为0.4rad/s，正为左转，负为右转
cmdVelPub.publish(speed); // 将刚才设置的指令发送给机器人
loopRate.sleep();
}
return 0;
}
int main (int argc, char** argv)
{
// Ros init
ros::init(argc, argv, "computer_load_monitor");
// Node handler
int update_rate;
ros::NodeHandle nodeHandler("~");
nodeHandler.param<int>("update_rate", update_rate, 5);
ros::Rate loopRate(update_rate);
// loads (CPU/Memory)
ros::Publisher memloadPub = nodeHandler.advertise<msgs::FloatStamped>("/fmInformation/memory_load", 5);
msgs::FloatStamped memMsg;
ros::Publisher cpuloadPub = nodeHandler.advertise<msgs::FloatStamped>("/fmInformation/cpu_load", 5);
msgs::FloatStamped cpuMsg;
// Monitor message
// ROS_INFO(" Monitoring CPU and memory load ...");
while (ros::ok() && nodeHandler.ok())
{
// Publish memory load
memMsg.header.stamp = ros::Time::now();
memMsg.data = getMemoryload(readFile(MEMORY_load));
memloadPub.publish(memMsg); // Output is in range 0.0 to 1.0 as memory load
// Publish cpu load
cpuMsg.header.stamp = ros::Time::now();
cpuMsg.data = getCpuload(readFile(CPU_load));
cpuloadPub.publish(cpuMsg); // Output is in range 0.0 to 1.0 as cpu load since last publish (averaged) across all siblings
loopRate.sleep();
}
return 0;
}
bool AerialManipulationControl::waitforResults(kuri_msgs::Object goal)
{
std::cout << "goal.color "<< goal.color << std::endl << std::flush ;
std::cout << "Constructor of class 2 " << std::endl << std::flush ;
//publish pointcloud msgs:
std::string topic2 = nh_.resolveName("/uav_1/mavros/setpoint_velocity/cmd_vel");
velocity_pub_ = nh_.advertise<geometry_msgs::TwistStamped>(topic2, 1);
img_sub_ = new message_filters::Subscriber<sensor_msgs::Image>(nh_, "/uav_1/downward_cam/camera/image", 10);
camera_info_sub_ = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nh_, "/uav_1/downward_cam/camera/camera_info", 10);
uav_odom_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(nh_, "/uav_1/mavros/local_position/odom", 10);
//initialize base_link to camera optical link
BaseToCamera.setOrigin(tf::Vector3(0.0, 0.0, -0.2));
BaseToCamera.setRotation(tf::Quaternion(0.707, -0.707, 0.000, -0.000));
std::cout << " Start the action server " << std::endl << std::flush ;
#define Ground_Z 0.0
//test
tf::TransformBroadcaster br;
ros::Rate loopRate(10);
while (ros::ok())
{
ros::spinOnce();
loopRate.sleep();
if (this->flag_2)
{
return true ;
std::cout << " return treu" << std::endl << std::flush ;
}
else
{
std::cout << "Continue" << std::endl << std::flush ;
continue ;
}
}
}
void MotorController::runNode(ros::NodeHandle handle) {
ros::Rate loopRate(CONTROL_FREQ);
ras_arduino_msgs::PWM motion; // header is automatically filled
while (ros::ok()) {
std::cout << "left difference: " << (leftAngularVel - leftVelEstimate) << std::endl
<< "right difference: " << (rightAngularVel - rightVelEstimate) << std::endl
<< "left difference + gain: " << LEFT_GAIN * (leftAngularVel - leftVelEstimate) << std::endl
<< "right difference: " << RIGHT_GAIN * (rightAngularVel - rightVelEstimate) << std::endl
<< "Setpoint left: " << leftAngularVel << std::endl
<< "setpoint right: " << rightAngularVel << std::endl;
// compute preliminary PWM values
float prePWM1 = motion.PWM1 + LEFT_GAIN * (leftAngularVel - leftVelEstimate);
float prePWM2 = motion.PWM2 + RIGHT_GAIN * (rightAngularVel - rightVelEstimate);
// if a value exceeds 255, clip it and preserve the ratio
if (prePWM1 > 255 || prePWM2 > 255) {
float largest = std::max(prePWM1, prePWM2);
prePWM1 = (prePWM1/largest) * 255;
prePWM2 = (prePWM2/largest) * 255;
}
motion.PWM1 = prePWM1;
motion.PWM2 = prePWM2;
std::cout << "Publishing PWM1: " << motion.PWM1
<< " PWM2: " << motion.PWM2 << std::endl;
pub_PWM.publish(motion);
ros::spinOnce();
loopRate.sleep();
}
}
// Method which publish position to the pan_tilt.
void run() {
ros::Rate loopRate(50);
trajectory_msgs::JointTrajectory positions;
positions.joint_names.push_back("head_pan_joint");
positions.joint_names.push_back("head_tilt_joint");
positions.points.resize(1);
positions.points[0].positions.resize(2);
positions.points[0].velocities.push_back(0);
positions.points[0].velocities.push_back(0);
while (ros::ok()) {
if (_deadManButtonActive) {
positions.points[0].time_from_start = ros::Duration(0.1);
positions.points[0].positions[0] = _panPos;
positions.points[0].positions[1] = _tiltPos;
#ifdef DEBUG_JOY
ROS_INFO_STREAM("[" << ros::this_node::getName() << "]: " << positions);
#endif
_panTiltCommand.publish(positions);
}
ros::spinOnce();
loopRate.sleep();
}
}
int main(int argc, char **argv)
{
// init ros
ros::init(argc, argv, "vel_center");
MyNodeHandle node;
ros::Subscriber goalVelSub = node.subscribe("/goal_vel", 100, goalVelCallback);
ros::Subscriber goalLinearSub = node.subscribe("/goal_linear", 100, goalLinearCallback);
ros::Subscriber goalAngularSub = node.subscribe("/goal_angular", 100, goalAngularCallback);
ros::Subscriber adjustLinearSub = node.subscribe("/adjust_linear", 100, adjustLinearCallback);
ros::Subscriber adjustAngularSub = node.subscribe("/adjust_angular", 100, adjustAngularCallback);
ros::Subscriber increLinearSub = node.subscribe("/incre_linear", 100, increLinearCallback);
ros::Subscriber increAngularSub = node.subscribe("/incre_angular", 100, increAngularCallback);
ros::Publisher cmdVelPub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 100);
// get params
double minLinearSpeed = node.getParamEx("vel_center/minLinearSpeed", 0.03);
double minAngularSpeed = node.getParamEx("vel_center/minAngularSpeed", 0.15);
double maxLinearSpeed = node.getParamEx("vel_center/maxLinearSpeed", 0.5);
double maxAngularSpeed = node.getParamEx("vel_center/maxAngularSpeed", 2.5);
double minAdjustLinear = node.getParamEx("vel_center/minAdjustLinear", 0.02);
double minAdjustAngular = node.getParamEx("vel_center/minAdjustAngular", 0.04);
double maxAdjustLinear = node.getParamEx("vel_center/maxAdjustLinear", 0.4);
double maxAdjustAngular = node.getParamEx("vel_center/maxAdjustAngular", 1.5);
double linearAccelerate = node.getParamEx("vel_center/linearAccelerate", 0.25); // num / s
double angularAccelerate = node.getParamEx("vel_center/angularAccelerate", 1); // num / s
int rate = node.getParamEx("vel_center/rate", 10);
// start pub loop
ros::Rate loopRate(rate);
double linearSpeed = 0; // current speed
double angularSpeed = 0; // current speed
double calcLinearSpeed = 0; // goal speed after adjust
double calcAngularSpeed = 0; // goal speed after adjust
linearAccelerate /= rate; // accelerate limit per loop
angularAccelerate /= rate; // accelerate limit per loop
while (ros::ok())
{
// calc adjust speed
adjustLinearSpeed += increLinearSpeed;
adjustAngularSpeed += increAngularSpeed;
increLinearSpeed = 0;
increAngularSpeed = 0;
// calc last goal linear speed
if (fabs(adjustLinearSpeed) < minAdjustLinear || fabs(goalLinearSpeed) < minLinearSpeed)
{
calcLinearSpeed = goalLinearSpeed;
}
else if (adjustLinearSpeed > 0) // increase speed
{
if (adjustLinearSpeed > maxAdjustLinear) adjustLinearSpeed = maxAdjustLinear;
calcLinearSpeed = goalLinearSpeed + (goalLinearSpeed > 0 ? adjustLinearSpeed : -adjustLinearSpeed);
}
else // decrease speed
{
if (-adjustLinearSpeed > maxAdjustLinear) adjustLinearSpeed = -maxAdjustLinear;
if (fabs(goalLinearSpeed) - fabs(adjustLinearSpeed) < minLinearSpeed) calcLinearSpeed = minLinearSpeed;
else calcLinearSpeed = goalLinearSpeed + adjustLinearSpeed;
}
// calc last goal angular speed
if (fabs(adjustAngularSpeed) < minAdjustAngular || fabs(goalAngularSpeed) < minAngularSpeed)
{
calcAngularSpeed = goalAngularSpeed;
}
else if (adjustAngularSpeed > 0) // increase speed
{
if (adjustAngularSpeed > maxAdjustAngular) adjustAngularSpeed = maxAdjustAngular;
calcAngularSpeed = goalAngularSpeed + (goalAngularSpeed > 0 ? adjustAngularSpeed : -adjustAngularSpeed);
}
else // decrease speed
{
if (-adjustAngularSpeed > maxAdjustAngular) adjustAngularSpeed = -maxAdjustAngular;
if (fabs(goalAngularSpeed) - fabs(adjustAngularSpeed) < minAngularSpeed) calcAngularSpeed = minAngularSpeed;
else calcAngularSpeed = goalAngularSpeed + adjustAngularSpeed;
}
// verify the max speed
if (calcLinearSpeed > maxLinearSpeed || calcLinearSpeed < -maxLinearSpeed)
{
ROS_WARN("Goal linear speed is too high: [%f]", calcLinearSpeed);
calcLinearSpeed = calcLinearSpeed > 0 ? maxLinearSpeed : -maxLinearSpeed;
}
if (calcAngularSpeed > maxAngularSpeed || calcAngularSpeed < -maxAngularSpeed)
{
ROS_WARN("Goal angular speed is too high: [%f]", calcAngularSpeed);
calcAngularSpeed = calcAngularSpeed > 0 ? maxAngularSpeed : -maxAngularSpeed;
}
// smooth speed change
if (calcLinearSpeed - linearSpeed > linearAccelerate)
linearSpeed += linearAccelerate;
else if (linearSpeed - calcLinearSpeed > linearAccelerate)
linearSpeed -= linearAccelerate;
else
linearSpeed = calcLinearSpeed;
if (calcAngularSpeed - angularSpeed > angularAccelerate)
angularSpeed += angularAccelerate;
else if (angularSpeed - calcAngularSpeed > angularAccelerate)
angularSpeed -= angularAccelerate;
else
angularSpeed = calcAngularSpeed;
// pub msg
geometry_msgs::Twist cmdVel;
cmdVel.linear.x = linearSpeed;
cmdVel.angular.z = angularSpeed;
cmdVelPub.publish(cmdVel);
//ROS_INFO("linear: [%f] angular: [%f]", linearSpeed, angularSpeed);
ros::spinOnce();
loopRate.sleep();
}
return 0;
}
TEST(InterfacesTest, ImuDifferentialIO)
{
ros::NodeHandle nh;
ros::Publisher imu0Pub = nh.advertise<sensor_msgs::Imu>("/imu_input0", 5);
ros::Publisher imu1Pub = nh.advertise<sensor_msgs::Imu>("/imu_input1", 5);
ros::Publisher imuPub = nh.advertise<sensor_msgs::Imu>("/imu_input3", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
sensor_msgs::Imu imu;
imu.header.frame_id = "base_link";
tf2::Quaternion quat;
const double roll = M_PI/2.0;
const double pitch = -M_PI;
const double yaw = -M_PI/4.0;
quat.setRPY(roll, pitch, yaw);
imu.orientation = tf2::toMsg(quat);
imu.orientation_covariance[0] = 0.02;
imu.orientation_covariance[4] = 0.02;
imu.orientation_covariance[8] = 0.02;
imu.angular_velocity_covariance[0] = 0.02;
imu.angular_velocity_covariance[4] = 0.02;
imu.angular_velocity_covariance[8] = 0.02;
size_t setCount = 0;
while (setCount++ < 10)
{
imu.header.stamp = ros::Time::now();
imu0Pub.publish(imu); // Use this to move the absolute orientation
imu1Pub.publish(imu); // Use this to keep velocities at 0
ros::spinOnce();
ros::Duration(0.1).sleep();
imu.header.seq++;
}
size_t zeroCount = 0;
while (zeroCount++ < 10)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
ros::spinOnce();
ros::Duration(0.1).sleep();
imu.header.seq++;
}
double rollFinal = roll;
double pitchFinal = pitch;
double yawFinal = yaw;
// Move the orientation slowly, and see if we
// can push it to 0
ros::Rate loopRate(20);
for (size_t i = 0; i < 100; ++i)
{
yawFinal -= 0.01 * (3.0 * M_PI/4.0);
quat.setRPY(rollFinal, pitchFinal, yawFinal);
imu.orientation = tf2::toMsg(quat);
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
ros::spinOnce();
loopRate.sleep();
imu.header.seq++;
}
ros::spinOnce();
// Move the orientation slowly, and see if we
// can push it to 0
loopRate = ros::Rate(20);
for (size_t i = 0; i < 100; ++i)
{
rollFinal += 0.01 * (M_PI/2.0);
quat.setRPY(rollFinal, pitchFinal, yawFinal);
imu.orientation = tf2::toMsg(quat);
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
ros::spinOnce();
loopRate.sleep();
imu.header.seq++;
}
ros::spinOnce();
tf2::fromMsg(filtered_.pose.pose.orientation, quat);
tf2::Matrix3x3 mat(quat);
mat.getRPY(rollFinal, pitchFinal, yawFinal);
EXPECT_LT(::fabs(rollFinal), 0.2);
EXPECT_LT(::fabs(pitchFinal), 0.2);
EXPECT_LT(::fabs(yawFinal), 0.2);
resetFilter();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "go_and_back");
std::string topic = "/cmd_vel";
ros::NodeHandle node;
//Publisher to control the robot's speed
cmdVelPub = node.advertise<geometry_msgs::Twist>(topic, 1);
//How fast will we update the robot's movement?
double rate = 50;
//Set the equivalent ROS rate variable
ros::Rate loopRate(rate);
//execute a shutdown function when exiting
signal(SIGINT, shutdown);
ROS_INFO("timed_out_and_back.cpp start...");
//Set the forward linear speed to 0.2 meters per second
float linear_speed = 0.2;
//Set the travel distance to 1.0 meters
float goal_distance = 1.0;
//How long should it take us to get there?
float linear_duration = goal_distance / linear_speed;
//Set the rotation speed to 1.0 radians per second
float angular_speed = 1.0;
//Set the rotation angle to Pi radians (180 degrees)
float goal_angle = M_PI;
//How long should it take to rotate?
float angular_duration = goal_angle / angular_speed;
int count = 0;//Loop through the two legs of the trip
int ticks;
geometry_msgs::Twist speed; // 控制信号载体 Twist message
while (ros::ok())
{
speed.linear.x = linear_speed; // 设置线速度，正为前进，负为后退
// Move forward for a time to go 1 meter
ticks = int(linear_duration * rate);
for(int i = 0; i < ticks; i++)
{
cmdVelPub.publish(speed); // 将刚才设置的指令发送给机器人
loopRate.sleep();
}
//Stop the robot before the rotation
cmdVelPub.publish(geometry_msgs::Twist());
//loopRate.sleep();
ROS_INFO("rotation...!");
//Now rotate left roughly 180 degrees
speed.linear.x = 0;
//Set the angular speed
speed.angular.z = angular_speed; // 设置角速度，正为左转，负为右转
//Rotate for a time to go 180 degrees
ticks = int(angular_duration * rate);
for(int i = 0; i < ticks; i++)
{
cmdVelPub.publish(speed); // 将刚才设置的指令发送给机器人
loopRate.sleep();
}
speed.angular.z = 0;
//Stop the robot before the next leg
cmdVelPub.publish(geometry_msgs::Twist());
count++;
if(count == 2)
{
count = 0;
cmdVelPub.publish(geometry_msgs::Twist());
ROS_INFO("timed_out_and_back.cpp ended!");
ros::shutdown();
}
else
{
ROS_INFO("go back...!");
}
}
return 0;
}
TEST(InterfacesTest, OdomDifferentialIO)
{
ros::NodeHandle nh;
ros::Publisher odomPub = nh.advertise<nav_msgs::Odometry>("/odom_input1", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
nav_msgs::Odometry odom;
odom.pose.pose.position.x = 20.0;
odom.pose.pose.position.y = 10.0;
odom.pose.pose.position.z = -40.0;
odom.pose.pose.orientation.w = 1;
odom.pose.covariance[0] = 2.0;
odom.pose.covariance[7] = 2.0;
odom.pose.covariance[14] = 2.0;
odom.pose.covariance[21] = 0.2;
odom.pose.covariance[28] = 0.2;
odom.pose.covariance[35] = 0.2;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";
// No guaranteeing that the zero state
// we're expecting to see here isn't just
// a result of zeroing it out previously,
// so check 10 times in succession.
size_t zeroCount = 0;
while (zeroCount++ < 10)
{
odom.header.stamp = ros::Time::now();
odomPub.publish(odom);
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.x), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.y), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.z), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.w - 1), 0.01);
ros::Duration(0.1).sleep();
odom.header.seq++;
}
for (size_t ind = 0; ind < 36; ind+=7)
{
odom.pose.covariance[ind] = 1e-6;
}
// Slowly move the position, and hope that the
// differential position keeps up
ros::Rate loopRate(20);
for (size_t i = 0; i < 100; ++i)
{
odom.pose.pose.position.x += 0.01;
odom.pose.pose.position.y += 0.02;
odom.pose.pose.position.z -= 0.03;
odom.header.stamp = ros::Time::now();
odomPub.publish(odom);
ros::spinOnce();
loopRate.sleep();
odom.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1), 0.2);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 2), 0.4);
EXPECT_LT(::fabs(filtered_.pose.pose.position.z + 3), 0.6);
resetFilter();
}
TEST(InterfacesTest, PoseDifferentialIO)
{
ros::NodeHandle nh;
ros::Publisher posePub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("/pose_input1", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
geometry_msgs::PoseWithCovarianceStamped pose;
pose.pose.pose.position.x = 20.0;
pose.pose.pose.position.y = 10.0;
pose.pose.pose.position.z = -40.0;
pose.pose.pose.orientation.w = 1;
pose.pose.covariance[0] = 2.0;
pose.pose.covariance[7] = 2.0;
pose.pose.covariance[14] = 2.0;
pose.pose.covariance[21] = 0.2;
pose.pose.covariance[28] = 0.2;
pose.pose.covariance[35] = 0.2;
pose.header.frame_id = "odom";
// No guaranteeing that the zero state
// we're expecting to see here isn't just
// a result of zeroing it out previously,
// so check 10 times in succession.
size_t zeroCount = 0;
while (zeroCount++ < 10)
{
pose.header.stamp = ros::Time::now();
posePub.publish(pose);
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.x), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.y), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.z), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.w - 1), 0.01);
ros::Duration(0.1).sleep();
pose.header.seq++;
}
// ...but only if we give the measurement a tiny covariance
for (size_t ind = 0; ind < 36; ind+=7)
{
pose.pose.covariance[ind] = 1e-6;
}
// Issue this location repeatedly, and see if we get
// a final reported position of (1, 2, -3)
ros::Rate loopRate(20);
for (size_t i = 0; i < 100; ++i)
{
pose.pose.pose.position.x += 0.01;
pose.pose.pose.position.y += 0.02;
pose.pose.pose.position.z -= 0.03;
pose.header.stamp = ros::Time::now();
posePub.publish(pose);
ros::spinOnce();
loopRate.sleep();
pose.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1), 0.2);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 2), 0.4);
EXPECT_LT(::fabs(filtered_.pose.pose.position.z + 3), 0.6);
resetFilter();
}
TEST(InterfacesTest, ImuTwistBasicIO)
{
ros::NodeHandle nh;
ros::Publisher imuPub = nh.advertise<sensor_msgs::Imu>("/imu_input1", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
sensor_msgs::Imu imu;
tf2::Quaternion quat;
imu.angular_velocity.x = (M_PI / 2.0);
for (size_t ind = 0; ind < 9; ind+=4)
{
imu.angular_velocity_covariance[ind] = 1e-6;
}
imu.header.frame_id = "base_link";
ros::Rate loopRate(50);
for (size_t i = 0; i < 50; ++i)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
loopRate.sleep();
ros::spinOnce();
imu.header.seq++;
}
// Now check the values from the callback
tf2::fromMsg(filtered_.pose.pose.orientation, quat);
tf2::Matrix3x3 mat(quat);
double r, p, y;
mat.getRPY(r, p, y);
// Tolerances may seem loose, but the initial state covariances
// are tiny, so the filter is sluggish to accept velocity data
EXPECT_LT(::fabs(r - M_PI / 2.0), 0.7);
EXPECT_LT(::fabs(p), 0.1);
EXPECT_LT(::fabs(y), 0.1);
EXPECT_LT(filtered_.twist.covariance[21], 1e-3);
EXPECT_LT(filtered_.pose.covariance[21], 0.1);
resetFilter();
imu.angular_velocity.x = 0.0;
imu.angular_velocity.y = -(M_PI / 2.0);
// Make sure the pose reset worked. Test will timeout
// if this fails.
loopRate = ros::Rate(50);
for (size_t i = 0; i < 50; ++i)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
loopRate.sleep();
ros::spinOnce();
imu.header.seq++;
}
// Now check the values from the callback
tf2::fromMsg(filtered_.pose.pose.orientation, quat);
mat.setRotation(quat);
mat.getRPY(r, p, y);
EXPECT_LT(::fabs(r), 0.1);
EXPECT_LT(::fabs(p + M_PI / 2.0), 0.7);
EXPECT_LT(::fabs(y), 0.1);
EXPECT_LT(filtered_.twist.covariance[28], 1e-3);
EXPECT_LT(filtered_.pose.covariance[28], 0.1);
resetFilter();
imu.angular_velocity.y = 0;
imu.angular_velocity.z = M_PI / 4.0;
// Make sure the pose reset worked. Test will timeout
// if this fails.
loopRate = ros::Rate(50);
for (size_t i = 0; i < 50; ++i)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
loopRate.sleep();
ros::spinOnce();
imu.header.seq++;
}
// Now check the values from the callback
tf2::fromMsg(filtered_.pose.pose.orientation, quat);
mat.setRotation(quat);
mat.getRPY(r, p, y);
EXPECT_LT(::fabs(r), 0.1);
EXPECT_LT(::fabs(p), 0.1);
EXPECT_LT(::fabs(y - M_PI / 4.0), 0.7);
EXPECT_LT(filtered_.twist.covariance[35], 1e-3);
EXPECT_LT(filtered_.pose.covariance[35], 0.1);
resetFilter();
// Test to see if the angular velocity data is ignored when we set the
// first covariance value to -1
sensor_msgs::Imu imuIgnore;
imuIgnore.angular_velocity.x = 100;
imuIgnore.angular_velocity.y = 100;
imuIgnore.angular_velocity.z = 100;
imuIgnore.angular_velocity_covariance[0] = -1;
loopRate = ros::Rate(50);
for (size_t i = 0; i < 50; ++i)
{
imuIgnore.header.stamp = ros::Time::now();
imuPub.publish(imuIgnore);
loopRate.sleep();
ros::spinOnce();
imuIgnore.header.seq++;
}
tf2::fromMsg(filtered_.pose.pose.orientation, quat);
mat.setRotation(quat);
mat.getRPY(r, p, y);
EXPECT_LT(::fabs(r), 1e-3);
EXPECT_LT(::fabs(p), 1e-3);
EXPECT_LT(::fabs(y), 1e-3);
resetFilter();
}
TEST(InterfacesTest, ImuAccBasicIO)
{
ros::NodeHandle nh;
ros::Publisher imuPub = nh.advertise<sensor_msgs::Imu>("/imu_input2", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
sensor_msgs::Imu imu;
imu.header.frame_id = "base_link";
imu.linear_acceleration_covariance[0] = 1e-6;
imu.linear_acceleration_covariance[4] = 1e-6;
imu.linear_acceleration_covariance[8] = 1e-6;
imu.linear_acceleration.x = 1;
imu.linear_acceleration.y = -1;
imu.linear_acceleration.z = 1;
// Move with constant acceleration for 1 second,
// then check our velocity.
ros::Rate loopRate(50);
for (size_t i = 0; i < 50; ++i)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
loopRate.sleep();
ros::spinOnce();
imu.header.seq++;
}
EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - 1.0), 0.4);
EXPECT_LT(::fabs(filtered_.twist.twist.linear.y + 1.0), 0.4);
EXPECT_LT(::fabs(filtered_.twist.twist.linear.z - 1.0), 0.4);
imu.linear_acceleration.x = 0.0;
imu.linear_acceleration.y = 0.0;
imu.linear_acceleration.z = 0.0;
// Now move for another second, and see where we
// end up
loopRate = ros::Rate(50);
for (size_t i = 0; i < 50; ++i)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
loopRate.sleep();
ros::spinOnce();
imu.header.seq++;
}
EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1.2), 0.4);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 1.2), 0.4);
EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 1.2), 0.4);
resetFilter();
// Test to see if the linear acceleration data is ignored when we set the
// first covariance value to -1
sensor_msgs::Imu imuIgnore;
imuIgnore.linear_acceleration.x = 1000;
imuIgnore.linear_acceleration.y = 1000;
imuIgnore.linear_acceleration.z = 1000;
imuIgnore.linear_acceleration_covariance[0] = -1;
loopRate = ros::Rate(50);
for (size_t i = 0; i < 50; ++i)
{
imuIgnore.header.stamp = ros::Time::now();
imuPub.publish(imuIgnore);
loopRate.sleep();
ros::spinOnce();
imuIgnore.header.seq++;
}
EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 1e-3);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 1e-3);
EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 1e-3);
resetFilter();
}
TEST(InterfacesTest, OdomTwistBasicIO)
{
ros::NodeHandle nh;
ros::Publisher odomPub = nh.advertise<nav_msgs::Odometry>("/odom_input2", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
nav_msgs::Odometry odom;
odom.twist.twist.linear.x = 5.0;
odom.twist.twist.linear.y = 0.0;
odom.twist.twist.linear.z = 0.0;
odom.twist.twist.angular.x = 0.0;
odom.twist.twist.angular.y = 0.0;
odom.twist.twist.angular.z = 0.0;
for (size_t ind = 0; ind < 36; ind+=7)
{
odom.twist.covariance[ind] = 1e-6;
}
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";
ros::Rate loopRate(20);
for (size_t i = 0; i < 400; ++i)
{
odom.header.stamp = ros::Time::now();
odomPub.publish(odom);
ros::spinOnce();
loopRate.sleep();
odom.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), 0.1);
EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 100.0), 2.0);
resetFilter();
odom.twist.twist.linear.x = 0.0;
odom.twist.twist.linear.y = 5.0;
loopRate = ros::Rate(20);
for (size_t i = 0; i < 200; ++i)
{
odom.header.stamp = ros::Time::now();
odomPub.publish(odom);
ros::spinOnce();
loopRate.sleep();
odom.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.twist.twist.linear.y - odom.twist.twist.linear.y), 0.1);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 50.0), 1.0);
resetFilter();
odom.twist.twist.linear.y = 0.0;
odom.twist.twist.linear.z = 5.0;
loopRate = ros::Rate(20);
for (size_t i = 0; i < 100; ++i)
{
odom.header.stamp = ros::Time::now();
odomPub.publish(odom);
ros::spinOnce();
loopRate.sleep();
odom.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.twist.twist.linear.z - odom.twist.twist.linear.z), 0.1);
EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 25.0), 1.0);
resetFilter();
odom.twist.twist.linear.z = 0.0;
odom.twist.twist.linear.x = 1.0;
odom.twist.twist.angular.z = (M_PI/2) / (100.0 * 0.05);
loopRate = ros::Rate(20);
for (size_t i = 0; i < 100; ++i)
{
odom.header.stamp = ros::Time::now();
odomPub.publish(odom);
ros::spinOnce();
loopRate.sleep();
odom.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), 0.1);
EXPECT_LT(::fabs(filtered_.twist.twist.angular.z - odom.twist.twist.angular.z), 0.1);
EXPECT_LT(::fabs(filtered_.pose.pose.position.x - filtered_.pose.pose.position.y), 0.5);
resetFilter();
odom.twist.twist.linear.x = 0.0;
odom.twist.twist.angular.z = 0.0;
odom.twist.twist.angular.x = -(M_PI/2) / (100.0 * 0.05);
// First, roll the vehicle on its side
loopRate = ros::Rate(20);
for (size_t i = 0; i < 100; ++i)
{
odom.header.stamp = ros::Time::now();
odomPub.publish(odom);
ros::spinOnce();
loopRate.sleep();
odom.header.seq++;
}
ros::spinOnce();
odom.twist.twist.angular.x = 0.0;
odom.twist.twist.angular.y = (M_PI/2) / (100.0 * 0.05);
// Now, pitch it down (positive pitch velocity in vehicle frame)
loopRate = ros::Rate(20);
for (size_t i = 0; i < 100; ++i)
{
odom.header.stamp = ros::Time::now();
odomPub.publish(odom);
ros::spinOnce();
loopRate.sleep();
odom.header.seq++;
}
ros::spinOnce();
odom.twist.twist.angular.y = 0.0;
odom.twist.twist.linear.x = 3.0;
// We should now be on our side and facing -Y. Move forward in
// the vehicle frame X direction, and make sure Y decreases in
// the world frame.
loopRate = ros::Rate(20);
for (size_t i = 0; i < 100; ++i)
{
odom.header.stamp = ros::Time::now();
odomPub.publish(odom);
ros::spinOnce();
loopRate.sleep();
odom.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), 0.1);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 15), 1.0);
resetFilter();
}
TEST(InterfacesTest, ImuPoseBasicIO)
{
ros::NodeHandle nh;
ros::Publisher imuPub = nh.advertise<sensor_msgs::Imu>("/imu_input0", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
sensor_msgs::Imu imu;
tf2::Quaternion quat;
quat.setRPY(M_PI/4, -M_PI/4, M_PI/2);
imu.orientation = tf2::toMsg(quat);
for (size_t ind = 0; ind < 9; ind+=4)
{
imu.orientation_covariance[ind] = 1e-6;
}
imu.header.frame_id = "base_link";
// Make sure the pose reset worked. Test will timeout
// if this fails.
ros::Rate loopRate(50);
for (size_t i = 0; i < 50; ++i)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
ros::spinOnce();
loopRate.sleep();
imu.header.seq++;
}
// Now check the values from the callback
tf2::fromMsg(filtered_.pose.pose.orientation, quat);
tf2::Matrix3x3 mat(quat);
double r, p, y;
mat.getRPY(r, p, y);
EXPECT_LT(::fabs(r - M_PI/4), 0.1);
EXPECT_LT(::fabs(p + M_PI/4), 0.1);
EXPECT_LT(::fabs(y - M_PI/2), 0.1);
EXPECT_LT(filtered_.pose.covariance[21], 0.5);
EXPECT_LT(filtered_.pose.covariance[28], 0.25);
EXPECT_LT(filtered_.pose.covariance[35], 0.5);
resetFilter();
// Test to see if the orientation data is ignored when we set the
// first covariance value to -1
sensor_msgs::Imu imuIgnore;
imuIgnore.orientation.x = 0.1;
imuIgnore.orientation.y = 0.2;
imuIgnore.orientation.z = 0.3;
imuIgnore.orientation.w = 0.4;
imuIgnore.orientation_covariance[0] = -1;
loopRate = ros::Rate(50);
for (size_t i = 0; i < 50; ++i)
{
imuIgnore.header.stamp = ros::Time::now();
imuPub.publish(imuIgnore);
loopRate.sleep();
ros::spinOnce();
imuIgnore.header.seq++;
}
tf2::fromMsg(filtered_.pose.pose.orientation, quat);
mat.setRotation(quat);
mat.getRPY(r, p, y);
EXPECT_LT(::fabs(r), 1e-3);
EXPECT_LT(::fabs(p), 1e-3);
EXPECT_LT(::fabs(y), 1e-3);
resetFilter();
}
int main(int argc, char **argv) {
ros::init(argc, argv, "pan_tilt_api");
ros::NodeHandle nh;
pan_tilt_pub = nh.advertise<std_msgs::Float64MultiArray>("pan_tilt_controller/command", 10);
ros::Subscriber sub = nh.subscribe ("joint_states", 10, callback);
double max_rate,kp_pan,kp_tilt,kd_pan,kd_tilt,pre_pan_err=0,pre_tilt_err=0,target_tol;
std::string object_frame,depth_camera_frame;
double no_object_timeout;
double loop_hz;
nh.param<double>("max_rate", max_rate, 0.3); //rad/s
nh.param<std::string>("object_frame", object_frame, "object_frame");
nh.param<double>("kp_pan", kp_pan, 4.0);
nh.param<double>("kp_tilt", kp_tilt, 3.0);
nh.param<double>("kd_pan", kd_pan, 0.01);
nh.param<double>("kd_tilt", kd_tilt, 0.01);
nh.param<double>("loop_hz", loop_hz, 30);
nh.param<double>("target_tol", target_tol,1.0*M_PI/180.0);
nh.param<double>("no_object_timeout", no_object_timeout,5.0);
nh.param<std::string>("depth_camera_frame", depth_camera_frame, "kinect2_depth_optical_frame");
tf::TransformListener listener;
tf::StampedTransform transform;
ros::Duration dt;
ros::Rate loopRate(loop_hz);
ros::Time pre_t=ros::Time::now();
while (!listener.waitForTransform(depth_camera_frame, object_frame,ros::Time::now(), ros::Duration(1.0))) {
ROS_INFO("Waiting for tranformations...");
}
ROS_INFO("Ready to track!");
bool first=true;
while(ros::ok()) {
if ((have_tilt)&&(have_pan)) {
try {
listener.lookupTransform(depth_camera_frame, object_frame, ros::Time(0), transform);
ros::Duration d=ros::Time::now()-transform.stamp_;
if (d.toSec()>no_object_timeout) { //no object in last 3 seconds
reset();
continue;
}
tf::Vector3 v=transform.getOrigin();
double pan_err=-atan2(v.x(),v.z());
double tilt_err=atan2(v.y(),v.z());
if (first) {
pre_pan_err=pan_err;
pre_tilt_err=tilt_err;
first=false;
}
ros::Time now=ros::Time::now();
dt=(now-pre_t);
double pan_rate=pan_err*kp_pan+kd_pan*(pan_err-pre_pan_err)/dt.toSec();
double tilt_rate=tilt_err*kp_tilt+kd_tilt*(tilt_err-pre_tilt_err)/dt.toSec();;
if (pan_rate>max_rate) pan_rate=max_rate;
else if(pan_rate<-max_rate) pan_rate=-max_rate;
if (tilt_rate>max_rate) tilt_rate=max_rate;
else if(tilt_rate<-max_rate) tilt_rate=-max_rate;
if(fabs(pan_err)<target_tol) pan_rate=0;
if(fabs(tilt_err)<target_tol) tilt_rate=0;
pre_t=now;
std_msgs::Float64MultiArray multiArray;
double pan_cmd=pan_position+pan_rate*dt.toSec();
double tilt_cmd=tilt_position+tilt_rate*dt.toSec();
multiArray.data.push_back(pan_cmd); // pan (positive is left)
multiArray.data.push_back(tilt_cmd); // tilt (positive is down)
pan_tilt_pub.publish(multiArray);
}
catch (tf::TransformException ex) {
ROS_ERROR("Pan-Tilt tracking node error: %s",ex.what());
reset();
}
}
//
ros::spinOnce();
loopRate.sleep();
}
return 0;
}
int main(int argc, char **argv)
{
ROS_INFO("Starting keyboard_node");
ROS_INFO("Press Ctrl-C to kill node.");
// Start up ROS
ros::init(argc, argv, "keyboard_node");
ros::NodeHandle node;
ros::Publisher publisher = node.advertise<roboteq::MotorCommands>("motorSpeeds", 1);
ros::Rate loopRate(100);
int leftMotor = 0;
int rightMotor = 0;
int loopsSinceInput = 0;
while(ros::ok())
{
int key = getKey();
roboteq::MotorCommands msg;
/*
Up > 65
Down > 66
Right > 67
Left > 68
*/
if(key == 65 || key == 66 || key == 67 || key == 68)
{
if(key == 65)
{
msg.leftMotor += 100;
msg.rightMotor += 100;
}
if(key == 66)
{
msg.leftMotor -= 100;
msg.rightMotor -= 100;
}
if(key == 67)
{
msg.leftMotor += 100;
msg.rightMotor -= 100;
}
if(key == 68)
{
msg.leftMotor -= 100;
msg.rightMotor += 100;
}
// Clamp to -100 & 100
if(msg.leftMotor > 100)
{
msg.leftMotor = 100;
}
else if(msg.leftMotor < -100)
{
msg.leftMotor = -100;
}
if(msg.rightMotor > 100)
{
msg.rightMotor = 100;
}
else if(msg.rightMotor < -100)
{
msg.rightMotor = -100;
}
ROS_INFO("Sending MotorCommands: %i %i", msg.leftMotor, msg.rightMotor);
publisher.publish(msg);
loopsSinceInput = 0;
}
else if(key == -1)
{
// Nothing was pressed
if(loopsSinceInput > 25)
{
msg.leftMotor = 0;
msg.rightMotor = 0;
ROS_INFO("No user input. Stopping motors.");
publisher.publish(msg);
loopsSinceInput = 0;
}
loopsSinceInput++;
}
ros::spinOnce();
loopRate.sleep();
}
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "odom_pcl_limits_node");
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<apc_msgs::SetLimits>("apc/passthrough_filter/setLimits");
apc_msgs::SetLimits srv_msg;
std::string camera_link = "kinect_camera";
std::string origin_link = "odom_origin";
double timeOut = 0.5;
tf::TransformListener listener;
int Hz = 10;
ros::Rate loopRate(Hz);
/************************ Initialize Marker ************************/
ros::Publisher marker_pub_odom = n.advertise<visualization_msgs::MarkerArray>("apc/shelf_odom", 100);
ros::Publisher marker_pub_camera = n.advertise<visualization_msgs::MarkerArray>("apc/shelf_camera", 100);
visualization_msgs::MarkerArray marker_array_msg;
marker_array_msg.markers.resize(4);
for ( int i = 0; i < 4; i++)
{
marker_array_msg.markers[i].header.frame_id = origin_link;
// Set the namespace and id for this marker. This serves to create a unique ID
// Any marker sent with the same namespace and id will overwrite the old one
marker_array_msg.markers[i].ns = "apc_shelf";
marker_array_msg.markers[i].id = i;
// Set the marker type.
marker_array_msg.markers[i].type = visualization_msgs::Marker::CUBE;
// Set the marker action. Options are ADD and DELETE
marker_array_msg.markers[i].action = visualization_msgs::Marker::ADD;
// Set the scale of the marker -- 1x1x1 here means 1m on a side
marker_array_msg.markers[i].scale.x = 0.02;
marker_array_msg.markers[i].scale.y = 0.02;
marker_array_msg.markers[i].scale.z = 0.02;
// Set the color -- be sure to set alpha to something non-zero!
marker_array_msg.markers[i].color.r = 0.0f;
marker_array_msg.markers[i].color.g = 1.0f;
marker_array_msg.markers[i].color.b = 0.0f;
marker_array_msg.markers[i].color.a = 0.8;
marker_array_msg.markers[i].lifetime = ros::Duration(3.0/Hz); // how long this marker should stick around before being automatically deleted
}
// Lower left
marker_array_msg.markers[0].pose.position.x = 0.0;
marker_array_msg.markers[0].pose.position.y = 0.0;
marker_array_msg.markers[0].pose.position.z = 0.82;
marker_array_msg.markers[0].pose.orientation.x = 1.0;
// Lower right
marker_array_msg.markers[1].pose.position.x = 0.0;
marker_array_msg.markers[1].pose.position.y = -0.81;
marker_array_msg.markers[1].pose.position.z = 0.82;
marker_array_msg.markers[1].pose.orientation.x = 1.0;
// Upper left
marker_array_msg.markers[2].pose.position.x = 0.0;
marker_array_msg.markers[2].pose.position.y = 0.0;
marker_array_msg.markers[2].pose.position.z = 1.77;
marker_array_msg.markers[2].pose.orientation.x = 1.0;
// Upper right
marker_array_msg.markers[3].pose.position.x = 0.0;
marker_array_msg.markers[3].pose.position.y = -0.81;
marker_array_msg.markers[3].pose.position.z = 1.77;
marker_array_msg.markers[3].pose.orientation.x = 1.0;
visualization_msgs::MarkerArray marker_array_msg_camera_frame = marker_array_msg;
for ( int i = 0; i < 4; i++)
{
marker_array_msg_camera_frame.markers[i].header.frame_id = camera_link;
marker_array_msg.markers[i].color.r = 1.0f;
marker_array_msg.markers[i].color.g = 0.0f;
}
/**********************************************************************/
//wait for the listener to get the first message
ROS_INFO("Waiting for origin->camera transform ...");
if( !listener.waitForTransform(origin_link,camera_link, ros::Time(0), ros::Duration(5.0)) )
{
ROS_WARN("Could not find transform!");
}
else
{
ROS_INFO("Found transform!");
}
while (n.ok())
{
/* Publish Markers */
for ( int i = 0; i < 4; i++)
{
geometry_msgs::PoseStamped marker_point;
marker_point.header.frame_id = marker_array_msg.markers[i].header.frame_id;
marker_point.pose = marker_array_msg.markers[i].pose;
geometry_msgs::PoseStamped transformed_point;
try
{
listener.transformPose("kinect_camera", marker_point, transformed_point);
}
catch(tf::TransformException& ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(timeOut).sleep();
continue;
}
marker_array_msg_camera_frame.markers[i].pose = transformed_point.pose;
marker_array_msg_camera_frame.markers[i].id += 4;
marker_array_msg_camera_frame.markers[i].id %= 100;
}
for ( int i = 0; i < 4; i++)
{
marker_array_msg_camera_frame.markers[i].header.stamp = ros::Time::now();
marker_array_msg.markers[i].header.stamp = ros::Time::now();
}
//publish in kinect frame
marker_pub_camera.publish(marker_array_msg_camera_frame);
marker_pub_odom.publish(marker_array_msg);
/* Publish Filter limits */
geometry_msgs::PointStamped min_point;
min_point.header.frame_id = "odom_origin";
min_point.point.x = marker_array_msg.markers[0].pose.position.x - 0.15;
min_point.point.y = marker_array_msg.markers[1].pose.position.y - 0.15;
min_point.point.z = marker_array_msg.markers[0].pose.position.x - 0.15;
geometry_msgs::PointStamped transformed_min_point;
try
{
listener.transformPoint(camera_link, min_point, transformed_min_point);
}
catch(tf::TransformException& ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(timeOut).sleep();
continue;
}
geometry_msgs::PointStamped max_point;
max_point.header.frame_id = "odom_origin";
max_point.point.x = marker_array_msg.markers[0].pose.position.x + 0.65;
max_point.point.y = marker_array_msg.markers[0].pose.position.y + 0.15;
max_point.point.z = marker_array_msg.markers[2].pose.position.z + 0.15;
geometry_msgs::PointStamped transformed_max_point;
try
{
listener.transformPoint(camera_link, max_point, transformed_max_point);
}
catch(tf::TransformException& ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(timeOut).sleep();
continue;
}
srv_msg.request.x_min = transformed_max_point.point.x;
srv_msg.request.y_min = transformed_max_point.point.y;
srv_msg.request.z_min = transformed_min_point.point.z;
srv_msg.request.x_max = transformed_min_point.point.x;
srv_msg.request.y_max = transformed_min_point.point.y;
srv_msg.request.z_max = transformed_max_point.point.z;
std::cout<<srv_msg.request.x_min<<", "<<srv_msg.request.x_max<<"\n";
std::cout<<srv_msg.request.y_min<<", "<<srv_msg.request.y_max<<"\n";
std::cout<<srv_msg.request.z_min<<", "<<srv_msg.request.z_max<<"\n\n";
if (!client.call(srv_msg))
{
ROS_ERROR("Failed to call service apc/passthrough_filter/setLimits");
}
loopRate.sleep();
//ros::Duration(10.0).sleep();
}
ROS_INFO("Shutting down ... ");
ros::shutdown();
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "kinect_tf_publisher_node");
ros::NodeHandle n;
ros::ServiceClient getModelState_client = n.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
gazebo_msgs::GetModelState getmodelstate;
getmodelstate.request.model_name = "kinect_1";
getmodelstate.request.relative_entity_name = "baxter::base";
tf::TransformBroadcaster broadcaster;
int Hz = 50;
ros::Rate loopRate(Hz);
int count = 0;
while (n.ok())
{
if(!getModelState_client.call(getmodelstate))
{
ROS_ERROR("Failed to get model state");
}
else
{
tf::Transform tf_kinect;
tf_kinect.setOrigin(tf::Vector3(getmodelstate.response.pose.position.x,
getmodelstate.response.pose.position.y,
getmodelstate.response.pose.position.z));
tf_kinect.setRotation(tf::Quaternion(0.5,-0.5,0.5,-0.5)); // RPY = 0, 90, -90
/*
Get model state returns this instead:
x: 4.16362733088e-07
y: -2.39460619187e-06
z: -0.0144914886547
w: 0.999894992862
so the code below doesn't work:
*/
// tf_kinect.setRotation(tf::Quaternion(getmodelstate.response.pose.orientation.x, // FIXME for some reason this gives the wrong orientation
// getmodelstate.response.pose.orientation.y,
// getmodelstate.response.pose.orientation.z,
// getmodelstate.response.pose.orientation.w));
broadcaster.sendTransform(tf::StampedTransform(tf_kinect, ros::Time::now(), "base", "base_link"));
}
loopRate.sleep();
++count;
}
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "print_state_node");
ros::NodeHandle n;
std::string robot_frame = "base_link";
if (argc > 1)
{
robot_frame = argv[1];
}
ROS_INFO_STREAM("Frame id is: " << robot_frame );
ros::ServiceClient srv_get_joints_ = n.serviceClient<apc_msgs::ReturnJointStates>("return_joint_states");
if( !srv_get_joints_.waitForExistence(ros::Duration(1.0) ) )
ROS_ERROR("Joint state listener not running! Please run \n rosrun apc_robot joint_states_listener.py");
listenerPtr = new tf::TransformListener();
std::string r_controller_name = "r_arm_controller_loose";
std::string l_controller_name = "l_arm_controller_loose";
std::string r_frame = "r_gripper_tool_frame";
std::string l_frame = "l_gripper_tool_frame";
joints_right.request.name.resize(0);
joints_right.request.name.push_back( "r_shoulder_pan_joint" );
joints_right.request.name.push_back( "r_shoulder_lift_joint" );
joints_right.request.name.push_back( "r_upper_arm_roll_joint" );
joints_right.request.name.push_back( "r_elbow_flex_joint" );
joints_right.request.name.push_back( "r_forearm_roll_joint" );
joints_right.request.name.push_back( "r_wrist_flex_joint" );
joints_right.request.name.push_back( "r_wrist_roll_joint" );
joints_left.request.name.resize(0);
joints_left.request.name.push_back( "l_shoulder_pan_joint" );
joints_left.request.name.push_back( "l_shoulder_lift_joint" );
joints_left.request.name.push_back( "l_upper_arm_roll_joint" );
joints_left.request.name.push_back( "l_elbow_flex_joint" );
joints_left.request.name.push_back( "l_forearm_roll_joint" );
joints_left.request.name.push_back( "l_wrist_flex_joint" );
joints_left.request.name.push_back( "l_wrist_roll_joint" );
tf::StampedTransform transform;
int Hz = 50;
ros::Rate loopRate(Hz);
// Wait for the listener to get the first message
if(! listenerPtr->waitForTransform(l_frame, robot_frame, ros::Time(0), ros::Duration(3.0)) )
ROS_ERROR("Could not fprintTFind source->target transform!");
if(! listenerPtr->waitForTransform(r_frame, robot_frame, ros::Time(0), ros::Duration(3.0)) )
ROS_ERROR("Could not fprintTFind source->target transform!");
std::string tmp;
bool done = false;
while(ros::ok() && !done)
{
std::cout<<"Press [enter] to get joint state (q to quit):";
std::getline(std::cin, tmp);
if(tmp == "q")
{
done=true;
}
else
{
if( getTF(r_frame, robot_frame, transform) )
printTF(transform, RIGHT);
if( getTF(l_frame, robot_frame, transform) )
printTF(transform, LEFT);
}
std::cout<<"-----------------------\n";
if(srv_get_joints_.call(joints_left))
{
std::cout<<"left: = [";
for(int k = 0;k<7;k++)
{
std::cout<<joints_left.response.position[k];
if(k<6)
std::cout<<",";
else
std::cout<<"];\n";
}
}
if(srv_get_joints_.call(joints_right))
{
std::cout<<"right: = [";
for(int k = 0;k<7;k++)
{
std::cout<<joints_right.response.position[k];
if(k<6)
std::cout<<",";
else
std::cout<<"];\n";
}
}
std::cout<<"\n###############################\n";
ROS_INFO("");
}
ROS_INFO("Shutting down ... ");
ros::shutdown();
return 0;
}