Revision history [back]

Tully is right. You need to send the msgs, which are autogenerated by actionlib and then you are missing the support of actionlib server and/or client. Here is one messy little class that did the job for us so far.

using System;
using System.Threading;
using System.Collections;
using System.Collections.Generic;
using RosCS;
using RosCS.geometry_msgs;
using RosCS.nav_msgs;
using RosCS.tmc200_msgs;
using RosCS.actionlib_msgs;
using RosCS.move_base_msgs;
using RosCS.Services;
using RosCS.Services.nav_msgs;
using Castor;
public class ArmHelper
{
    public static Pose PointOfInterest { get; private set; }
    public static bool NewPOI { get; set; }
    public static bool ExplorationDone { get; set; }
    public static GoalID CurrentGoalID{get; private set;}
    public static GoalStatusArray GoalStatus {get; private set;}
    public static MoveBaseActionFeedback GoalFeedback {get; private set;}
    public static MoveBaseActionResult GoalResult {get; set;}
    public static bool MotionEnabled { get; set; }
    public static string MotionType { get; set; }

    protected static Node rosNode;
    protected static int id = 0;
    protected static MoveBaseActionGoal MoveBaseGoal{get; private set;}
    protected static SystemConfig sc = SystemConfig.LocalInstance;
    protected static ArmHelper instance = null;
    protected static object lockobject = new object();

    public static ArmHelper Get() {
        if(instance == null) {
            lock(lockobject) {
                if(instance == null) {
                    instance = new ArmHelper();
                    //Thread.MemoryBarrier();
                }
            }
        }
        return instance;
    }

    protected static Publisher motionPub;
    protected static Publisher goalPositionPub;
    protected static Publisher mBaseGoalPositionPub;
    protected static Publisher mBaseCancelGoalPub;

    protected static string GenerateUniqueId()
    {
        id++;
        return "goal_" + id;    
    }
    protected static GoalID GenerateActionLibGoalId()
    {
        CurrentGoalID = new GoalID();
        CurrentGoalID.Stamp = RosSharp.Now();
        CurrentGoalID.Id = GenerateUniqueId();
        return CurrentGoalID;   
    }

    public static void RosInit()
    {           
        if(rosNode!= null) return;          
        rosNode = Node.MainNode;            
        ExplorationDone = false;
        MotionEnabled = false;
        MotionType = sc["ImperaHelper"].GetString("ImperaHelper.PathHelper.MotionType");

        mBaseCancelGoalPub = new Publisher(Node.MainNode, "/move_base/cancel", GoalID.TypeId, 1);
        goalPositionPub = new Publisher(Node.MainNode, "/move_base_simple/goal", PoseStamped.TypeId, 1);
        mBaseGoalPositionPub = new Publisher(Node.MainNode, "/move_base/goal", MoveBaseActionGoal.TypeId, 1);
        motionPub = new Publisher(Node.MainNode,"MotionControl",MotionControl.TypeId,1);

        Node.MainNode.Subscribe("/move_base/status", OnGoalStatus);
        Node.MainNode.Subscribe("/move_base/feedback", OnGoalFeedback);
        Node.MainNode.Subscribe("/move_base/result", OnGoalResult);
        Node.MainNode.Subscribe("/PointOfInterest", OnPOI);
        Node.MainNode.Subscribe("/cmd_vel", OnTwist);           
    }

    protected ArmHelper()
    {       
        if(rosNode == null) {               
            RosInit();
        }
    }

    protected static void OnTwist(Twist tc) 
    {
        if (!MotionEnabled)
            return;         

        if(MotionType.Equals("tmc"))
        {
            MotionControl mc = new MotionControl();                                         
            mc.Motion.Angle = Math.Atan2(tc.Linear.Y, tc.Linear.X);
            mc.Motion.Translation = 1000*Math.Sqrt(tc.Linear.X * tc.Linear.X + tc.Linear.Y * tc.Linear.Y);
            mc.Motion.Rotation = tc.Angular.Z;  
            //Console.WriteLine("Velocity : " + mc.Motion.Translation + " Turn : " + mc.Motion.Rotation + " Angle : " + mc.Motion.Angle);
            motionPub.Send(mc);         
        }
    }   

