<commit_before>#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/PointCloud.h>
#include <math.h>

static const float tol = 0.000000000000001f;

double magnitude(double vec[3]){
    return sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]);
}

void normalize(double vec[3]){
    float m = magnitude(vec);
    if(tol >= m) m = 1;
    vec[0] /= m;
    vec[1] /= m;
    vec[2] /= m;

    if(fabs(vec[0]) < tol) vec[0] = 0.0f;
    if(fabs(vec[1]) < tol) vec[1] = 0.0f;
    if(fabs(vec[2]) < tol) vec[2] = 0.0f;
}

class ArtificialPotentialField{
public:
    ArtificialPotentialField(ros::NodeHandle &node) :
        cmd_pub_(node.advertise<geometry_msgs::Twist>("cmd_vel", 10)),
        obs_sub_(node.subscribe("slam_cloud", 10, &ArtificialPotentialField::obstacleCallback, this))

    {
        for(int i=0; i < 3; i++) obs_[i] = 0;
    }

    void spin(){
        ros::Rate r(10);

        ros::Duration(1).sleep();
        geometry_msgs::Twist cmd;
        cmd.linear.z = 0.15;
        cmd_pub_.publish(cmd);
        ros::Duration(3).sleep();

        cmd.linear.z = 0;
        cmd_pub_.publish(cmd);
        ros::Duration(3).sleep();

        const double A = 0;
        const double B = 3;
        const double n = 1;
        const double m = 1.5;
        const double force = 0.025;

        while(ros::ok()){
            double Fs[3];
            Fs[0] = Fs[1] = Fs[2] = 0;

            double u[3];
            u[0] = obs_[0];
            u[1] = obs_[1];
            u[2] = obs_[2];
            normalize(u);

            const double d = magnitude(obs_);
            double U = 0;
            if(fabs(d) > tol){
                U = -A/pow(d, n) + B/pow(d, m);
            }

            Fs[0] += U * u[0];
            Fs[1] += U * u[1];
            Fs[2] += U * u[2];

            cmd.linear.x = -Fs[0] * force;
            cmd.linear.y = Fs[1] * force;

            ROS_INFO("obs = (%f, %f)", obs_[0], obs_[1]);
            ROS_INFO_STREAM("cmd = " << cmd);
            cmd_pub_.publish(cmd);
            r.sleep();
            ros::spinOnce();
        }
    }

private:
    void obstacleCallback(const sensor_msgs::PointCloudPtr &obs_msg){

        if(obs_msg->points.size() == 0){
            obs_[0] = 0;
            obs_[1] = 0;
            obs_[2] = 0;
            return;
        }

        double min_obs[3];
        min_obs[0] = obs_msg->points[0].x;
        min_obs[1] = obs_msg->points[0].y;
        min_obs[2] = obs_msg->points[0].z;

        float min_dist = magnitude(min_obs);

        for(int i=1; i < obs_msg->points.size(); i++){
            double obs[3];
            obs[0] = obs_msg->points[i].x;
            obs[1] = obs_msg->points[i].y;
            obs[2] = obs_msg->points[i].z;

            //ROS_INFO("(%f, %f)", obs[0], obs[1]);

            double dist = magnitude(obs);
            if(dist < min_dist){
                min_obs[0] = obs[0];
                min_obs[1] = obs[1];
                min_obs[2] = obs[2];
                min_dist = dist;
            }
        }

        obs_[0] = min_obs[0];
        obs_[1] = min_obs[1];
        obs_[2] = min_obs[2];
    }

    double obs_[3];
    ros::Publisher cmd_pub_;
    ros::Subscriber obs_sub_;
};

int main(int argc, char *argv[]){
    ros::init(argc, argv, "obstacle_avoidance");

    ros::NodeHandle node;
    ArtificialPotentialField apf = ArtificialPotentialField(node);
    apf.spin();

    return 0;
}

<commit_msg>add calculate function of potential field<commit_after>#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/PointCloud.h>
#include <math.h>

static const float tol = 0.000000000000001f;

double magnitude(double vec[3]){
    return sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]);
}

void normalize(double vec[3]){
    float m = magnitude(vec);
    if(tol >= m) m = 1;
    vec[0] /= m;
    vec[1] /= m;
    vec[2] /= m;

    if(fabs(vec[0]) < tol) vec[0] = 0.0f;
    if(fabs(vec[1]) < tol) vec[1] = 0.0f;
    if(fabs(vec[2]) < tol) vec[2] = 0.0f;
}

class ArtificialPotentialField{
public:
    ArtificialPotentialField(ros::NodeHandle &node) :
        cmd_pub_(node.advertise<geometry_msgs::Twist>("cmd_vel", 10)),
        obs_sub_(node.subscribe("slam_cloud", 10, &ArtificialPotentialField::obstacleCallback, this))

    {
        for(int i=0; i < 3; i++) obs_[i] = 0;
    }

    void spin(){
        ros::Rate r(10);

        ros::Duration(1).sleep();
        geometry_msgs::Twist cmd;
        cmd.linear.z = 0.15;
        cmd_pub_.publish(cmd);
        ros::Duration(3).sleep();

        cmd.linear.z = 0;
        cmd_pub_.publish(cmd);
        ros::Duration(3).sleep();

        const double A = 0;
        const double B = 3;
        const double n = 1;
        const double m = 1.5;
        const double force = 0.025;

        while(ros::ok()){
            double Fs[3];
            Fs[0] = Fs[1] = Fs[2] = 0;

            double u[3];
            u[0] = obs_[0];
            u[1] = obs_[1];
            u[2] = obs_[2];
            normalize(u);

            const double d = magnitude(obs_);
            double U = 0;
            if(fabs(d) > tol){
                U = -A/pow(d, n) + B/pow(d, m);
            }

            Fs[0] += U * u[0];
            Fs[1] += U * u[1];
            Fs[2] += U * u[2];

            cmd.linear.x = -Fs[0] * force;
            cmd.linear.y = Fs[1] * force;

            ROS_INFO("obs = (%f, %f)", obs_[0], obs_[1]);
            ROS_INFO_STREAM("cmd = " << cmd);
            cmd_pub_.publish(cmd);
            r.sleep();
            ros::spinOnce();
        }
    }

private:
    void get_potential_force(double dest_lc[3], double f_out[3], double A = 1, double B = 3, double n = 1, double m = 1.5){
        double u[3];
        u[0] = dest_lc[0];
        u[1] = dest_lc[1];
        u[2] = dest_lc[2];
        normalize(u);

        const double d = magnitude(dest_lc);
        double U = 0;
        if(fabs(d) > tol){
            U = -A/pow(d, n) + B/pow(d, m);
        }

        f_out[0] = U * u[0];
        f_out[1] = U * u[1];
        f_out[2] = U * u[2];
    }

    void obstacleCallback(const sensor_msgs::PointCloudPtr &obs_msg){

        if(obs_msg->points.size() == 0){
            obs_[0] = 0;
            obs_[1] = 0;
            obs_[2] = 0;
            return;
        }

        double min_obs[3];
        min_obs[0] = obs_msg->points[0].x;
        min_obs[1] = obs_msg->points[0].y;
        min_obs[2] = obs_msg->points[0].z;

        float min_dist = magnitude(min_obs);

        for(int i=1; i < obs_msg->points.size(); i++){
            double obs[3];
            obs[0] = obs_msg->points[i].x;
            obs[1] = obs_msg->points[i].y;
            obs[2] = obs_msg->points[i].z;

            //ROS_INFO("(%f, %f)", obs[0], obs[1]);

            double dist = magnitude(obs);
            if(dist < min_dist){
                min_obs[0] = obs[0];
                min_obs[1] = obs[1];
                min_obs[2] = obs[2];
                min_dist = dist;
            }
        }

        obs_[0] = min_obs[0];
        obs_[1] = min_obs[1];
        obs_[2] = min_obs[2];
    }

    double obs_[3];
    ros::Publisher cmd_pub_;
    ros::Subscriber obs_sub_;
};

int main(int argc, char *argv[]){
    ros::init(argc, argv, "obstacle_avoidance");

    ros::NodeHandle node;
    ArtificialPotentialField apf = ArtificialPotentialField(node);
    apf.spin();

    return 0;
}

<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
/**
 * @file navigator_rtl.cpp
 * Helper class to access RTL
 * @author Julian Oes <julian@oes.ch>
 * @author Anton Babushkin <anton.babushkin@me.com>
 */

#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <fcntl.h>
#include <float.h>

#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>

#include <uORB/uORB.h>
#include <navigator/navigation.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vtol_vehicle_status.h>

#include "navigator.h"
#include "rtl.h"

#define DELAY_SIGMA	0.01f

RTL::RTL(Navigator *navigator, const char *name) :
	MissionBlock(navigator, name),
	_rtl_state(RTL_STATE_NONE),
	_rtl_start_lock(false),
	_param_return_alt(this, "RTL_RETURN_ALT", false),
	_param_min_loiter_alt(this, "MIS_LTRMIN_ALT", false),
	_param_descend_alt(this, "RTL_DESCEND_ALT", false),
	_param_land_delay(this, "RTL_LAND_DELAY", false),
	_param_rtl_min_dist(this, "RTL_MIN_DIST", false)
{
	/* load initial params */
	updateParams();
	/* initial reset */
	on_inactive();
}

RTL::~RTL()
{
}

void
RTL::on_inactive()
{
	/* reset RTL state only if setpoint moved */
	if (!_navigator->get_can_loiter_at_sp()) {
		_rtl_state = RTL_STATE_NONE;
	}
}

void
RTL::on_activation()
{
	/* reset starting point so we override what the triplet contained from the previous navigation state */
	_rtl_start_lock = false;
	set_current_position_item(&_mission_item);
	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);

	/* decide where to enter the RTL procedure when we switch into it */
	if (_rtl_state == RTL_STATE_NONE) {
		/* for safety reasons don't go into RTL if landed */
		if (_navigator->get_land_detected()->landed) {
			_rtl_state = RTL_STATE_LANDED;
			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already landed, not executing RTL");

			/* if lower than return altitude, climb up first */

		} else if (_navigator->get_global_position()->alt < (_navigator->get_home_position()->alt
				+ _param_return_alt.get())) {
			_rtl_state = RTL_STATE_CLIMB;

			/* otherwise go straight to return */

		} else {
			/* set altitude setpoint to current altitude */
			_rtl_state = RTL_STATE_RETURN;
			_mission_item.altitude_is_relative = false;
			_mission_item.altitude = _navigator->get_global_position()->alt;
		}

	}

	set_rtl_item();
}

void
RTL::on_active()
{
	if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
		advance_rtl();
		set_rtl_item();
	}
}

