void cmdVelReceived(const geometry_msgs::Twist::ConstPtr& cmd_vel)
{
if (!command_tested) {
ROS_ERROR_ONCE("Test of robot base driver is not completed. Can't move base.");
return;
};
double lin = cmd_vel->linear.x;
double ang = cmd_vel->angular.z;
if (lin > max_lin) lin = max_lin;
if (lin < -max_lin) lin = -max_lin;
if (ang > max_ang) lin = max_ang;
if (ang < -max_ang) ang = -max_ang;
//double dt = ros::Time::now().toSec() - latest_command.toSec();
g_lin = lin;
g_ang = ang;
latest_command = ros::Time::now();
}
double WholeBodyController::getJointPosition(const std::string& joint_name) const
{
std::map<std::string,unsigned int>::const_iterator index_iter = joint_name_to_index_.find(joint_name);
if (index_iter != joint_name_to_index_.end())
{
return q_current_(index_iter->second);
} else {
ROS_ERROR_ONCE("Joint %s is not listed",joint_name.c_str());
return 0;
}
}
///////////////////////////////////////////////////////////////////////////////////////////
//Function: makes publisher and subscriber and read the params for this node.
//pre: -
//post:
///////////////////////////////////////////////////////////////////////////////////////////
Subscriber::Subscriber(){
// Read joy topic
sub_VelSub = nh_.subscribe<sensor_msgs::Joy>(SUB_TOPIC_NAME, SUB_TOPIC_BUFFER_SIZE, &Subscriber::joyCallback, this);
// Publish Twist message with motorspeed
pub_VelPub = nh_.advertise<geometry_msgs::Twist>(PUB_TOPIC_NAME, PUB_TOPIC_BUFFER_SIZE);
//define values for safety
dScaleAxisX = 0;
dScaleAxisY = 0;
dScaleAxisZ = 0;
//Check params x-axis, y-axis, z-axis
if (nh_.hasParam("iAxisX"),nh_.hasParam("iAxisY"), nh_.hasParam("iAxisZ"))
{
nh_.getParam("iAxisX",iAxisX);
nh_.getParam("iAxisY",iAxisY);
nh_.getParam("iAxisZ",iAxisZ);
ROS_INFO("The motorparameters are initialized with values: Motor1: %i, Motor2: %i, Motor3: %i.", iAxisX, iAxisY, iAxisZ);
}
else
{
ROS_ERROR_ONCE("The motorparameter(s) cannot be initialized");
iAxisX=0; iAxisY=0, iAxisZ=0;
}
//Check params scaler x, y, z
if (nh_.hasParam("dScaleAxisX"),nh_.hasParam("dScaleAxisY"),nh_.hasParam("dScaleAxisZ"))
{
nh_.getParam("dScaleAxisX",dScaleAxisX);
nh_.getParam("dScaleAxisY",dScaleAxisY);
nh_.getParam("dScaleAxisZ",dScaleAxisZ);
ROS_INFO("The scalerparameters are initialized with values: Scaler1: %f, Scaler2: %f, Scaler3: %f.", dScaleAxisX, dScaleAxisY, dScaleAxisZ);
}
else
{
ROS_ERROR_ONCE("The scalerparameter(s) cannot be initialized");
dScaleAxisX=0; dScaleAxisY=0, dScaleAxisZ=0;
}
}
void WholeBodyController::setMeasuredJointPosition(const std::string& joint_name, double pos)
{
std::map<std::string,unsigned int>::iterator index_iter = joint_name_to_index_.find(joint_name);
if (index_iter != joint_name_to_index_.end())
{
unsigned int index = index_iter->second;
q_current_(index) = pos;
//std::cout << joint_name <<"\t"<<q_current_(index) <<"\t"<<index<< std::endl;
robot_state_.tree_.rearrangeJntArrayToTree(q_current_);
//robot_state_.collectFKSolutions();
}
else
{
ROS_ERROR_ONCE("Joint %s is not listed",joint_name.c_str());
}
}
void
pcl_ros::PCDReader::onInit ()
{
ros::NodeHandle private_nh = getMTPrivateNodeHandle ();
// Provide a latched topic
ros::Publisher pub_output = private_nh.advertise<PointCloud2> ("output", max_queue_size_, true);
private_nh.getParam ("publish_rate", publish_rate_);
private_nh.getParam ("tf_frame", tf_frame_);
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - publish_rate : %f\n"
" - tf_frame : %s",
getName ().c_str (),
publish_rate_, tf_frame_.c_str ());
PointCloud2Ptr output_new;
output_ = boost::make_shared<PointCloud2> ();
output_new = boost::make_shared<PointCloud2> ();
// Wait in a loop until someone connects
do
{
ROS_DEBUG_ONCE ("[%s::onInit] Waiting for a client to connect...", getName ().c_str ());
ros::spinOnce ();
ros::Duration (0.01).sleep ();
}
while (private_nh.ok () && pub_output.getNumSubscribers () == 0);
std::string file_name;
while (private_nh.ok ())
{
// Get the current filename parameter. If no filename set, loop
if (!private_nh.getParam ("filename", file_name_) && file_name_.empty ())
{
ROS_ERROR_ONCE ("[%s::onInit] Need a 'filename' parameter to be set before continuing!", getName ().c_str ());
ros::Duration (0.01).sleep ();
ros::spinOnce ();
continue;
}
// If the filename parameter holds a different value than the last one we read
if (file_name_.compare (file_name) != 0 && !file_name_.empty ())
{
NODELET_INFO ("[%s::onInit] New file given: %s", getName ().c_str (), file_name_.c_str ());
file_name = file_name_;
PointCloud2 cloud;
if (impl_.read (file_name_, cloud) < 0)
{
NODELET_ERROR ("[%s::onInit] Error reading %s !", getName ().c_str (), file_name_.c_str ());
return;
}
output_ = boost::make_shared<PointCloud2> (cloud);
output_->header.stamp = ros::Time::now ();
output_->header.frame_id = tf_frame_;
}
// We do not publish empty data
if (output_->data.size () == 0)
continue;
if (publish_rate_ == 0)
{
if (output_ != output_new)
{
NODELET_DEBUG ("Publishing data once (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ());
pub_output.publish (output_);
output_new = output_;
}
ros::Duration (0.01).sleep ();
}
else
{
NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ());
output_->header.stamp = ros::Time::now ();
pub_output.publish (output_);
ros::Duration (publish_rate_).sleep ();
}
ros::spinOnce ();
// Update parameters from server
private_nh.getParam ("publish_rate", publish_rate_);
private_nh.getParam ("tf_frame", tf_frame_);
}
}
int main(int argc, char **argv)
{
//----------------------------------------------------------------
//Init
//----------------------------------------------------------------
uav_detected = false;
ros::init(argc, argv, "control_node");
ros::NodeHandle nh;
ros::Rate rate(updateRate);
//Create subscribers and publichers//
#ifdef SIMULATION
//Subscripe to MAVROS local pose
ros::Subscriber estimated_pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose",1,pose_cb);
#else
//Subscribe to pose estimater
ros::Subscriber estimated_pose_sub = nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/mavros/vision_pose/pose_cov",1,pose_cb);
#endif
//Subscribe to UAV state
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
//Subscribe to UAV extended sshift + 'D'tate
ros::Subscriber extended_state_sub = nh.subscribe<mavros_msgs::ExtendedState>("mavros/extended_state", 10, extended_state_cb);
//Create published for publiching setpoints
local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local",1); //Queue set to one (Allways publich the newest!)
//Create service client for setting UAV state
set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
//init boolean for streaming commands enabled/disabled
ros::ServiceClient land_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/land");
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming");
mavros_msgs::CommandBool disarm_cmd;
disarm_cmd.request.value = false;
mavros_msgs::CommandBool land_cmd;
land_cmd.request.value = true;
streaming_setpoints = false;
//----------------------------------------------------------------
//Wait for FCU connection and for the UAV to be armed
//----------------------------------------------------------------
//wait for FCU connection
ROS_INFO("Waiting for FCU connection");
while(current_state.connected)
{
ros::spinOnce();
rate.sleep();
if(!ros::ok())
{
ROS_INFO("Shutting down control node");
return 0;
}
}
ROS_INFO("Connected to FCU");
//wait for UAV to be armed
ROS_INFO("Wating for UAV to be armed");
while(!current_state.armed )
{
ros::spinOnce();
rate.sleep();
if(!ros::ok())
{
ROS_INFO("Shutting down control node");
return 0;
}
}
ROS_INFO("UAV is armed");
//ROS_INFO("Press and hold \"space\" to trigger failsafe");
ROS_INFO("Press and hold \"space\" to trigger KILLSWITCH! (disarm immediately)");
//----------------------------------------------------------------
//State machine
//----------------------------------------------------------------
int state = IDLE;
int next_state = IDLE;
bool statePrinted = false;
while(ros::ok())
{
if(state != next_state)
{
state = next_state;
statePrinted = false;
}
int c = getch();
//if(32 == c)
//{
// state = FAILSAFE;
// next_state = FAILSAFE;
//}
if(32 == c)
{
state = KILLSWITCH;
next_state = KILLSWITCH;
}
switch(state)
{
case IDLE://Wait for user input
if(!statePrinted)
{
ROS_INFO("STATE: ILDE");
ROS_INFO_ONCE("Press and hold 's' to start system");
statePrinted = true;
}
if('s' == getch())
{
next_state = SEARCHING;
}
break;
case SEARCHING:
if(!statePrinted)
{
ROS_INFO("STATE: SEARCHING");
ROS_INFO("Searching for UAV");
statePrinted = true;
}
//WAIT FOR UAV TO BE DETECTED
if(uav_detected)
ROS_INFO("UAV detected");
next_state = START_SETPOINT_STREAM;
break;
case START_SETPOINT_STREAM:
if(!statePrinted)
{
ROS_INFO("STATE: START_SETPOINT_STREAM");
statePrinted = true;
}
if(start_setpoint_stream())
next_state = ENABLE_OFFBOARD;
break;
case ENABLE_OFFBOARD:
if(!statePrinted)
{
ROS_INFO("STATE: ENABLE_OFFBOARD");
ROS_INFO("Enableling offboard mode");
statePrinted = true;
}
if(enable_offboard())
{
ROS_INFO("Offboard mode enabled");
next_state = WAIT_FOR_POS_REACHED;
}
else
ROS_INFO("Failed to enable offboard mode");
break;
case WAIT_FOR_POS_REACHED:
if(!statePrinted)
{
ROS_INFO("STATE: WAIT_FOR_POS_REACHED");
ROS_INFO("Waiting for UAV to center over platform");
statePrinted = true;
}
//Go to descend if within 0.05 meters of setpoint
if(eu_dist_to_target_point() < MaxDistToTargetPoint)
{
ROS_INFO("UAV centered over platform");
next_state = DESCEND;
}
break;
case DESCEND:
if(!statePrinted)
{
ROS_INFO("STATE: DESCEND");
ROS_INFO("Staring to descend");
statePrinted = true;
}
if(z_dist() < DisarmHeigh)
{
ROS_INFO("Disarm height reached");
ROS_INFO("Disarming");
if( arming_client.call(disarm_cmd) &&
disarm_cmd.response.success){
next_state = DISARMED;
}
}
else if(eu_dist_to_center() < MaxDistToCenterXYPlane)
{
descend();
}
else
{
ROS_WARN("UAV moved away form center");
ROS_INFO("Retrying landing");
next_state = WAIT_FOR_POS_REACHED;
}
break;
case DISARMED:
if(!statePrinted)
{
ROS_INFO("STATE: DISARMED");
statePrinted = true;
}
if(extended_current_state.landed_state == mavros_msgs::ExtendedState::LANDED_STATE_ON_GROUND)
{
ROS_INFO("Landing detected");
next_state = LANDED;
}
break;
case LANDED:
if(!statePrinted)
{
ROS_INFO("STATE: LANDED");
ROS_INFO("Landing compleate");
ROS_WARN("Control_node shutting down");
statePrinted = true;
return 0;
}
break;
case FAILSAFE:
//Land
ROS_ERROR_ONCE("STOPPED STREAMING SETPOINT! THIS WILL TRIGGER THE FAILSAFE");
ROS_ERROR_ONCE("PRESS AND HOLD shift + 'D' TO DISARM UAV NOW!");
streaming_setpoints = false;
if(extended_current_state.landed_state == mavros_msgs::ExtendedState::LANDED_STATE_ON_GROUND)
{
ROS_INFO_ONCE("Landing detected");
next_state = LANDED;
}
break;
case KILLSWITCH: //DISARM THE UAV REGARDLESS OF STATE
ROS_FATAL_ONCE("KILLSWITCH ENABLED! UAV WILL BE DISARMED IMMEDIATELY!");
if( arming_client.call(disarm_cmd) &&
disarm_cmd.response.success){
ROS_FATAL_ONCE("UAV DISARMED!");
}
break;
}
if(streaming_setpoints)
{
ROS_INFO_ONCE("Streaming target positions");
local_pos_pub.publish(TargetPos);
}
ros::spinOnce();
rate.sleep();
}
//----------------------------------------------------------------
//Shutdown
//----------------------------------------------------------------
ROS_INFO("control_node shutting down");
return 0;
}
void StereoUndistort::processAndSendImage(
const sensor_msgs::ImageConstPtr& image_msg_in, const CameraSide& side) {
cv_bridge::CvImageConstPtr image_in_ptr =
cv_bridge::toCvShare(image_msg_in, output_image_type_);
std::string encoding = image_in_ptr->encoding;
if (encoding == "8UC1") {
// ros does not recognize U8C1 and it will crash the disparity node
encoding = "mono8";
}
cv_bridge::CvImagePtr image_out_ptr(
new cv_bridge::CvImage(image_in_ptr->header, encoding));
updateUndistorter(side);
if (side == CameraSide::FIRST) {
image_out_ptr->header.frame_id = first_output_frame_;
first_undistorter_ptr_->undistortImage(image_in_ptr->image,
&(image_out_ptr->image));
first_image_pub_.publish(*(image_out_ptr->toImageMsg()));
if (publish_tf_) {
Eigen::Matrix4d T =
stereo_camera_parameters_ptr_->getFirst().getOutputPtr()->T();
tf::Matrix3x3 R_ros;
tf::Vector3 p_ros;
tf::matrixEigenToTF(T.topLeftCorner<3, 3>(), R_ros);
tf::vectorEigenToTF(T.topRightCorner<3, 1>(), p_ros);
tf::Transform(R_ros, p_ros);
std::string frame = image_in_ptr->header.frame_id;
if (rename_input_frame_) {
frame = first_input_frame_;
}
if (frame.empty()) {
ROS_ERROR_ONCE("Image frame name is blank, cannot construct tf");
} else {
br_.sendTransform(tf::StampedTransform(tf::Transform(R_ros, p_ros),
image_out_ptr->header.stamp,
frame, first_output_frame_));
}
}
} else {
image_out_ptr->header.frame_id = second_output_frame_;
second_undistorter_ptr_->undistortImage(image_in_ptr->image,
&(image_out_ptr->image));
second_image_pub_.publish(*(image_out_ptr->toImageMsg()));
if (publish_tf_) {
Eigen::Matrix4d T =
stereo_camera_parameters_ptr_->getSecond().getOutputPtr()->T();
tf::Matrix3x3 R_ros;
tf::Vector3 p_ros;
tf::matrixEigenToTF(T.topLeftCorner<3, 3>(), R_ros);
tf::vectorEigenToTF(T.topRightCorner<3, 1>(), p_ros);
tf::Transform(R_ros, p_ros);
std::string frame = image_in_ptr->header.frame_id;
if (rename_input_frame_) {
frame = second_input_frame_;
}
if (frame.empty()) {
ROS_ERROR_ONCE("Image frame name is blank, cannot construct tf");
} else {
br_.sendTransform(tf::StampedTransform(tf::Transform(R_ros, p_ros),
image_out_ptr->header.stamp,
frame, second_output_frame_));
}
}
}
}