    protected static void OnPOI(PoseStamped ps) {

        NewPOI = true;          
        PointOfInterest = ps.Pose;
        Console.WriteLine("PH: Got new POI");
    }

    protected static void OnGoalStatus(GoalStatusArray gStatus)
    {
        GoalStatus = new GoalStatusArray();
        GoalStatus.Header = gStatus.Header;
        GoalStatus.Status_list = gStatus.Status_list;
        //Console.WriteLine("Total Goal Count: "+GoalStatus.Status_list.Count);
    }

    protected static void OnGoalFeedback(MoveBaseActionFeedback feedback)
    {           
        GoalFeedback = new MoveBaseActionFeedback();
        GoalFeedback.Feedback = feedback.Feedback;
        GoalFeedback.Header = feedback.Header;
        GoalFeedback.Status = feedback.Status;
        //Console.WriteLine("Feedback: GoalID: "+GoalFeedback.Status.Goal_id.Id+ " Status: "+GoalFeedback.Status.Status);
    }

    protected static void OnGoalResult(MoveBaseActionResult result)
    {           
        GoalResult = new MoveBaseActionResult();
        GoalResult.Header = result.Header;          
        GoalResult.Result = result.Result;
        GoalResult.Status = result.Status;
        Console.WriteLine("Result: GoalID: "+GoalResult.Status.Goal_id.Id+ " Status: "+GoalResult.Status.Status);
    }

    public static void SendToSimpleGoal(PoseStamped pose) 
    {
        goalPositionPub.Send(pose);         
    }

    public static void SendToMoveBaseGoal(PoseStamped pose)
    {           
        MoveBaseGoal = new MoveBaseActionGoal();
        MoveBaseGoal.Goal_id = GenerateActionLibGoalId();
        MoveBaseGoal.Header.Stamp = RosSharp.Now();
        MoveBaseGoal.Header.Frame_id = pose.Header.Frame_id;
        MoveBaseGoal.Goal.Target_pose = pose;

        Console.WriteLine("Sending goal: "+ CurrentGoalID.Id);
        mBaseGoalPositionPub.Send(MoveBaseGoal);            
    }


    public static void SendToMoveBaseGoal(Pose goal, string frameId = "/map")
    {   
        PoseStamped pose = GeometryHelper.ConvertToPoseStamped(goal,frameId);
        SendToMoveBaseGoal(pose);           
    }

    public static void SendToMoveBaseGoal(Point goal, string frameId = "/map")
    {   
        PoseStamped pose = GeometryHelper.ConvertToPoseStamped(goal,frameId);
        SendToMoveBaseGoal(pose);           
    }


    public static void CancelMoveBaseGoal(GoalID goalID)
    {
        if(goalID != null)
            mBaseCancelGoalPub.Send(goalID);
    }
    public static void CancelCurrentMoveBaseGoal()
    {
        if(CurrentGoalID != null)
            mBaseCancelGoalPub.Send(CurrentGoalID);
    }

    public static Path GetNavigationPath(Pose start, Pose goal, string frameId = "/map")
    {
        PoseStamped startPS = GeometryHelper.ConvertToPoseStamped(start, frameId);
        PoseStamped goalPS = GeometryHelper.ConvertToPoseStamped(goal, frameId);

        Service sNode = Service.MainNode;
        ServiceClient sp = new ServiceClient(sNode,"/move_base_node/make_plan",GetPlan.TypeId);
        GetPlan srvPlan = new GetPlan();
        srvPlan.Request.Start = startPS;
        srvPlan.Request.Goal = goalPS;
        srvPlan.Request.Tolerance = 0.5f;
        sp.Call(srvPlan);           
        return srvPlan.Response.Plan;
    }


    public static double GetPathLength(Path p)
    {
        double length = 0;  
        if(p.Poses.Count == 0)
            return 100000000;
        for(int i = 1; i<p.Poses.Count;i++)
        {
            length += GeometryHelper.Distance(p.Poses[i-1].Pose.Position,p.Poses[i].Pose.Position);
        }

        return length;
    }

}