void
RTL::set_rtl_item()
{
	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

	if (!_rtl_start_lock) {
		set_previous_pos_setpoint();
	}

	_navigator->set_can_loiter_at_sp(false);

	switch (_rtl_state) {
	case RTL_STATE_CLIMB: {

			// check if we are pretty close to home already
			float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat,
					  _navigator->get_home_position()->lon,
					  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);

			// if we are close to home we do not climb as high, otherwise we climb to return alt
			float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();

			// we are close to home, limit climb to min
			if (home_dist < _param_rtl_min_dist.get()) {
				climb_alt = _navigator->get_home_position()->alt + _param_min_loiter_alt.get();
			}

			_mission_item.lat = _navigator->get_global_position()->lat;
			_mission_item.lon = _navigator->get_global_position()->lon;
			_mission_item.altitude_is_relative = false;
			_mission_item.altitude = climb_alt;
			_mission_item.yaw = NAN;
			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = 0.0f;
			_mission_item.autocontinue = true;
			_mission_item.origin = ORIGIN_ONBOARD;

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
					 (int)(climb_alt),
					 (int)(climb_alt - _navigator->get_home_position()->alt));
			break;
		}

	case RTL_STATE_RETURN: {
			_mission_item.lat = _navigator->get_home_position()->lat;
			_mission_item.lon = _navigator->get_home_position()->lon;
			// don't change altitude

			// use home yaw if close to home
			/* check if we are pretty close to home already */
			float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat,
					  _navigator->get_home_position()->lon,
					  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);

			if (home_dist < _param_rtl_min_dist.get()) {
				_mission_item.yaw = _navigator->get_home_position()->yaw;

			} else {
				if (pos_sp_triplet->previous.valid) {
					/* if previous setpoint is valid then use it to calculate heading to home */
					_mission_item.yaw = get_bearing_to_next_waypoint(
								    pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
								    _mission_item.lat, _mission_item.lon);

				} else {
					/* else use current position */
					_mission_item.yaw = get_bearing_to_next_waypoint(
								    _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
								    _mission_item.lat, _mission_item.lon);
				}
			}

			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = 0.0f;
			_mission_item.autocontinue = true;
			_mission_item.origin = ORIGIN_ONBOARD;

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
					 (int)(_mission_item.altitude),
					 (int)(_mission_item.altitude - _navigator->get_home_position()->alt));

			_rtl_start_lock = true;
			break;
		}

	case RTL_STATE_TRANSITION_TO_MC: {
			_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
			_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
			break;
		}

	case RTL_STATE_DESCEND: {
			_mission_item.lat = _navigator->get_home_position()->lat;
			_mission_item.lon = _navigator->get_home_position()->lon;
			_mission_item.altitude_is_relative = false;
			_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();

			// check if we are already lower - then we will just stay there
			if (_mission_item.altitude > _navigator->get_global_position()->alt) {
				_mission_item.altitude = _navigator->get_global_position()->alt;
			}

			_mission_item.yaw = _navigator->get_home_position()->yaw;

			// except for vtol which might be still off here and should point towards this location
			float d_current = get_distance_to_next_waypoint(
						  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
						  _mission_item.lat, _mission_item.lon);

			if (_navigator->get_vstatus()->is_vtol && d_current > _navigator->get_acceptance_radius()) {
				_mission_item.yaw = get_bearing_to_next_waypoint(
							    _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
							    _mission_item.lat, _mission_item.lon);
			}

			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = 0.0f;
			_mission_item.autocontinue = false;
			_mission_item.origin = ORIGIN_ONBOARD;

			/* disable previous setpoint to prevent drift */
			pos_sp_triplet->previous.valid = false;

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
					 (int)(_mission_item.altitude),
					 (int)(_mission_item.altitude - _navigator->get_home_position()->alt));
			break;
		}

	case RTL_STATE_LOITER: {
			bool autoland = _param_land_delay.get() > -DELAY_SIGMA;

			_mission_item.lat = _navigator->get_home_position()->lat;
			_mission_item.lon = _navigator->get_home_position()->lon;
			// don't change altitude
			_mission_item.yaw = _navigator->get_home_position()->yaw;
			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
			_mission_item.autocontinue = autoland;
			_mission_item.origin = ORIGIN_ONBOARD;

			_navigator->set_can_loiter_at_sp(true);

			if (autoland && (Navigator::get_time_inside(_mission_item) > FLT_EPSILON)) {
				mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs",
						 (double)Navigator::get_time_inside(_mission_item));

			} else {
				mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");
			}

			break;
		}

	case RTL_STATE_LAND: {
			_mission_item.yaw = _navigator->get_home_position()->yaw;
			set_land_item(&_mission_item, false);

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home");
			break;
		}

	case RTL_STATE_LANDED: {
			set_idle_item(&_mission_item);

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, landed");
			break;
		}

	default:
		break;
	}

	reset_mission_item_reached();

	/* execute command if set */
	if (!item_contains_position(&_mission_item)) {
		issue_command(&_mission_item);
	}

	/* convert mission item to current position setpoint and make it valid */
	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
	pos_sp_triplet->next.valid = false;

	_navigator->set_position_setpoint_triplet_updated();
}

void
RTL::advance_rtl()
{
	switch (_rtl_state) {
	case RTL_STATE_CLIMB:
		_rtl_state = RTL_STATE_RETURN;
		break;

	case RTL_STATE_RETURN:
		_rtl_state = RTL_STATE_DESCEND;

		if (_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing) {
			_rtl_state = RTL_STATE_TRANSITION_TO_MC;
		}

		break;

	case RTL_STATE_TRANSITION_TO_MC:
		_rtl_state = RTL_STATE_RETURN;
		break;

	case RTL_STATE_DESCEND:

		/* only go to land if autoland is enabled */
		if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
			_rtl_state = RTL_STATE_LOITER;

		} else {
			_rtl_state = RTL_STATE_LAND;
		}

		break;

	case RTL_STATE_LOITER:
		_rtl_state = RTL_STATE_LAND;
		break;

	case RTL_STATE_LAND:
		_rtl_state = RTL_STATE_LANDED;
		break;

	default:
		break;
	}
}
<commit_msg>Navigator: Re-initialize RTL every time it gets re-enabled<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
/**
 * @file navigator_rtl.cpp
 * Helper class to access RTL
 * @author Julian Oes <julian@oes.ch>
 * @author Anton Babushkin <anton.babushkin@me.com>
 */

#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <fcntl.h>
#include <float.h>

#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>

#include <uORB/uORB.h>
#include <navigator/navigation.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vtol_vehicle_status.h>

#include "navigator.h"
#include "rtl.h"

#define DELAY_SIGMA	0.01f

RTL::RTL(Navigator *navigator, const char *name) :
	MissionBlock(navigator, name),
	_rtl_state(RTL_STATE_NONE),
	_rtl_start_lock(false),
	_param_return_alt(this, "RTL_RETURN_ALT", false),
	_param_min_loiter_alt(this, "MIS_LTRMIN_ALT", false),
	_param_descend_alt(this, "RTL_DESCEND_ALT", false),
	_param_land_delay(this, "RTL_LAND_DELAY", false),
	_param_rtl_min_dist(this, "RTL_MIN_DIST", false)
{
	/* load initial params */
	updateParams();
	/* initial reset */
	on_inactive();
}

RTL::~RTL()
{
}

void
RTL::on_inactive()
{
	// reset RTL state
	_rtl_state = RTL_STATE_NONE;
}

void
RTL::on_activation()
{
	/* reset starting point so we override what the triplet contained from the previous navigation state */
	_rtl_start_lock = false;
	set_current_position_item(&_mission_item);
	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);

	/* for safety reasons don't go into RTL if landed */
	if (_navigator->get_land_detected()->landed) {
		_rtl_state = RTL_STATE_LANDED;
		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already landed, not executing RTL");

		/* if lower than return altitude, climb up first */

	} else if (_navigator->get_global_position()->alt < (_navigator->get_home_position()->alt
			+ _param_return_alt.get())) {
		_rtl_state = RTL_STATE_CLIMB;

		/* otherwise go straight to return */

	} else {
		/* set altitude setpoint to current altitude */
		_rtl_state = RTL_STATE_RETURN;
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = _navigator->get_global_position()->alt;
	}

	set_rtl_item();
}

void
RTL::on_active()
{
	if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
		advance_rtl();
		set_rtl_item();
	}
}

void
RTL::set_rtl_item()
{
	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

	if (!_rtl_start_lock) {
		set_previous_pos_setpoint();
	}

	_navigator->set_can_loiter_at_sp(false);

	switch (_rtl_state) {
	case RTL_STATE_CLIMB: {

			// check if we are pretty close to home already
			float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat,
					  _navigator->get_home_position()->lon,
					  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);

			// if we are close to home we do not climb as high, otherwise we climb to return alt
			float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();

			// we are close to home, limit climb to min
			if (home_dist < _param_rtl_min_dist.get()) {
				climb_alt = _navigator->get_home_position()->alt + _param_min_loiter_alt.get();
			}

			_mission_item.lat = _navigator->get_global_position()->lat;
			_mission_item.lon = _navigator->get_global_position()->lon;
			_mission_item.altitude_is_relative = false;
			_mission_item.altitude = climb_alt;
			_mission_item.yaw = NAN;
			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = 0.0f;
			_mission_item.autocontinue = true;
			_mission_item.origin = ORIGIN_ONBOARD;

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
					 (int)(climb_alt),
					 (int)(climb_alt - _navigator->get_home_position()->alt));
			break;
		}

	case RTL_STATE_RETURN: {
			_mission_item.lat = _navigator->get_home_position()->lat;
			_mission_item.lon = _navigator->get_home_position()->lon;
			// don't change altitude

			// use home yaw if close to home
			/* check if we are pretty close to home already */
			float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat,
					  _navigator->get_home_position()->lon,
					  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);

			if (home_dist < _param_rtl_min_dist.get()) {
				_mission_item.yaw = _navigator->get_home_position()->yaw;

			} else {
				if (pos_sp_triplet->previous.valid) {
					/* if previous setpoint is valid then use it to calculate heading to home */
					_mission_item.yaw = get_bearing_to_next_waypoint(
								    pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
								    _mission_item.lat, _mission_item.lon);

				} else {
					/* else use current position */
					_mission_item.yaw = get_bearing_to_next_waypoint(
								    _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
								    _mission_item.lat, _mission_item.lon);
				}
			}

			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = 0.0f;
			_mission_item.autocontinue = true;
			_mission_item.origin = ORIGIN_ONBOARD;

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
					 (int)(_mission_item.altitude),
					 (int)(_mission_item.altitude - _navigator->get_home_position()->alt));

			_rtl_start_lock = true;
			break;
		}

	case RTL_STATE_TRANSITION_TO_MC: {
			_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
			_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
			break;
		}

	case RTL_STATE_DESCEND: {
			_mission_item.lat = _navigator->get_home_position()->lat;
			_mission_item.lon = _navigator->get_home_position()->lon;
			_mission_item.altitude_is_relative = false;
			_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();

			// check if we are already lower - then we will just stay there
			if (_mission_item.altitude > _navigator->get_global_position()->alt) {
				_mission_item.altitude = _navigator->get_global_position()->alt;
			}

			_mission_item.yaw = _navigator->get_home_position()->yaw;

			// except for vtol which might be still off here and should point towards this location
			float d_current = get_distance_to_next_waypoint(
						  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
						  _mission_item.lat, _mission_item.lon);

			if (_navigator->get_vstatus()->is_vtol && d_current > _navigator->get_acceptance_radius()) {
				_mission_item.yaw = get_bearing_to_next_waypoint(
							    _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
							    _mission_item.lat, _mission_item.lon);
			}

			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = 0.0f;
			_mission_item.autocontinue = false;
			_mission_item.origin = ORIGIN_ONBOARD;

			/* disable previous setpoint to prevent drift */
			pos_sp_triplet->previous.valid = false;

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
					 (int)(_mission_item.altitude),
					 (int)(_mission_item.altitude - _navigator->get_home_position()->alt));
			break;
		}

	case RTL_STATE_LOITER: {
			bool autoland = _param_land_delay.get() > -DELAY_SIGMA;

			_mission_item.lat = _navigator->get_home_position()->lat;
			_mission_item.lon = _navigator->get_home_position()->lon;
			// don't change altitude
			_mission_item.yaw = _navigator->get_home_position()->yaw;
			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
			_mission_item.autocontinue = autoland;
			_mission_item.origin = ORIGIN_ONBOARD;

			_navigator->set_can_loiter_at_sp(true);

			if (autoland && (Navigator::get_time_inside(_mission_item) > FLT_EPSILON)) {
				mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs",
						 (double)Navigator::get_time_inside(_mission_item));

			} else {
				mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");
			}

			break;
		}

	case RTL_STATE_LAND: {
			_mission_item.yaw = _navigator->get_home_position()->yaw;
			set_land_item(&_mission_item, false);

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home");
			break;
		}

	case RTL_STATE_LANDED: {
			set_idle_item(&_mission_item);

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, landed");
			break;
		}

	default:
		break;
	}

	reset_mission_item_reached();

	/* execute command if set */
	if (!item_contains_position(&_mission_item)) {
		issue_command(&_mission_item);
	}

	/* convert mission item to current position setpoint and make it valid */
	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
	pos_sp_triplet->next.valid = false;

	_navigator->set_position_setpoint_triplet_updated();
}

