Vector3 computeDesiredForce(const SE3Type & pose, const Vector3 & linear_velocity,
double delta_time) {
Vector3 desired_position(0,0,1);
Vector3 desired_velocity(0,0,0);
// Vector3 kp(1,1,1) ;
// Vector3 kd(1,1,1);
// Vector3 ki(0.0015,0.0015,0.0015) ;
//for ukf after redirecting to log
Vector3 kp(4,4,4) ;
Vector3 kd(3,3,3);
Vector3 ki(0.002,0.002,0.002) ;
Vector3 current_position=pose.translation();
std::cout << "\nCurrent Position" << current_position ;
Vector3 proportiona_val= kp.cwiseProduct(desired_position-current_position);
Vector3 differential_val=kd.cwiseProduct(desired_velocity-linear_velocity);
accumulated_error = accumulated_error+(desired_position-current_position) ;
Vector3 integral_value=ki.cwiseProduct(accumulated_error);
Vector3 desired_force=proportiona_val+ differential_val+integral_value;
std::cout << "\ndesired_force is" << desired_force ;
return desired_force;
}
static void maybe_move(ProxMapState* prox_map, Sensors* sens) {
Position next;
if (fabs(prox_map->lower_left.x + HALF_SIZE - sens->current.x) < 2 + FLICKER_INERTIA
&& fabs(prox_map->lower_left.y + HALF_SIZE - sens->current.y) < 1 + FLICKER_INERTIA) {
return;
}
/* Need to move. */
desired_position(&next, sens);
/* If (for example) our offset increases, the map-data needs to shift
* towards the negative. Thus, we swap the signs here and let
* map_move() believe that we are moving the map-data (and not our
* frame of reference). */
map_move(map_get_proximity(),
next.x - prox_map->lower_left.x,
next.y - prox_map->lower_left.y);
prox_map->lower_left = next;
}
int main(int argc, char** argv){
ros::init(argc, argv, "square_moving_example");
ros::NodeHandle nh;
ros::Publisher trajectory_pub =
nh.advertise<trajectory_msgs::MultiDOFJointTrajectory>(
mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);
ROS_INFO("Started square moving example.");
std_srvs::Empty srv;
bool unpaused = ros::service::call("/gazebo/unpause_physics", srv);
unsigned int i = 0;
// Trying to unpause Gazebo for 10 seconds.
while (i <= 10 && !unpaused) {
ROS_INFO("Wait for 1 second before trying to unpause Gazebo again.");
std::this_thread::sleep_for(std::chrono::seconds(1));
unpaused = ros::service::call("/gazebo/unpause_physics", srv);
++i;
}
if (!unpaused) {
ROS_FATAL("Could not wake up Gazebo.");
return -1;
}
else {
ROS_INFO("Unpaused the Gazebo simulation.");
}
// Wait for 5 seconds to let the Gazebo GUI show up.
ros::Duration(5.0).sleep();
trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;
trajectory_msg.header.stamp = ros::Time::now();
Eigen::Vector3d desired_position(0.0, 0.0, 1.0);
double desired_yaw = 0.0;
mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(desired_position,
desired_yaw, &trajectory_msg);
ROS_INFO("Publishing waypoint on namespace %s: [%f, %f, %f].",
nh.getNamespace().c_str(),
desired_position.x(),
desired_position.y(),
desired_position.z());
trajectory_pub.publish(trajectory_msg);
// Wait for 2 seconds to send the point coordinates.
ros::Duration(2.0).sleep();
trajectory_msg.header.stamp = ros::Time::now();
desired_position[0]=2.0;
desired_yaw = 0.0;
mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(desired_position,
desired_yaw, &trajectory_msg);
ROS_INFO("Publishing waypoint on namespace %s: [%f, %f, %f].",
nh.getNamespace().c_str(),
desired_position.x(),
desired_position.y(),
desired_position.z());
trajectory_pub.publish(trajectory_msg);
// Wait for 2 seconds to send the point coordinates.
ros::Duration(2.0).sleep();
trajectory_msg.header.stamp = ros::Time::now();
desired_position[1]=2.0;
desired_yaw = 0.0;
mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(desired_position,
desired_yaw, &trajectory_msg);
ROS_INFO("Publishing waypoint on namespace %s: [%f, %f, %f].",
nh.getNamespace().c_str(),
desired_position.x(),
desired_position.y(),
desired_position.z());
trajectory_pub.publish(trajectory_msg);
// Wait for 2 seconds to send the point coordinates.
ros::Duration(2.0).sleep();
trajectory_msg.header.stamp = ros::Time::now();
desired_position[0]=0.0;
desired_yaw = 0.0;
mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(desired_position,
desired_yaw, &trajectory_msg);
ROS_INFO("Publishing waypoint on namespace %s: [%f, %f, %f].",
nh.getNamespace().c_str(),
desired_position.x(),
desired_position.y(),
desired_position.z());
trajectory_pub.publish(trajectory_msg);
// Wait for 2 seconds to send the point coordinates.
ros::Duration(2.0).sleep();
trajectory_msg.header.stamp = ros::Time::now();
desired_position[1]=0.0;
desired_yaw = 0.0;
mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(desired_position,
desired_yaw, &trajectory_msg);
ROS_INFO("Publishing waypoint on namespace %s: [%f, %f, %f].",
nh.getNamespace().c_str(),
desired_position.x(),
desired_position.y(),
desired_position.z());
trajectory_pub.publish(trajectory_msg);
ros::spin();
}
void proximity_reset(ProxMapState* prox_map, Sensors* sens) {
desired_position(&prox_map->lower_left, sens);
map_clear(map_get_proximity());
prox_map->time_sent = hal_get_time();
}
int main(int argc, char** argv) {
ros::init(argc, argv, "Demo_obstacle");
ros::NodeHandle nh;
trajectory_pub = nh.advertise<trajectory_msgs::MultiDOFJointTrajectory>(
mav_msgs::default_topics::COMMAND_TRAJECTORY, 1);
ros::Subscriber sub = nh.subscribe<std_msgs::Bool>("pclsize",1,callback);
ros::Subscriber waypoint_sub = nh.subscribe<geometry_msgs::Vector3Stamped>("new_waypoint",1,waypoint_sub_callback);
// ros::Subscriber sub = nh.subscribe<pcl::PointCloud<pcl::PointXYZ>>("filteredpoints",1,callback);
ros::Duration(2).sleep();
ros::Rate r(20); //10 Hz
while(ros::ok())
{
if(init == 1)
{
p.x= p.x+0.1;
p.y= 0;
p.z= 1;
}
if(init == 0 )
{
p.x= 0;
p.y= 0;
p.z= 1;
p.yaw=0;
init =1;
}
//std_msgs::Bool* obs = const_cast<std_msgs::Bool*>(&obstacle);
// if(filter_cloud.points.size()>1 && init !=0)
// bool obs = obstacle;
trajectory_msg.header.stamp = ros::Time::now();
Eigen::Vector3d desired_position(p.x, p.y, p.z);
const float DEG_2_RAD = M_PI / 180.0;
double desired_yaw = p.yaw * DEG_2_RAD;
mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(desired_position, desired_yaw, &trajectory_msg);
ROS_INFO("Publishing waypoint: [%f, %f, %f].",
desired_position.x(),
desired_position.y(),
desired_position.z());
trajectory_pub.publish(trajectory_msg);
// if(init ==2)
// {
// r.sleep();//ros::Duration(1.5).sleep();
// }
//init =1;
ros::spinOnce();
r.sleep();
}
return 0;
}
