test_planner_2dof_serial_joints.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2017, the neonavigation authors
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * 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.
 *     * Neither the name of the copyright holder 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.
 */

#include <ros/ros.h>
#include <planner_cspace_msgs/PlannerStatus.h>
#include <sensor_msgs/JointState.h>
#include <trajectory_msgs/JointTrajectory.h>

#include <gtest/gtest.h>

TEST(Planner2DOFSerialJoints, Plan)
{
  ros::NodeHandle nh;
  ros::Publisher pub_state = nh.advertise<sensor_msgs::JointState>("joint_states", 1, true);
  ros::Publisher pub_cmd = nh.advertise<trajectory_msgs::JointTrajectory>("trajectory_in", 1, true);

  trajectory_msgs::JointTrajectory::ConstPtr planned;
  const auto cb_plan = [&planned](const trajectory_msgs::JointTrajectory::ConstPtr& msg)
  {
    planned = msg;
  };
  ros::Subscriber sub_plan = nh.subscribe<trajectory_msgs::JointTrajectory>("joint_trajectory", 1, cb_plan);

  planner_cspace_msgs::PlannerStatus::ConstPtr status;
  const auto cb_status = [&status](const planner_cspace_msgs::PlannerStatus::ConstPtr& msg)
  {
    status = msg;
  };
  ros::Subscriber sub_status = nh.subscribe<planner_cspace_msgs::PlannerStatus>(
      "/planner_2dof_serial_joints/group0/status", 1, cb_status);

  sensor_msgs::JointState s;
  s.name.push_back("front");
  s.position.push_back(-1.57);
  s.name.push_back("rear");
  s.position.push_back(0.0);

  trajectory_msgs::JointTrajectory cmd;
  cmd.joint_names.push_back("front");
  cmd.joint_names.push_back("rear");
  trajectory_msgs::JointTrajectoryPoint p;
  p.positions.push_back(-4.71);
  p.positions.push_back(0.0);
  cmd.points.push_back(p);

  ros::Rate rate(1);
  const ros::Time deadline = ros::Time::now() + ros::Duration(10);
  int cnt = 0;
  while (ros::ok())
  {
    if (ros::Time::now() > deadline)
    {
      FAIL() << "Timeout";
    }

    pub_state.publish(s);
    pub_cmd.publish(cmd);

    ros::spinOnce();
    rate.sleep();
    if (planned && status)
    {
      cnt++;
      if (cnt > 5)
      {
        break;
      }
    }
  }
  ASSERT_TRUE(ros::ok());

  ASSERT_EQ(planner_cspace_msgs::PlannerStatus::DOING, status->status);
  ASSERT_EQ(planner_cspace_msgs::PlannerStatus::GOING_WELL, status->error);

  ASSERT_EQ(2u, planned->joint_names.size());
  ASSERT_EQ("front", planned->joint_names[0]);
  ASSERT_EQ("rear", planned->joint_names[1]);

  // collision at front=-3.14, rear=0.0 must be avoided.
  const float fc = -3.14;
  const float rc = 0.0;
  for (int i = 1; i < static_cast<int>(planned->points.size()); ++i)
  {
    const trajectory_msgs::JointTrajectoryPoint& p0 = planned->points[i - 1];
    const trajectory_msgs::JointTrajectoryPoint& p1 = planned->points[i];
    ASSERT_EQ(2u, p0.positions.size());
    ASSERT_EQ(2u, p1.positions.size());

    const float f0 = p0.positions[0];
    const float r0 = p0.positions[1];
    const float f1 = p1.positions[0];
    const float r1 = p1.positions[1];

    ASSERT_LT(-6.28, f0);
    ASSERT_GT(0.0, f0);
    ASSERT_LT(-3.14, r0);
    ASSERT_GT(3.14, r0);
    ASSERT_LT(-6.28, f1);
    ASSERT_GT(0.0, f1);
    ASSERT_LT(-3.14, r1);
    ASSERT_GT(3.14, r1);

    const float front_diff = f1 - f0;
    const float rear_diff = r1 - r0;

    const float d =
        std::abs(
            rear_diff * fc -
            front_diff * rc +
            f1 * r0 -
            r1 * f0) /
        std::hypot(front_diff, rear_diff);
    // std::cerr << d << std::endl;
    ASSERT_GT(d, 0.15);

    ASSERT_EQ(planner_cspace_msgs::PlannerStatus::DOING, status->status);
    ASSERT_EQ(planner_cspace_msgs::PlannerStatus::GOING_WELL, status->error);
  }
}

TEST(Planner2DOFSerialJoints, NoPath)
{
  ros::NodeHandle nh;
  ros::Publisher pub_state = nh.advertise<sensor_msgs::JointState>("joint_states", 1, true);
  ros::Publisher pub_cmd = nh.advertise<trajectory_msgs::JointTrajectory>("trajectory_in", 1, true);

  planner_cspace_msgs::PlannerStatus::ConstPtr status;
  const auto cb_status = [&status](const planner_cspace_msgs::PlannerStatus::ConstPtr& msg)
  {
    status = msg;
  };
  ros::Subscriber sub_status = nh.subscribe<planner_cspace_msgs::PlannerStatus>(
      "/planner_2dof_serial_joints/group0/status", 1, cb_status);

  sensor_msgs::JointState s;
  s.name.push_back("front");
  s.position.push_back(-1.57);
  s.name.push_back("rear");
  s.position.push_back(0.0);

  trajectory_msgs::JointTrajectory cmd;
  cmd.joint_names.push_back("front");
  cmd.joint_names.push_back("rear");
  trajectory_msgs::JointTrajectoryPoint p;
  p.positions.push_back(3.14);
  p.positions.push_back(0.0);  // Collided state
  cmd.points.push_back(p);

  ros::Rate rate(1);
  const ros::Time deadline = ros::Time::now() + ros::Duration(10);
  int cnt = 0;
  while (ros::ok())
  {
    if (ros::Time::now() > deadline)
    {
      FAIL() << "Timeout";
    }

    pub_state.publish(s);
    pub_cmd.publish(cmd);

    ros::spinOnce();
    rate.sleep();
    if (status)
    {
      cnt++;
      if (cnt > 5)
      {
        break;
      }
    }
  }
  ASSERT_TRUE(ros::ok());

  ASSERT_EQ(planner_cspace_msgs::PlannerStatus::DOING, status->status);
  ASSERT_EQ(planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND, status->error);
}

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  ros::init(argc, argv, "test_planner_2dof_serial_joints");

  return RUN_ALL_TESTS();
}