void
RTL::advance_rtl()
{
	switch (_rtl_state) {
	case RTL_STATE_CLIMB:
		_rtl_state = RTL_STATE_RETURN;
		break;

	case RTL_STATE_RETURN:
		_rtl_state = RTL_STATE_DESCEND;

		if (_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing) {
			_rtl_state = RTL_STATE_TRANSITION_TO_MC;
		}

		break;

	case RTL_STATE_TRANSITION_TO_MC:
		_rtl_state = RTL_STATE_RETURN;
		break;

	case RTL_STATE_DESCEND:

		/* only go to land if autoland is enabled */
		if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
			_rtl_state = RTL_STATE_LOITER;

		} else {
			_rtl_state = RTL_STATE_LAND;
		}

		break;

	case RTL_STATE_LOITER:
		_rtl_state = RTL_STATE_LAND;
		break;

	case RTL_STATE_LAND:
		_rtl_state = RTL_STATE_LANDED;
		break;

	default:
		break;
	}
}
<|endoftext|>


<commit_before>/*
    Copyright 2014 Néstor Morales Hernández <nestor@isaatc.ull.es>

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

        http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
*/

#include "oflow_3d_generator.h"

#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <pcl-1.7/pcl/point_cloud.h>
#include <pcl-1.7/pcl/impl/point_types.hpp>

namespace oflow_3d_generator {

OFlow3dGenerator::OFlow3dGenerator(const std::string& transport)
{
    ros::NodeHandle nh;
    int queue_size;

    ros::NodeHandle local_nh("~");
    local_nh.param("queue_size", queue_size, 5);

    // Topics
    std::string stereo_ns = nh.resolveName("stereo");
    std::string left_topic = ros::names::clean(stereo_ns + "/left/" + nh.resolveName("image"));
    std::string right_topic = ros::names::clean(stereo_ns + "/disparity/" + nh.resolveName("image"));
    std::string left_info_topic = stereo_ns + "/left/camera_info";
    std::string right_info_topic = stereo_ns + "/right/camera_info";

    image_transport::ImageTransport it(nh);
    m_left_sub.subscribe(it, left_topic, 1, transport);
    m_disp_sub.subscribe(it, right_topic, 1, transport);
    m_left_info_sub.subscribe(nh, left_info_topic, 1);
    m_right_info_sub.subscribe(nh, right_info_topic, 1);

    m_flowVectorsPub = nh.advertise<sensor_msgs::PointCloud2> ("flow_vectors", 1);

    // Check the frame w.r.t. motion is computed.
    local_nh.param<string>("motion_frame_id", m_motionFrame, "map");

    // Synchronize input topics. Optionally do approximate synchronization.
    bool approx;
    local_nh.param("approximate_sync", approx, false);
    if (approx)
    {
        m_approximate_sync.reset(new ApproximateSync(ApproximatePolicy(queue_size),
                                                    m_left_sub, m_disp_sub, m_left_info_sub, m_right_info_sub) );
        m_approximate_sync->registerCallback(boost::bind(&OFlow3dGenerator::process, this, _1, _2, _3, _4));
    }
    else
    {
        m_exact_sync.reset(new ExactSync(ExactPolicy(queue_size),
                                        m_left_sub, m_disp_sub, m_left_info_sub, m_right_info_sub) );
        m_exact_sync->registerCallback(boost::bind(&OFlow3dGenerator::process, this, _1, _2, _3, _4));
    }

    // Create the elas processing class
    m_param = Elas::parameters(Elas::MIDDLEBURY);
    m_param.match_texture = 1;
    m_param.postprocess_only_left = 1;
    m_param.ipol_gap_width = 2;

    m_elas.reset(new Elas(m_param));
}

void OFlow3dGenerator::process(const sensor_msgs::ImageConstPtr& l_image_msg, const sensor_msgs::ImageConstPtr& d_image_msg,
                                 const sensor_msgs::CameraInfoConstPtr& l_info_msg, const sensor_msgs::CameraInfoConstPtr& r_info_msg)
{
    cv_bridge::CvImageConstPtr leftImgPtr, dispImgPtr;
    leftImgPtr = cv_bridge::toCvShare(l_image_msg, sensor_msgs::image_encodings::MONO8);
    dispImgPtr = cv_bridge::toCvShare(d_image_msg, sensor_msgs::image_encodings::MONO8);
    m_model.fromCameraInfo(*l_info_msg, *r_info_msg);

    tf::StampedTransform tfCamera2Motion;
    try {
        m_tfListener.lookupTransform(m_motionFrame, l_info_msg->header.frame_id, ros::Time(0), tfCamera2Motion);

        // Accumulate images and frames, and process them
        m_leftImages.push_back(leftImgPtr->image);
        m_dispImages.push_back(dispImgPtr->image);
        m_camera2MotionTransformation.push_back(tfCamera2Motion);

        if (m_leftImages.size() > 2) m_leftImages.pop_front();
        if (m_dispImages.size() > 2) m_dispImages.pop_front();
        if (m_camera2MotionTransformation.size() > 2) m_camera2MotionTransformation.pop_front();

        if (m_leftImages.size() == 2) {
            vector<cv::Point2f> points1, points2;
            pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr outputVectors;
            findPairsOFlow(m_leftImages[0], m_leftImages[1], points1, points2);
            compute3DVectors(points1, points2, m_leftImages[1], m_dispImages[0], m_dispImages[1], outputVectors);

            // Publish results
            sensor_msgs::PointCloud2 cloudMsg;
            pcl::toROSMsg (*outputVectors, cloudMsg);
            cloudMsg.header.frame_id = l_info_msg->header.frame_id;
            cloudMsg.header.stamp = ros::Time::now();
            m_flowVectorsPub.publish(cloudMsg);
        }
    } catch (tf::TransformException ex){
        ROS_ERROR("%s",ex.what());
    }
}

inline void OFlow3dGenerator::findPairsOFlow(const cv::Mat & img1, const cv::Mat & img2,
                                             vector<cv::Point2f> & outPoints1, vector<cv::Point2f> & outPoints2)
{

    // We look for correspondences using Optical flow
    // vector of keypoints
    vector<cv::KeyPoint> keypoints1;
    cv::FastFeatureDetector fastDetector(50);
    fastDetector.detect(img1, keypoints1);

    if (keypoints1.size() == 0)
        return;

    vector<cv::Point2f> points1(keypoints1.size()), points2, points1B;
    {
        uint32_t idx = 0;
        for (vector<cv::KeyPoint>::iterator it = keypoints1.begin(); it != keypoints1.end(); it++, idx++) {
            points1[idx] = it->pt;
        }
    }
    // Optical flow
    vector<uint8_t> status, statusB;
    vector<float_t> error, errorB;

    cv::calcOpticalFlowPyrLK(img1, img2, points1, points2, status, error, cv::Size(3, 3), 3);
    cv::calcOpticalFlowPyrLK(img2, img1, points2, points1B, statusB, errorB, cv::Size(3, 3), 3);

    vector<cv::Point2f> pointsA(points1.size()), pointsB(points2.size());
    {
        uint32_t idx = 0;
        for (uint32_t i = 0; i < points1.size(); i++) {
            if ((status[i] == 1) && (statusB[i] == 1)) {
                if (cv::norm(points1[i] - points1B[i]) < 1.0) {
                    pointsA[idx] = points1[i];
                    pointsB[idx] = points2[i];
                }
            }
            idx++;
        }
        pointsA.resize(idx);
        pointsB.resize(idx);
    }

    outPoints1 = pointsA;
    outPoints2 = pointsB;

}

inline void OFlow3dGenerator::compute3DVectors(const vector<cv::Point2f> & origPoints,
                                               const vector<cv::Point2f> & destPoints, const cv::Mat & img,
                                               const cv::Mat & origDispImg, const cv::Mat & destDispImg,
                                               pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & outputVectors)
{
    outputVectors.reset(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
    outputVectors->reserve(origPoints.size());

    for (vector<cv::Point2f>::const_iterator itOrig = origPoints.begin(), itDest = destPoints.begin();
            itOrig != origPoints.end(); itOrig++, itDest++) {

        pcl::PointXYZRGBNormal flow;
        cv::Point3d  origPoint3D, destPoint3D, motOrigPoint3D, motDestPoint3D;
        if (! get3DfromDisp(origDispImg, *itOrig, origPoint3D)) continue;
        if (! get3DfromDisp(destDispImg, *itDest, destPoint3D)) continue;

        transformPoint(origPoint3D, m_camera2MotionTransformation[0], motOrigPoint3D);
        transformPoint(destPoint3D, m_camera2MotionTransformation[1], motDestPoint3D);

        flow.x = destPoint3D.x;
        flow.y = destPoint3D.y;
        flow.z = destPoint3D.z;

        flow.normal_x = motDestPoint3D.x - motOrigPoint3D.x;
        flow.normal_y = motDestPoint3D.y - motOrigPoint3D.y;
        flow.normal_z = motDestPoint3D.z - motOrigPoint3D.z;

        flow.b = img.at<cv::Vec3b>(itDest->y, itDest->x)[0];
        flow.g = img.at<cv::Vec3b>(itDest->y, itDest->x)[1];
        flow.r = img.at<cv::Vec3b>(itDest->y, itDest->x)[2];

        outputVectors->push_back(flow);
    }
}

inline bool OFlow3dGenerator::get3DfromDisp(const cv::Mat & dispImg,
                                            const cv::Point2d point2D, cv::Point3d & point3D)
{
    const float dMax = (float)(m_param.disp_max);
    const double value = dispImg.at<uint8_t>(point2D.y, point2D.x);

    if (value == 0.0)
        return false;

    m_model.projectDisparityTo3d(point2D, value, point3D);

    return true;
}

inline void OFlow3dGenerator::transformPoint(const cv::Point3d & inputPoint3D, const tf::StampedTransform & tfCamera2Motion,
                                             cv::Point3d & outputPoint3D)
{
    tf::Vector3 tmpPoint = tfCamera2Motion * tf::Vector3(inputPoint3D.x, inputPoint3D.y, inputPoint3D.z);
    outputPoint3D.x = tmpPoint.getX();
    outputPoint3D.y = tmpPoint.getY();
    outputPoint3D.z = tmpPoint.getZ();
}

}<commit_msg>Changed the frame in which the origin of the flow vectors is published.<commit_after>/*
    Copyright 2014 Néstor Morales Hernández <nestor@isaatc.ull.es>

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

        http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
*/

#include "oflow_3d_generator.h"

#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <pcl-1.7/pcl/point_cloud.h>
#include <pcl-1.7/pcl/impl/point_types.hpp>

namespace oflow_3d_generator {

OFlow3dGenerator::OFlow3dGenerator(const std::string& transport)
{
    ros::NodeHandle nh;
    int queue_size;

    ros::NodeHandle local_nh("~");
    local_nh.param("queue_size", queue_size, 5);

    // Topics
    std::string stereo_ns = nh.resolveName("stereo");
    std::string left_topic = ros::names::clean(stereo_ns + "/left/" + nh.resolveName("image"));
    std::string right_topic = ros::names::clean(stereo_ns + "/disparity/" + nh.resolveName("image"));
    std::string left_info_topic = stereo_ns + "/left/camera_info";
    std::string right_info_topic = stereo_ns + "/right/camera_info";

    image_transport::ImageTransport it(nh);
    m_left_sub.subscribe(it, left_topic, 1, transport);
    m_disp_sub.subscribe(it, right_topic, 1, transport);
    m_left_info_sub.subscribe(nh, left_info_topic, 1);
    m_right_info_sub.subscribe(nh, right_info_topic, 1);

    m_flowVectorsPub = nh.advertise<sensor_msgs::PointCloud2> ("flow_vectors", 1);

    // Check the frame w.r.t. motion is computed.
    local_nh.param<string>("motion_frame_id", m_motionFrame, "map");

    // Synchronize input topics. Optionally do approximate synchronization.
    bool approx;
    local_nh.param("approximate_sync", approx, false);
    if (approx)
    {
        m_approximate_sync.reset(new ApproximateSync(ApproximatePolicy(queue_size),
                                                    m_left_sub, m_disp_sub, m_left_info_sub, m_right_info_sub) );
        m_approximate_sync->registerCallback(boost::bind(&OFlow3dGenerator::process, this, _1, _2, _3, _4));
    }
    else
    {
        m_exact_sync.reset(new ExactSync(ExactPolicy(queue_size),
                                        m_left_sub, m_disp_sub, m_left_info_sub, m_right_info_sub) );
        m_exact_sync->registerCallback(boost::bind(&OFlow3dGenerator::process, this, _1, _2, _3, _4));
    }

    // Create the elas processing class
    m_param = Elas::parameters(Elas::MIDDLEBURY);
    m_param.match_texture = 1;
    m_param.postprocess_only_left = 1;
    m_param.ipol_gap_width = 2;

    m_elas.reset(new Elas(m_param));
}

void OFlow3dGenerator::process(const sensor_msgs::ImageConstPtr& l_image_msg, const sensor_msgs::ImageConstPtr& d_image_msg,
                                 const sensor_msgs::CameraInfoConstPtr& l_info_msg, const sensor_msgs::CameraInfoConstPtr& r_info_msg)
{
    cv_bridge::CvImageConstPtr leftImgPtr, dispImgPtr;
    leftImgPtr = cv_bridge::toCvShare(l_image_msg, sensor_msgs::image_encodings::MONO8);
    dispImgPtr = cv_bridge::toCvShare(d_image_msg, sensor_msgs::image_encodings::MONO8);
    m_model.fromCameraInfo(*l_info_msg, *r_info_msg);

    tf::StampedTransform tfCamera2Motion;
    try {
        m_tfListener.lookupTransform(m_motionFrame, l_info_msg->header.frame_id, ros::Time(0), tfCamera2Motion);

        // Accumulate images and frames, and process them
        m_leftImages.push_back(leftImgPtr->image);
        m_dispImages.push_back(dispImgPtr->image);
        m_camera2MotionTransformation.push_back(tfCamera2Motion);

        if (m_leftImages.size() > 2) m_leftImages.pop_front();
        if (m_dispImages.size() > 2) m_dispImages.pop_front();
        if (m_camera2MotionTransformation.size() > 2) m_camera2MotionTransformation.pop_front();

        if (m_leftImages.size() == 2) {
            vector<cv::Point2f> points1, points2;
            pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr outputVectors;
            findPairsOFlow(m_leftImages[0], m_leftImages[1], points1, points2);
            compute3DVectors(points1, points2, m_leftImages[1], m_dispImages[0], m_dispImages[1], outputVectors);

            // Publish results
            sensor_msgs::PointCloud2 cloudMsg;
            pcl::toROSMsg (*outputVectors, cloudMsg);
            cloudMsg.header.frame_id = m_motionFrame; // l_info_msg->header.frame_id;
            cloudMsg.header.stamp = ros::Time::now();
            m_flowVectorsPub.publish(cloudMsg);
        }
    } catch (tf::TransformException ex){
        ROS_ERROR("%s",ex.what());
    }
}

inline void OFlow3dGenerator::findPairsOFlow(const cv::Mat & img1, const cv::Mat & img2,
                                             vector<cv::Point2f> & outPoints1, vector<cv::Point2f> & outPoints2)
{

    // We look for correspondences using Optical flow
    // vector of keypoints
    vector<cv::KeyPoint> keypoints1;
    cv::FastFeatureDetector fastDetector(50);
    fastDetector.detect(img1, keypoints1);

    if (keypoints1.size() == 0)
        return;

    vector<cv::Point2f> points1(keypoints1.size()), points2, points1B;
    {
        uint32_t idx = 0;
        for (vector<cv::KeyPoint>::iterator it = keypoints1.begin(); it != keypoints1.end(); it++, idx++) {
            points1[idx] = it->pt;
        }
    }
    // Optical flow
    vector<uint8_t> status, statusB;
    vector<float_t> error, errorB;

    cv::calcOpticalFlowPyrLK(img1, img2, points1, points2, status, error, cv::Size(3, 3), 3);
    cv::calcOpticalFlowPyrLK(img2, img1, points2, points1B, statusB, errorB, cv::Size(3, 3), 3);

    vector<cv::Point2f> pointsA(points1.size()), pointsB(points2.size());
    {
        uint32_t idx = 0;
        for (uint32_t i = 0; i < points1.size(); i++) {
            if ((status[i] == 1) && (statusB[i] == 1)) {
                if (cv::norm(points1[i] - points1B[i]) < 1.0) {
                    pointsA[idx] = points1[i];
                    pointsB[idx] = points2[i];
                }
            }
            idx++;
        }
        pointsA.resize(idx);
        pointsB.resize(idx);
    }

    outPoints1 = pointsA;
    outPoints2 = pointsB;

}

inline void OFlow3dGenerator::compute3DVectors(const vector<cv::Point2f> & origPoints,
                                               const vector<cv::Point2f> & destPoints, const cv::Mat & img,
                                               const cv::Mat & origDispImg, const cv::Mat & destDispImg,
                                               pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & outputVectors)
{
    outputVectors.reset(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
    outputVectors->reserve(origPoints.size());

    for (vector<cv::Point2f>::const_iterator itOrig = origPoints.begin(), itDest = destPoints.begin();
            itOrig != origPoints.end(); itOrig++, itDest++) {

        pcl::PointXYZRGBNormal flow;
        cv::Point3d  origPoint3D, destPoint3D, motOrigPoint3D, motDestPoint3D;
        if (! get3DfromDisp(origDispImg, *itOrig, origPoint3D)) continue;
        if (! get3DfromDisp(destDispImg, *itDest, destPoint3D)) continue;

        transformPoint(origPoint3D, m_camera2MotionTransformation[0], motOrigPoint3D);
        transformPoint(destPoint3D, m_camera2MotionTransformation[1], motDestPoint3D);

        flow.x = motDestPoint3D.x;
        flow.y = motDestPoint3D.y;
        flow.z = motDestPoint3D.z;

        flow.normal_x = motDestPoint3D.x - motOrigPoint3D.x;
        flow.normal_y = motDestPoint3D.y - motOrigPoint3D.y;
        flow.normal_z = motDestPoint3D.z - motOrigPoint3D.z;

        flow.b = img.at<cv::Vec3b>(itDest->y, itDest->x)[0];
        flow.g = img.at<cv::Vec3b>(itDest->y, itDest->x)[1];
        flow.r = img.at<cv::Vec3b>(itDest->y, itDest->x)[2];

        outputVectors->push_back(flow);
    }
}

inline bool OFlow3dGenerator::get3DfromDisp(const cv::Mat & dispImg,
                                            const cv::Point2d point2D, cv::Point3d & point3D)
{
    const float dMax = (float)(m_param.disp_max);
    const double value = dispImg.at<uint8_t>(point2D.y, point2D.x);

    if (value == 0.0)
        return false;

    m_model.projectDisparityTo3d(point2D, value, point3D);

    return true;
}

inline void OFlow3dGenerator::transformPoint(const cv::Point3d & inputPoint3D, const tf::StampedTransform & tfCamera2Motion,
                                             cv::Point3d & outputPoint3D)
{
    tf::Vector3 tmpPoint = tfCamera2Motion * tf::Vector3(inputPoint3D.x, inputPoint3D.y, inputPoint3D.z);
    outputPoint3D.x = tmpPoint.getX();
    outputPoint3D.y = tmpPoint.getY();
    outputPoint3D.z = tmpPoint.getZ();
}

}<|endoftext|>


<commit_before>/*
 * calc/contact.cpp --
 *
 * This file is part of nettcl2d application.
 *
 * Copyright (c) 2012 Andrey V. Nakin <andrey.nakin@gmail.com>
 * All rights reserved.
 *
 * See the file "COPYING" for information on usage and redistribution
 * of this file, and for a DISCLAIMER OF ALL WARRANTIES.
 *
 */

/*
 * I've took a calculator example from the link below and adopted it
 * http://www.oreillynet.com/network/2003/05/06/examples/calculatorexample.html
 */

#include <string>
#include <map>
#include <cstdlib>
#include <iomanip>
#include <boost/spirit/include/classic.hpp>
#include <boost/spirit/include/phoenix1_binders.hpp>
#include <boost/lambda/bind.hpp>
#include "contact.hpp"

struct calculator : boost::spirit::classic::grammar<calculator> {

	calculator(bool& result, const Contact::TagContainer& tags) :
		result(result), tags(tags) {}

	// A production can have an associated closure, to store information
	// for that production.
	struct value_closure : boost::spirit::classic::closure<value_closure, bool> {
		member1 value;
	};

	struct string_closure : boost::spirit::classic::closure<string_closure, std::string> {
		member1 name;
	};

	// Following is the grammar definition.
	template <typename ScannerT>
	struct definition {
		definition(calculator const& self) {
			using namespace boost::spirit::classic;
			using namespace phoenix;

			// The lexeme_d directive tells the scanner to treat white space as
			// significant. Thus, an identifier cannot have internal white space.
			// The alpha_p and alnum_p parsers are built-in.
			// Notice how the semantic action uses a Phoenix lambda function
			// that constructs a std::string. The arg1 and arg2 placeholders are
			// are bound at runtime to the iterator range that matches this rule.
			identifier =
				lexeme_d[
					+( alnum_p | '_' | '-')
				][identifier.name = construct_<std::string>(arg1, arg2)];

			group =
				'('	>> expression[group.value = arg1] >> ')';

			// A statement can end at the end of the line, or with a semicolon.
			statement =
				expression[bind(&calculator::do_print)(self, arg1)] >> end_p;

			// A variable name must be looked up. This is a straightforward
			// Phoenix binding.
			factor =
				group[factor.value = arg1]
				| identifier[factor.value = bind(&calculator::lookup)(self, arg1)];

			term =
				factor[term.value = arg1] >> *('&' >> factor[term.value *= arg1]);

			expression =
				term[expression.value = arg1] >> *('|' >> term[expression.value += arg1]);
		}

		// The start symbol is returned from start().
		boost::spirit::classic::rule<ScannerT> const& start() const {
			return statement;
		}

		// Each rule must be declared, optionally with an associated closure.
		boost::spirit::classic::rule<ScannerT> statement;
		boost::spirit::classic::rule<ScannerT, string_closure::context_t> identifier;
		boost::spirit::classic::rule<ScannerT, value_closure::context_t> expression, factor, group, term;
	};

	bool lookup(const std::string& name) const {
		return "*" == name && !tags.empty()
			? true
			: tags.end() != tags.find(name);
	}

	void do_print(bool x) const {
		result = x;
	}

private:

	bool& result;
	const Contact::TagContainer& tags;

};

bool Contact::matches(const std::string& expression) const {
	using namespace boost::spirit::classic;

	bool result = false;
	calculator calc(result, tags);

	parse_info<std::string::const_iterator> info = parse(expression.begin(), expression.end(), calc, space_p);
	if (!info.hit) {
		throw ParseException(std::string(expression.begin(), info.stop));
	} else if (!info.full) {
		throw ParseException(std::string("expression is not full"));
	}

	return result;
}
<commit_msg>	deleted:    contact.cpp<commit_after><|endoftext|>


<commit_before>//*****************************************************************************
// Copyright 2017-2020 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//*****************************************************************************
#include "ngraph/op/fused/clamp.hpp"

#include "ngraph/builder/make_constant.hpp"
#include "ngraph/op/maximum.hpp"
#include "ngraph/op/minimum.hpp"
#include "ngraph/runtime/reference/clamp.hpp"
#include "ngraph/util.hpp"

using namespace std;
using namespace ngraph;

constexpr NodeTypeInfo op::Clamp::type_info;

namespace
{
    template <element::Type_t ET, typename T>
    bool evaluate(const HostTensorPtr& arg, const HostTensorPtr& out, T min, T max, size_t count)
    {
        runtime::reference::clamp<T>(
            arg->get_data_ptr<ET>(), out->get_data_ptr<ET>(), min, max, count);
        return true;
    }

    bool evaluate_clamp(
        const HostTensorPtr& arg, const HostTensorPtr& out, double min, double max, size_t count)
    {
        auto ceil_func = [](double x) { return std::ceil(x); };
        auto floor_func = [](double x) { return std::floor(x); };

        bool rc = true;
        switch (arg->get_element_type())
        {
            TYPE_CASE(i8)
            (arg,
             out,
             double_to_int<int8_t>(min, ceil_func),
             double_to_int<int8_t>(max, floor_func),
             count);
            break;
            TYPE_CASE(i16)
            (arg,
             out,
             double_to_int<int16_t>(min, ceil_func),
             double_to_int<int16_t>(max, floor_func),
             count);
            break;
            TYPE_CASE(i32)
            (arg,
             out,
             double_to_int<int32_t>(min, ceil_func),
             double_to_int<int32_t>(max, floor_func),
             count);
            break;
            TYPE_CASE(i64)
            (arg,
             out,
             double_to_int<int64_t>(min, ceil_func),
             double_to_int<int64_t>(max, floor_func),
             count);
            break;
            TYPE_CASE(u8)
            (arg,
             out,
             double_to_int<uint8_t>(min, ceil_func),
             double_to_int<uint8_t>(max, floor_func),
             count);
            break;
            TYPE_CASE(u16)
            (arg,
             out,
             double_to_int<uint16_t>(min, ceil_func),
             double_to_int<uint16_t>(max, floor_func),
             count);
            break;
            TYPE_CASE(u32)
            (arg,
             out,
             double_to_int<uint32_t>(min, ceil_func),
             double_to_int<uint32_t>(max, floor_func),
             count);
            break;
            TYPE_CASE(u64)
            (arg,
             out,
             double_to_int<uint64_t>(min, ceil_func),
             double_to_int<uint64_t>(max, floor_func),
             count);
            break;
            TYPE_CASE(f32)(arg, out, static_cast<float>(min), static_cast<float>(max), count);
            break;
            TYPE_CASE(f64)(arg, out, min, max, count);
            break;
        default: rc = false; break;
        }
        return rc;
    }
}

bool op::v0::Clamp::evaluate(const HostTensorVector& outputs, const HostTensorVector& inputs)
{
    return evaluate_clamp(
        inputs[0], outputs[0], get_min(), get_max(), shape_size(get_output_shape(0)));
}

op::Clamp::Clamp(const Output<Node>& data, const double min, const double max)
    : FusedOp({data})
    , m_min{min}
    , m_max{max}
{
    constructor_validate_and_infer_types();
}

void op::Clamp::pre_validate_and_infer_types()
{
    NODE_VALIDATION_CHECK(
        this, m_min < m_max, "The 'min' parameter needs to be less than 'max' for Clamp");
    set_output_type(0, get_input_element_type(0), get_input_partial_shape(0));
}

NodeVector op::Clamp::decompose_op() const
{
    const auto data = input_value(0);
    const auto type = data.get_element_type();
    const auto shape = data.get_shape();

    auto ceil_func = [](double x) { return std::ceil(x); };
    auto floor_func = [](double x) { return std::floor(x); };

    shared_ptr<Node> clamp_min, clamp_max;

    switch (type)
    {
    case element::Type_t::i8:
    {
        clamp_min = builder::make_constant(type, shape, double_to_int<int8_t>(m_min, ceil_func));
        clamp_max = builder::make_constant(type, shape, double_to_int<int8_t>(m_max, floor_func));
        break;
    }
    case element::Type_t::i16:
    {
        clamp_min = builder::make_constant(type, shape, double_to_int<int16_t>(m_min, ceil_func));
        clamp_max = builder::make_constant(type, shape, double_to_int<int16_t>(m_max, floor_func));
        break;
    }
    case element::Type_t::i32:
    {
        clamp_min = builder::make_constant(type, shape, double_to_int<int32_t>(m_min, ceil_func));
        clamp_max = builder::make_constant(type, shape, double_to_int<int32_t>(m_max, floor_func));
        break;
    }
    case element::Type_t::i64:
    {
        clamp_min = builder::make_constant(type, shape, double_to_int<int64_t>(m_min, ceil_func));
        clamp_max = builder::make_constant(type, shape, double_to_int<int64_t>(m_max, floor_func));
        break;
    }
    case element::Type_t::u8:
    {
        clamp_min = builder::make_constant(type, shape, double_to_int<uint8_t>(m_min, ceil_func));
        clamp_max = builder::make_constant(type, shape, double_to_int<uint8_t>(m_max, floor_func));
        break;
    }
    case element::Type_t::u16:
    {
        clamp_min = builder::make_constant(type, shape, double_to_int<uint16_t>(m_min, ceil_func));
        clamp_max = builder::make_constant(type, shape, double_to_int<uint16_t>(m_max, floor_func));
        break;
    }
    case element::Type_t::u32:
    {
        clamp_min = builder::make_constant(type, shape, double_to_int<uint32_t>(m_min, ceil_func));
        clamp_max = builder::make_constant(type, shape, double_to_int<uint32_t>(m_max, floor_func));
        break;
    }
    case element::Type_t::u64:
    {
        clamp_min = builder::make_constant(type, shape, double_to_int<uint64_t>(m_min, ceil_func));
        clamp_max = builder::make_constant(type, shape, double_to_int<uint64_t>(m_max, floor_func));
        break;
    }
    case element::Type_t::f32:
    {
        clamp_min = builder::make_constant(type, shape, static_cast<float>(m_min));
        clamp_max = builder::make_constant(type, shape, static_cast<float>(m_max));
        break;
    }
    case element::Type_t::f64:
    {
        clamp_min = builder::make_constant(type, shape, m_min);
        clamp_max = builder::make_constant(type, shape, m_max);
        break;
    }
    default: throw runtime_error("Unsupported data type in op Clamp"); break;
    }

    auto max = std::make_shared<ngraph::op::Maximum>(clamp_min, data);
    return {std::make_shared<ngraph::op::Minimum>(clamp_max, max)};
}

shared_ptr<Node> op::Clamp::clone_with_new_inputs(const OutputVector& new_args) const
{
    NODE_VALIDATION_CHECK(this,
                          new_args.size() == 1,
                          "Expected 1 element in new_args for the Clamp op but got ",
                          new_args.size());

    return make_shared<Clamp>(new_args.at(0), m_min, m_max);
}

bool op::Clamp::visit_attributes(AttributeVisitor& visitor)
{
    visitor.on_attribute("min", m_min);
    visitor.on_attribute("max", m_max);
    return true;
}
<commit_msg>use input shape<commit_after>//*****************************************************************************
// Copyright 2017-2020 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//*****************************************************************************
#include "ngraph/op/fused/clamp.hpp"

#include "ngraph/builder/make_constant.hpp"
#include "ngraph/op/maximum.hpp"
#include "ngraph/op/minimum.hpp"
#include "ngraph/runtime/reference/clamp.hpp"
#include "ngraph/util.hpp"

using namespace std;
using namespace ngraph;

constexpr NodeTypeInfo op::Clamp::type_info;

namespace
{
    template <element::Type_t ET, typename T>
    bool evaluate(const HostTensorPtr& arg, const HostTensorPtr& out, T min, T max, size_t count)
    {
        runtime::reference::clamp<T>(
            arg->get_data_ptr<ET>(), out->get_data_ptr<ET>(), min, max, count);
        return true;
    }

    bool evaluate_clamp(
        const HostTensorPtr& arg, const HostTensorPtr& out, double min, double max, size_t count)
    {
        auto ceil_func = [](double x) { return std::ceil(x); };
        auto floor_func = [](double x) { return std::floor(x); };

        bool rc = true;
        switch (arg->get_element_type())
        {
            TYPE_CASE(i8)
            (arg,
             out,
             double_to_int<int8_t>(min, ceil_func),
             double_to_int<int8_t>(max, floor_func),
             count);
            break;
            TYPE_CASE(i16)
            (arg,
             out,
             double_to_int<int16_t>(min, ceil_func),
             double_to_int<int16_t>(max, floor_func),
             count);
            break;
            TYPE_CASE(i32)
            (arg,
             out,
             double_to_int<int32_t>(min, ceil_func),
             double_to_int<int32_t>(max, floor_func),
             count);
            break;
            TYPE_CASE(i64)
            (arg,
             out,
             double_to_int<int64_t>(min, ceil_func),
             double_to_int<int64_t>(max, floor_func),
             count);
            break;
            TYPE_CASE(u8)
            (arg,
             out,
             double_to_int<uint8_t>(min, ceil_func),
             double_to_int<uint8_t>(max, floor_func),
             count);
            break;
            TYPE_CASE(u16)
            (arg,
             out,
             double_to_int<uint16_t>(min, ceil_func),
             double_to_int<uint16_t>(max, floor_func),
             count);
            break;
            TYPE_CASE(u32)
            (arg,
             out,
             double_to_int<uint32_t>(min, ceil_func),
             double_to_int<uint32_t>(max, floor_func),
             count);
            break;
            TYPE_CASE(u64)
            (arg,
             out,
             double_to_int<uint64_t>(min, ceil_func),
             double_to_int<uint64_t>(max, floor_func),
             count);
            break;
            TYPE_CASE(f32)(arg, out, static_cast<float>(min), static_cast<float>(max), count);
            break;
            TYPE_CASE(f64)(arg, out, min, max, count);
            break;
        default: rc = false; break;
        }
        return rc;
    }
}

bool op::v0::Clamp::evaluate(const HostTensorVector& outputs, const HostTensorVector& inputs)
{
    return evaluate_clamp(
        inputs[0], outputs[0], get_min(), get_max(), shape_size(get_input_shape(0)));
}

op::Clamp::Clamp(const Output<Node>& data, const double min, const double max)
    : FusedOp({data})
    , m_min{min}
    , m_max{max}
{
    constructor_validate_and_infer_types();
}

void op::Clamp::pre_validate_and_infer_types()
{
    NODE_VALIDATION_CHECK(
        this, m_min < m_max, "The 'min' parameter needs to be less than 'max' for Clamp");
    set_output_type(0, get_input_element_type(0), get_input_partial_shape(0));
}

NodeVector op::Clamp::decompose_op() const
{
    const auto data = input_value(0);
    const auto type = data.get_element_type();
    const auto shape = data.get_shape();

    auto ceil_func = [](double x) { return std::ceil(x); };
    auto floor_func = [](double x) { return std::floor(x); };

    shared_ptr<Node> clamp_min, clamp_max;

    switch (type)
    {
    case element::Type_t::i8:
    {
        clamp_min = builder::make_constant(type, shape, double_to_int<int8_t>(m_min, ceil_func));
        clamp_max = builder::make_constant(type, shape, double_to_int<int8_t>(m_max, floor_func));
        break;
    }
    case element::Type_t::i16:
    {
        clamp_min = builder::make_constant(type, shape, double_to_int<int16_t>(m_min, ceil_func));
        clamp_max = builder::make_constant(type, shape, double_to_int<int16_t>(m_max, floor_func));
        break;
    }
    case element::Type_t::i32:
    {
        clamp_min = builder::make_constant(type, shape, double_to_int<int32_t>(m_min, ceil_func));
        clamp_max = builder::make_constant(type, shape, double_to_int<int32_t>(m_max, floor_func));
        break;
    }
    case element::Type_t::i64:
    {
        clamp_min = builder::make_constant(type, shape, double_to_int<int64_t>(m_min, ceil_func));
        clamp_max = builder::make_constant(type, shape, double_to_int<int64_t>(m_max, floor_func));
        break;
    }
    case element::Type_t::u8:
    {
        clamp_min = builder::make_constant(type, shape, double_to_int<uint8_t>(m_min, ceil_func));
        clamp_max = builder::make_constant(type, shape, double_to_int<uint8_t>(m_max, floor_func));
        break;
    }
    case element::Type_t::u16:
    {
        clamp_min = builder::make_constant(type, shape, double_to_int<uint16_t>(m_min, ceil_func));
        clamp_max = builder::make_constant(type, shape, double_to_int<uint16_t>(m_max, floor_func));
        break;
    }
    case element::Type_t::u32:
    {
        clamp_min = builder::make_constant(type, shape, double_to_int<uint32_t>(m_min, ceil_func));
        clamp_max = builder::make_constant(type, shape, double_to_int<uint32_t>(m_max, floor_func));
        break;
    }
    case element::Type_t::u64:
    {
        clamp_min = builder::make_constant(type, shape, double_to_int<uint64_t>(m_min, ceil_func));
        clamp_max = builder::make_constant(type, shape, double_to_int<uint64_t>(m_max, floor_func));
        break;
    }
    case element::Type_t::f32:
    {
        clamp_min = builder::make_constant(type, shape, static_cast<float>(m_min));
        clamp_max = builder::make_constant(type, shape, static_cast<float>(m_max));
        break;
    }
    case element::Type_t::f64:
    {
        clamp_min = builder::make_constant(type, shape, m_min);
        clamp_max = builder::make_constant(type, shape, m_max);
        break;
    }
    default: throw runtime_error("Unsupported data type in op Clamp"); break;
    }

    auto max = std::make_shared<ngraph::op::Maximum>(clamp_min, data);
    return {std::make_shared<ngraph::op::Minimum>(clamp_max, max)};
}

shared_ptr<Node> op::Clamp::clone_with_new_inputs(const OutputVector& new_args) const
{
    NODE_VALIDATION_CHECK(this,
                          new_args.size() == 1,
                          "Expected 1 element in new_args for the Clamp op but got ",
                          new_args.size());

    return make_shared<Clamp>(new_args.at(0), m_min, m_max);
}

bool op::Clamp::visit_attributes(AttributeVisitor& visitor)
{
    visitor.on_attribute("min", m_min);
    visitor.on_attribute("max", m_max);
    return true;
}
<|endoftext|>


<commit_before>// Copyright 2004-present Facebook. All Rights Reserved.

#include "yarpl/observable/Subscriptions.h"
#include <atomic>
#include <iostream>
#include <glog/logging.h>

namespace yarpl {
namespace observable {

/**
 * Implementation that allows checking if a Subscription is cancelled.
 */
void Subscription::cancel() {
  cancelled_ = true;
  for(auto& subscription : *tiedSubscriptions_.rlock()) {
    subscription->cancel();
  }
}

bool Subscription::isCancelled() const {
  return cancelled_;
}

void Subscription::tieSubscription(Reference<Subscription> subscription) {
  CHECK(subscription);
  if (isCancelled()) {
    subscription->cancel();
  }
  tiedSubscriptions_->push_back(std::move(subscription));
}

/**
 * Implementation that gets a callback when cancellation occurs.
 */
CallbackSubscription::CallbackSubscription(std::function<void()> onCancel)
    : onCancel_(std::move(onCancel)) {}

void CallbackSubscription::cancel() {
  bool expected = false;
  // mark cancelled 'true' and only if successful invoke 'onCancel()'
  if (cancelled_.compare_exchange_strong(expected, true)) {
    onCancel_();
    for(auto& subscription : *tiedSubscriptions_.rlock()) {
      subscription->cancel();
    }
  }
}

Reference<Subscription> Subscriptions::create(std::function<void()> onCancel) {
  return make_ref<CallbackSubscription>(std::move(onCancel));
}

Reference<Subscription> Subscriptions::create(std::atomic_bool& cancelled) {
  return create([&cancelled]() { cancelled = true; });
}

Reference<Subscription> Subscriptions::create() {
  return make_ref<Subscription>();
}

}
}
<commit_msg>Fix unsynchronized access on subscription cancellation (#832)<commit_after>// Copyright 2004-present Facebook. All Rights Reserved.

#include "yarpl/observable/Subscriptions.h"
#include <atomic>
#include <iostream>
#include <glog/logging.h>

namespace yarpl {
namespace observable {

/**
 * Implementation that allows checking if a Subscription is cancelled.
 */
void Subscription::cancel() {
  cancelled_ = true;
  // Lock must be obtained here and not in the range expression for it to
  // apply to the loop body.
  auto locked = tiedSubscriptions_.wlock();
  for(auto& subscription : *locked) {
    subscription->cancel();
  }
}

bool Subscription::isCancelled() const {
  return cancelled_;
}

void Subscription::tieSubscription(Reference<Subscription> subscription) {
  CHECK(subscription);
  if (isCancelled()) {
    subscription->cancel();
  }
  tiedSubscriptions_->push_back(std::move(subscription));
}

/**
 * Implementation that gets a callback when cancellation occurs.
 */
CallbackSubscription::CallbackSubscription(std::function<void()> onCancel)
    : onCancel_(std::move(onCancel)) {}

void CallbackSubscription::cancel() {
  bool expected = false;
  // mark cancelled 'true' and only if successful invoke 'onCancel()'
  if (cancelled_.compare_exchange_strong(expected, true)) {
    onCancel_();
    // Lock must be obtained here and not in the range expression for it to
    // apply to the loop body.
    auto locked = tiedSubscriptions_.wlock();
    for(auto& subscription : *locked) {
      subscription->cancel();
    }
  }
}

Reference<Subscription> Subscriptions::create(std::function<void()> onCancel) {
  return make_ref<CallbackSubscription>(std::move(onCancel));
}

Reference<Subscription> Subscriptions::create(std::atomic_bool& cancelled) {
  return create([&cancelled]() { cancelled = true; });
}

Reference<Subscription> Subscriptions::create() {
  return make_ref<Subscription>();
}

}
}
<|endoftext|>


<commit_before>#include "sampling.h"
#include "config.h"
#include "sys_config.h"
#include "arch/MGSystem.h"

#include <algorithm>
#include <map>
#include <vector>
#include <ctime>
#include <fnmatch.h>
#include <unistd.h>

struct VarInfo
{
    void *                 var;
    size_t                 width;
    SampleVariableDataType type;
    SampleVariableCategory cat;
    std::vector<char>      max;

    VarInfo() {};
};

typedef std::map<std::string, VarInfo> var_registry_t;

static
var_registry_t registry;

void _RegisterSampleVariable(void *var, size_t width, const std::string& name, SampleVariableDataType type, SampleVariableCategory cat, void *maxval)
{
    assert (registry.find(name) == registry.end()); // no duplicates allowed.

    VarInfo vinfo;

    vinfo.var = var;
    vinfo.width = width;
    vinfo.type = type;
    vinfo.cat = cat;

    const char *maxdata = (const char*)maxval;
    for (size_t i = 0; i < width; ++i)
        vinfo.max.push_back(maxdata[i]);

    registry[name] = vinfo;
}

static
void ListSampleVariables_header(std::ostream& os)
{
    os << "# size\ttype\tdtype\tmax\taddress\tname" << std::endl;
}

static
void ListSampleVariables_onevar(std::ostream& os, const std::string& name, const VarInfo& vinfo)
{
    os << vinfo.width << "\t";

    switch(vinfo.cat)
    {
    case SVC_LEVEL: os << "level"; break;
    case SVC_STATE: os << "state"; break;
    case SVC_WATERMARK: os << "wmark"; break;
    case SVC_CUMULATIVE: os << "cumul"; break;
    default: os << "unknown"; break;
    }
    os << (const char*)((vinfo.type == SV_INTEGER) ? "\tint\t" : "\tfloat\t");

    if (vinfo.cat == SVC_LEVEL || vinfo.cat == SVC_WATERMARK)
    {
        const void *p = &vinfo.max[0];
        switch(vinfo.type) {
        case SV_INTEGER:
            switch(vinfo.width) {
            case 1: os << std::dec << *(uint8_t*)p; break;
            case 2: os << std::dec << *(uint16_t*)p; break;
            case 4: os << std::dec << *(uint32_t*)p; break;
            case 8: os << std::dec << *(uint64_t*)p; break;
            default: os << "<invsize>"; break;
            }
            break;
        case SV_FLOAT:
            if (vinfo.width == sizeof(float))
                os << *(float*)p;
            else
                os << *(double*)p;
            break;
        }
    }
    else
        os << "N/A";

    os << '\t' << vinfo.var << '\t'
       << name
       << std::endl;
}

void ListSampleVariables(std::ostream& os, const std::string& pat)
{
    ListSampleVariables_header(os);
    for (var_registry_t::const_iterator i = registry.begin();
         i != registry.end();
         ++i)
    {
        if (FNM_NOMATCH == fnmatch(pat.c_str(), i->first.c_str(), FNM_CASEFOLD))
            continue;
        ListSampleVariables_onevar(os, i->first, i->second);
    }
}

bool ReadSampleVariables(std::ostream& os, const std::string& pat)
{
    bool some = false;
    for (var_registry_t::const_iterator i = registry.begin();
         i != registry.end();
         ++i)
    {
        if (FNM_NOMATCH == fnmatch(pat.c_str(), i->first.c_str(), FNM_CASEFOLD))
            continue;

        os << i->first << " = ";

        const VarInfo& vinfo = i->second;
        void *p = vinfo.var;
        switch(vinfo.type) {
        case SV_INTEGER:
            switch(vinfo.width) {
            case 1: os << std::dec << *(uint8_t*)p; break;
            case 2: os << std::dec << *(uint16_t*)p; break;
            case 4: os << std::dec << *(uint32_t*)p; break;
            case 8: os << std::dec << *(uint64_t*)p; break;
            default: os << "<invsize>"; break;
            }
            break;
        case SV_FLOAT:
            if (vinfo.width == sizeof(float))
                os << *(float*)p;
            else
                os << *(double*)p;
            break;
        }
        os << std::endl;
        some = true;
    }
    return some;
}

typedef std::pair<const std::string*, const VarInfo*> varsel_t;
typedef std::vector<varsel_t> varvec_t;

static
bool comparevars(const varsel_t& left, const varsel_t& right)
{
    return left.second->var < right.second->var;
}

BinarySampler::BinarySampler(std::ostream& os, const Config& config,
                             const std::vector<std::string>& pats)
    : m_datasize(0)
{

    varvec_t vars;

    //
    // Select variables to sample
    //

    for (std::vector<std::string>::const_iterator i = pats.begin(); i != pats.end(); ++i)
        for (var_registry_t::const_iterator j = registry.begin();
             j != registry.end();
             ++j)
    {
        if (FNM_NOMATCH == fnmatch(i->c_str(), j->first.c_str(), FNM_CASEFOLD))
            continue;
        vars.push_back(std::make_pair(&j->first, &j->second));
    }

    if (vars.size() >= 2)
        // we sort everything but the first and last variables,
        // which should be the cycle counters. The cycle counters
        // must be sampled once before and after everything else,
        // to evaluate how imprecise the measurement is.
        std::sort(vars.begin()+1, vars.end()-1, comparevars);

    //
    // Generate header for output file
    //
    time_t cl = time(0);
    std::string timestr = asctime(gmtime(&cl));

    os << "# date: " << timestr // asctime already embeds a newline character
       << "# generator: " << PACKAGE_STRING << std::endl;

    char hn[255];
    if (gethostname(hn, 255) == 0)
        os << "# host: " << hn << std::endl;

    os << "# configuration:" << std::endl;
    std::vector<std::pair<std::string, std::string> > rawconf = config.getRawConfiguration();
    for (size_t i = 0; i < rawconf.size(); ++i)
        os << "# " << rawconf[i].first << " = " << rawconf[i].second << std::endl;

    os << "# varinfo: " << vars.size() << std::endl;
    // ListSampleVariables_header(os);
    for (varvec_t::const_iterator i = vars.begin(); i != vars.end(); ++i)
    {
        m_datasize += i->second->width;
        m_vars.push_back(std::make_pair((const char*)i->second->var, i->second->width));
        ListSampleVariables_onevar(os, *i->first, *i->second);
    }
    os << "# recwidth: " << m_datasize << std::endl;
}
<commit_msg>[mgsim-refactor] Fix reporting of 1-byte sampling variables.<commit_after>#include "sampling.h"
#include "config.h"
#include "sys_config.h"
#include "arch/MGSystem.h"

#include <algorithm>
#include <map>
#include <vector>
#include <ctime>
#include <fnmatch.h>
#include <unistd.h>

struct VarInfo
{
    void *                 var;
    size_t                 width;
    SampleVariableDataType type;
    SampleVariableCategory cat;
    std::vector<char>      max;

    VarInfo() {};
};

typedef std::map<std::string, VarInfo> var_registry_t;

static
var_registry_t registry;

void _RegisterSampleVariable(void *var, size_t width, const std::string& name, SampleVariableDataType type, SampleVariableCategory cat, void *maxval)
{
    assert (registry.find(name) == registry.end()); // no duplicates allowed.

    VarInfo vinfo;

    vinfo.var = var;
    vinfo.width = width;
    vinfo.type = type;
    vinfo.cat = cat;

    const char *maxdata = (const char*)maxval;
    for (size_t i = 0; i < width; ++i)
        vinfo.max.push_back(maxdata[i]);

    registry[name] = vinfo;
}

static
void ListSampleVariables_header(std::ostream& os)
{
    os << "# size\ttype\tdtype\tmax\taddress\tname" << std::endl;
}

static
void ListSampleVariables_onevar(std::ostream& os, const std::string& name, const VarInfo& vinfo)
{
    os << vinfo.width << "\t";

    switch(vinfo.cat)
    {
    case SVC_LEVEL: os << "level"; break;
    case SVC_STATE: os << "state"; break;
    case SVC_WATERMARK: os << "wmark"; break;
    case SVC_CUMULATIVE: os << "cumul"; break;
    default: os << "unknown"; break;
    }
    os << (const char*)((vinfo.type == SV_INTEGER) ? "\tint\t" : "\tfloat\t");

    if (vinfo.cat == SVC_LEVEL || vinfo.cat == SVC_WATERMARK)
    {
        const void *p = &vinfo.max[0];
        switch(vinfo.type) {
        case SV_INTEGER:
            switch(vinfo.width) {
            case 1: os << std::dec << (unsigned)*(uint8_t*)p; break;
            case 2: os << std::dec << *(uint16_t*)p; break;
            case 4: os << std::dec << *(uint32_t*)p; break;
            case 8: os << std::dec << *(uint64_t*)p; break;
            default: os << "<invsize>"; break;
            }
            break;
        case SV_FLOAT:
            if (vinfo.width == sizeof(float))
                os << *(float*)p;
            else
                os << *(double*)p;
            break;
        }
    }
    else
        os << "N/A";

    os << '\t' << vinfo.var << '\t'
       << name
       << std::endl;
}

void ListSampleVariables(std::ostream& os, const std::string& pat)
{
    ListSampleVariables_header(os);
    for (var_registry_t::const_iterator i = registry.begin();
         i != registry.end();
         ++i)
    {
        if (FNM_NOMATCH == fnmatch(pat.c_str(), i->first.c_str(), FNM_CASEFOLD))
            continue;
        ListSampleVariables_onevar(os, i->first, i->second);
    }
}

bool ReadSampleVariables(std::ostream& os, const std::string& pat)
{
    bool some = false;
    for (var_registry_t::const_iterator i = registry.begin();
         i != registry.end();
         ++i)
    {
        if (FNM_NOMATCH == fnmatch(pat.c_str(), i->first.c_str(), FNM_CASEFOLD))
            continue;

        os << i->first << " = ";

        const VarInfo& vinfo = i->second;
        void *p = vinfo.var;
        switch(vinfo.type) {
        case SV_INTEGER:
            switch(vinfo.width) {
            case 1: os << std::dec << (unsigned)*(uint8_t*)p; break;
            case 2: os << std::dec << *(uint16_t*)p; break;
            case 4: os << std::dec << *(uint32_t*)p; break;
            case 8: os << std::dec << *(uint64_t*)p; break;
            default: os << "<invsize>"; break;
            }
            break;
        case SV_FLOAT:
            if (vinfo.width == sizeof(float))
                os << *(float*)p;
            else
                os << *(double*)p;
            break;
        }
        os << std::endl;
        some = true;
    }
    return some;
}

typedef std::pair<const std::string*, const VarInfo*> varsel_t;
typedef std::vector<varsel_t> varvec_t;

static
bool comparevars(const varsel_t& left, const varsel_t& right)
{
    return left.second->var < right.second->var;
}

BinarySampler::BinarySampler(std::ostream& os, const Config& config,
                             const std::vector<std::string>& pats)
    : m_datasize(0)
{

    varvec_t vars;

    //
    // Select variables to sample
    //

    for (std::vector<std::string>::const_iterator i = pats.begin(); i != pats.end(); ++i)
        for (var_registry_t::const_iterator j = registry.begin();
             j != registry.end();
             ++j)
    {
        if (FNM_NOMATCH == fnmatch(i->c_str(), j->first.c_str(), FNM_CASEFOLD))
            continue;
        vars.push_back(std::make_pair(&j->first, &j->second));
    }

    if (vars.size() >= 2)
        // we sort everything but the first and last variables,
        // which should be the cycle counters. The cycle counters
        // must be sampled once before and after everything else,
        // to evaluate how imprecise the measurement is.
        std::sort(vars.begin()+1, vars.end()-1, comparevars);

    //
    // Generate header for output file
    //
    time_t cl = time(0);
    std::string timestr = asctime(gmtime(&cl));

    os << "# date: " << timestr // asctime already embeds a newline character
       << "# generator: " << PACKAGE_STRING << std::endl;

    char hn[255];
    if (gethostname(hn, 255) == 0)
        os << "# host: " << hn << std::endl;

    os << "# configuration:" << std::endl;
    std::vector<std::pair<std::string, std::string> > rawconf = config.getRawConfiguration();
    for (size_t i = 0; i < rawconf.size(); ++i)
        os << "# " << rawconf[i].first << " = " << rawconf[i].second << std::endl;

    os << "# varinfo: " << vars.size() << std::endl;
    // ListSampleVariables_header(os);
    for (varvec_t::const_iterator i = vars.begin(); i != vars.end(); ++i)
    {
        m_datasize += i->second->width;
        m_vars.push_back(std::make_pair((const char*)i->second->var, i->second->width));
        ListSampleVariables_onevar(os, *i->first, *i->second);
    }
    os << "# recwidth: " << m_datasize << std::endl;
}
<|endoftext|>


<commit_before>// The libMesh Finite Element Library.
// Copyright (C) 2002-2018 Benjamin S. Kirk, John W. Peterson, Roy H. Stogner

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

// Local Includes
#include "libmesh/preconditioner.h"
#include "libmesh/eigen_preconditioner.h"
#include "libmesh/petsc_preconditioner.h"
#include "libmesh/trilinos_preconditioner.h"

namespace libMesh
{

template <typename T>
std::unique_ptr<Preconditioner<T>>
Preconditioner<T>::build_preconditioner(const libMesh::Parallel::Communicator & comm,
                                        const SolverPackage solver_package)
{
  // Avoid unused parameter warnings when no solver packages are enabled.
  libmesh_ignore(comm);

  // Build and return the appropriate Preconditioner object.
  switch (solver_package)
    {

#ifdef LIBMESH_HAVE_PETSC
    case PETSC_SOLVERS:
      {
        return libmesh_make_unique<PetscPreconditioner<T>>(comm);
      }
#endif

#ifdef LIBMESH_TRILINOS_HAVE_EPETRA
    case TRILINOS_SOLVERS:
      return libmesh_make_unique<TrilinosPreconditioner<T>>(comm);
#endif

#ifdef LIBMESH_HAVE_EIGEN
    case EIGEN_SOLVERS:
      return libmesh_make_unique<EigenPreconditioner<T>>(comm);
#endif

    default:
      libmesh_error_msg("ERROR:  Unrecognized solver package: " << solver_package);
    }
}

#ifdef LIBMESH_ENABLE_DEPRECATED

template <typename T>
Preconditioner<T> *
Preconditioner<T>::build(const libMesh::Parallel::Communicator & comm,
                         const SolverPackage solver_package)
{
  // You should be calling build_preconditioner() instead.
  libmesh_deprecated();

  // Call the non-deprecated method
  std::unique_ptr<Preconditioner<T>> ptr =
    Preconditioner<T>::build_preconditioner(comm, solver_package);

  // Vaya con dios
  return ptr.release();
}

#endif

//------------------------------------------------------------------
// Explicit instantiations
template class Preconditioner<Number>;

} // namespace libMesh
<commit_msg>Add header for libmesh_make_unique<commit_after>// The libMesh Finite Element Library.
// Copyright (C) 2002-2018 Benjamin S. Kirk, John W. Peterson, Roy H. Stogner

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

// Local Includes
#include "libmesh/preconditioner.h"

#include "libmesh/auto_ptr.h"
#include "libmesh/eigen_preconditioner.h"
#include "libmesh/petsc_preconditioner.h"
#include "libmesh/trilinos_preconditioner.h"

namespace libMesh
{

template <typename T>
std::unique_ptr<Preconditioner<T>>
Preconditioner<T>::build_preconditioner(const libMesh::Parallel::Communicator & comm,
                                        const SolverPackage solver_package)
{
  // Avoid unused parameter warnings when no solver packages are enabled.
  libmesh_ignore(comm);

  // Build and return the appropriate Preconditioner object.
  switch (solver_package)
    {

#ifdef LIBMESH_HAVE_PETSC
    case PETSC_SOLVERS:
      {
        return libmesh_make_unique<PetscPreconditioner<T>>(comm);
      }
#endif

#ifdef LIBMESH_TRILINOS_HAVE_EPETRA
    case TRILINOS_SOLVERS:
      return libmesh_make_unique<TrilinosPreconditioner<T>>(comm);
#endif

#ifdef LIBMESH_HAVE_EIGEN
    case EIGEN_SOLVERS:
      return libmesh_make_unique<EigenPreconditioner<T>>(comm);
#endif

    default:
      libmesh_error_msg("ERROR:  Unrecognized solver package: " << solver_package);
    }
}

#ifdef LIBMESH_ENABLE_DEPRECATED

template <typename T>
Preconditioner<T> *
Preconditioner<T>::build(const libMesh::Parallel::Communicator & comm,
                         const SolverPackage solver_package)
{
  // You should be calling build_preconditioner() instead.
  libmesh_deprecated();

  // Call the non-deprecated method
  std::unique_ptr<Preconditioner<T>> ptr =
    Preconditioner<T>::build_preconditioner(comm, solver_package);

  // Vaya con dios
  return ptr.release();
}

#endif

//------------------------------------------------------------------
// Explicit instantiations
template class Preconditioner<Number>;

} // namespace libMesh
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: dlgsize.cxx,v $
 *
 *  $Revision: 1.4 $
 *
 *  last change: $Author: rt $ $Date: 2005-09-08 15:01:34 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/
#ifndef DBAUI_DLGSIZE_HRC
#include "dlgsize.hrc"
#endif
#ifndef _DBAUI_DLGSIZE_HXX
#include "dlgsize.hxx"
#endif
#ifndef _DBAUI_MODULE_DBU_HXX_
#include "moduledbu.hxx"
#endif
#ifndef _DBU_DLG_HRC_
#include "dbu_dlg.hrc"
#endif

//.........................................................................
namespace dbaui
{
//.........................................................................

#define DEF_ROW_HEIGHT  45
#define DEF_COL_WIDTH   227

//==================================================================
DlgSize::DlgSize( Window* pParent, sal_Int32 nVal, sal_Bool bRow, sal_Int32 _nAlternativeStandard )
        :ModalDialog( pParent, ModuleRes(bRow ? DLG_ROWHEIGHT : DLG_COLWIDTH))
        ,aFT_VALUE(this,    ResId( FT_VALUE))
        ,aMF_VALUE(this,    ResId( MF_VALUE))
        ,aCB_STANDARD(this, ResId(CB_STANDARD))
        ,aPB_OK(this,       ResId(PB_OK))
        ,aPB_CANCEL(this,   ResId(PB_CANCEL))
        ,aPB_HELP(this,     ResId(PB_HELP))
        ,m_nPrevValue(nVal)
        ,m_nStandard(bRow ? DEF_ROW_HEIGHT : DEF_COL_WIDTH)
{
    if ( _nAlternativeStandard > 0 )
        m_nStandard = _nAlternativeStandard;
    aCB_STANDARD.SetClickHdl(LINK(this,DlgSize,CbClickHdl));

    aMF_VALUE.EnableEmptyFieldValue(sal_True);
    sal_Bool bDefault = -1 == nVal;
    aCB_STANDARD.Check(bDefault);
    if (bDefault)
    {
        SetValue(m_nStandard);
        m_nPrevValue = m_nStandard;
    }
    LINK(this,DlgSize,CbClickHdl).Call(&aCB_STANDARD);

    FreeResource();
}

//------------------------------------------------------------------------------
DlgSize::~DlgSize()
{
}

//------------------------------------------------------------------------------
void DlgSize::SetValue( sal_Int32 nVal )
{
    aMF_VALUE.SetValue(nVal, FUNIT_CM );
}

//------------------------------------------------------------------------------
sal_Int32 DlgSize::GetValue()
{
    if (aCB_STANDARD.IsChecked())
        return -1;
    return (sal_Int32)aMF_VALUE.GetValue( FUNIT_CM );
}

//------------------------------------------------------------------------------
IMPL_LINK( DlgSize, CbClickHdl, Button *, pButton )
{

    if( pButton == &aCB_STANDARD )
    {
        aMF_VALUE.Enable(!aCB_STANDARD.IsChecked());
        if (aCB_STANDARD.IsChecked())
        {
            m_nPrevValue = aMF_VALUE.GetValue(FUNIT_CM);
                // don't use getValue as this will use aCB_STANDARD.to determine if we're standard
            aMF_VALUE.SetEmptyFieldValue();
        }
        else
        {
            SetValue( m_nPrevValue );
        }
    }
    return 0;
}
// -----------------------------------------------------------------------------
//.........................................................................
}   // namespace dbaui
//.........................................................................

<commit_msg>INTEGRATION: CWS dba201b (1.3.418); FILE MERGED 2005/09/21 08:37:37 oj 1.3.418.2: RESYNC: (1.3-1.4); FILE MERGED 2005/07/11 13:37:20 fs 1.3.418.1: merging CWS dba201 into CWS dba201b<commit_after>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: dlgsize.cxx,v $
 *
 *  $Revision: 1.5 $
 *
 *  last change: $Author: hr $ $Date: 2005-09-23 12:30:46 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/
#ifndef DBAUI_DLGSIZE_HRC
#include "dlgsize.hrc"
#endif
#ifndef _DBAUI_DLGSIZE_HXX
#include "dlgsize.hxx"
#endif
#ifndef _DBAUI_MODULE_DBU_HXX_
#include "moduledbu.hxx"
#endif
#ifndef _DBU_DLG_HRC_
#include "dbu_dlg.hrc"
#endif

//.........................................................................
namespace dbaui
{
//.........................................................................

#define DEF_ROW_HEIGHT  45
#define DEF_COL_WIDTH   227

DBG_NAME(DlgSize)
//==================================================================
DlgSize::DlgSize( Window* pParent, sal_Int32 nVal, sal_Bool bRow, sal_Int32 _nAlternativeStandard )
        :ModalDialog( pParent, ModuleRes(bRow ? DLG_ROWHEIGHT : DLG_COLWIDTH))
        ,aFT_VALUE(this,    ResId( FT_VALUE))
        ,aMF_VALUE(this,    ResId( MF_VALUE))
        ,aCB_STANDARD(this, ResId(CB_STANDARD))
        ,aPB_OK(this,       ResId(PB_OK))
        ,aPB_CANCEL(this,   ResId(PB_CANCEL))
        ,aPB_HELP(this,     ResId(PB_HELP))
        ,m_nPrevValue(nVal)
        ,m_nStandard(bRow ? DEF_ROW_HEIGHT : DEF_COL_WIDTH)
{
    DBG_CTOR(DlgSize,NULL);

    if ( _nAlternativeStandard > 0 )
        m_nStandard = _nAlternativeStandard;
    aCB_STANDARD.SetClickHdl(LINK(this,DlgSize,CbClickHdl));

    aMF_VALUE.EnableEmptyFieldValue(sal_True);
    sal_Bool bDefault = -1 == nVal;
    aCB_STANDARD.Check(bDefault);
    if (bDefault)
    {
        SetValue(m_nStandard);
        m_nPrevValue = m_nStandard;
    }
    LINK(this,DlgSize,CbClickHdl).Call(&aCB_STANDARD);

    FreeResource();
}

//------------------------------------------------------------------------------
DlgSize::~DlgSize()
{

    DBG_DTOR(DlgSize,NULL);
}

//------------------------------------------------------------------------------
void DlgSize::SetValue( sal_Int32 nVal )
{
    aMF_VALUE.SetValue(nVal, FUNIT_CM );
}

//------------------------------------------------------------------------------
sal_Int32 DlgSize::GetValue()
{
    if (aCB_STANDARD.IsChecked())
        return -1;
    return (sal_Int32)aMF_VALUE.GetValue( FUNIT_CM );
}

//------------------------------------------------------------------------------
IMPL_LINK( DlgSize, CbClickHdl, Button *, pButton )
{

    if( pButton == &aCB_STANDARD )
    {
        aMF_VALUE.Enable(!aCB_STANDARD.IsChecked());
        if (aCB_STANDARD.IsChecked())
        {
            m_nPrevValue = aMF_VALUE.GetValue(FUNIT_CM);
                // don't use getValue as this will use aCB_STANDARD.to determine if we're standard
            aMF_VALUE.SetEmptyFieldValue();
        }
        else
        {
            SetValue( m_nPrevValue );
        }
    }
    return 0;
}
// -----------------------------------------------------------------------------
//.........................................................................
}   // namespace dbaui
//.........................................................................

<|endoftext|>